三角形相关算法--求解三角形顶点坐标

问题描述：

　　已知三角形两个顶点p1，p2的坐标，p1与p0的距离len，p0-p1边与p1-p2边的夹角angle，求解p0的坐标。

求解思路：

　　1、根据p1与p0的距离可以列出一个等式：

(p1.x-p0.x)²+(p1.y-p0.y)²=len²

　　2、p1-p0向量为v1=(p0.x-p1.x, p0.y-p1.y)，p1-p2向量为v2=(p2.x-p1.x, p2.y-p1.y)，两向量的夹角为angle。根据向量夹角公式可得到一个等式：

cos(angle)=(v1•v2)/(|v1|*|v2|)

v1•v2=(p0.x-p1.x)*(p2.x-p1.x)+(p0.y-p1.y)*(p2.y-p1.y)

|v1|=sqrt((p1.x-p0.x)²+(p1.y-p0.y)²)=len

|v2|=sqrt((p2.x-p1.x)²+(p2.y-p1.y)²)

　　根据这两个等式，可推导p0.x、p0.y的求解公式，一般应有两组解。

　　3、在推导过程中，(p2.x-p1.x)被用作分母，因此，在实际计算时，需要将p2.x=p1.x的情况（即p1-p2边与纵轴平行）单独考虑。

C#算法：

/// <summary>
/// 三角形的顶点为p0、p1、p2，
/// 已知三角形两个顶点p1，p2的坐标，p1与p0的距离len，p0-p1边与p1-p2边的夹角angle，
/// 求解p0的坐标（结果应有2组坐标）
/// </summary>
/// <param name="p1">顶点p1的坐标</param>
/// <param name="p2">顶点p2的坐标</param>
/// <param name="len">顶点p1与p0的距离</param>
/// <param name="angle">p0-p1边与p1-p2边的夹角</param>
/// <returns>p0的坐标（若len不等于0，则结果应有2组；否则结果为p1）</returns>
public static TwoDimPoint[] GetVertex(TwoDimPoint p1, TwoDimPoint p2, double len, double angle)
{            
    TwoDimPoint[] result;

    //若p1与p0的距离为0，则p0直接取p1的坐标
    if (Utility.IsEqualZero(len))
    {
        result = new TwoDimPoint[1];
        result[0] = new TwoDimPoint(p1);
    }
    else
    {
        if (Utility.IsEqualZero(TwoDimPoint.GetDistance(p1, p2)))
        {
            throw new NotImplementedException("已知两顶点的坐标一致，无法计算未知顶点的坐标！");
        }
        angle %= 2 * Math.PI;
        if (Utility.IsEqualZero(p2.X - p1.X))//p1-p2边与纵轴平行
        {
            if (angle % Math.PI == 0)//p0与p1、p2在一条直线上
            {
                result = new TwoDimPoint[1];
                result[0] = new TwoDimPoint();
                result[0].X = p1.X;

                if (angle == 0)
                {
                    if (p1.Y > p2.Y)
                        result[0].Y = p1.Y - len;
                    else
                        result[0].Y = p1.Y + len;
                }
                else
                {
                    if (p1.Y > p2.Y)
                        result[0].Y = p1.Y + len;
                    else
                        result[0].Y = p1.Y - len; 
                }
            }
            else
            {
                result = new TwoDimPoint[2];
                result[0] = new TwoDimPoint();
                result[0].X = p1.X + len * Math.Sin(angle);
                if (p1.Y > p2.Y)
                    result[0].Y = p1.Y - len * Math.Cos(angle);
                else
                    result[0].Y = p1.Y + len * Math.Cos(angle);

                result[1] = new TwoDimPoint();
                result[1].X = p1.X - len * Math.Sin(angle);
                result[1].Y = result[0].Y;
            }
        }
        else
        {
            double A = TwoDimPoint.GetDistance(p1, p2);//p1,p2的距离
            double B = (p2.Y - p1.Y) / (p2.X - p1.X);
            double C = (len * A * Math.Cos(angle) + p1.Y * p2.Y - Utility.Square(p1.Y)) / (p2.X - p1.X);
            double D = Utility.Square(B) + 1;
            double E = (B * C + p1.Y) * 2;
            double F = Utility.Square(C) + Utility.Square(p1.Y) - Utility.Square(len);
            double G = Utility.Square(E) - 4 * D * F;

            if (Utility.IsEqualZero(G))
            {
                result = new TwoDimPoint[1];

                result[0] = new TwoDimPoint();
                result[0].Y = E / (2 * D);
                result[0].X = C + p1.X - B * result[0].Y;
            }
            else
            {
                result = new TwoDimPoint[2];

                result[0] = new TwoDimPoint();
                result[0].Y = (E + Math.Sqrt(G)) / (2 * D);
                result[0].X = C + p1.X - B * result[0].Y;

                result[1] = new TwoDimPoint();
                result[1].Y = (E - Math.Sqrt(G)) / (2 * D);
                result[1].X = C + p1.X - B * result[1].Y;
            }
        }
    }            
    return result;
}

/// <summary>
/// 二维点
/// </summary>
public class TwoDimPoint
{
    public double X;
    public double Y;

    public TwoDimPoint()
    {
        this.X = 0;
        this.Y = 0;
    }

    public TwoDimPoint(double x, double y)
    {
        this.X = x;
        this.Y = y;
    }

    public TwoDimPoint(TwoDimPoint p)
    {
        this.X = p.X;
        this.Y = p.Y;
    }

    /// <summary>
    /// 计算两点的距离
    /// </summary>
    /// <param name="p1"></param>
    /// <param name="p2"></param>
    /// <returns></returns>
    public static double GetDistance(TwoDimPoint p1, TwoDimPoint p2)
    {
        return Math.Sqrt(Utility.Square(p1.X - p2.X) + Utility.Square(p1.Y - p2.Y));
    }
}

public class Utility
{
    /// <summary>
    /// 判断数值是否等于0
    /// </summary>
    /// <param name="value"></param>
    /// <returns></returns>
    public static bool IsEqualZero(double value)
    {
        return Math.Abs(value) < Constant.MinPositiveNum;
    }

    /// <summary>
    /// 计算数值的平方
    /// </summary>
    /// <param name="value"></param>
    /// <returns></returns>
    public static double Square(double value)
    {
        return Math.Pow(value, 2);
    }
}