三点定位算法--用于平面2D定位

项目中用到三边定位，c#中没有MATLAB solve函数来解方程，只好人工去解方程了。

class Program
    {
        static void Main(string[] args)
        {
            Point p1 = new Point() { X = 0, Y = 2, Distance = Math.Sqrt(5) };
            Point p2 = new Point() { X = 2, Y = 2, Distance = Math.Sqrt(5) };
            Point p3 = new Point() { X = 1, Y = 0, Distance = Math.Sqrt(0) };
            var p= GetPiontByThree(p1, p2, p3);
            Console.WriteLine("Point x:{0}",p.X);
            Console.WriteLine("Point y:{0}", p.Y);
            Console.ReadKey();
        }

        /// <summary>
        /// 三点绝对定位
        /// </summary>
        private static Point GetPiontByThree(Point p1, Point p2, Point p3)
        {
            /* Math.Pow(y1-Y)+Math.Pow(X-x1)=Math.Pow(D1)
             * Math.Pow(y2-Y)+Math.Pow(X-x2)=Math.Pow(D2)
             * Math.Pow(y3-Y)+Math.Pow(X-x3)=Math.Pow(D3)
             * 1-3.2-3解得：
             * 2 * (p1.X - p3.X)x + 2 * (p1.Y - p3.Y)y = Math.Pow(p1.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p1.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p1.Distance, 2);
             * 2 * (p2.X - p3.X)x + 2 * (p2.Y - p3.Y)y = Math.Pow(p2.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p2.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p2.Distance, 2);
             * 简化：
             * 2Ax+2By=C
             * 2Dx+2Ey=F
             * 简化：
             * x=(BF-EC)/(2BD-2AE)
             * y=(AF-DC)/(2AE-2BD)
             */
            var A = p1.X - p3.X;
            var B = p1.Y - p3.Y;
            var C = Math.Pow(p1.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p1.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p1.Distance, 2);
            var D = p2.X - p3.X;
            var E = p2.Y - p3.Y;
            var F = Math.Pow(p2.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p2.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p2.Distance, 2);

            var x = (B * F - E * C) / (2 * B * D - 2 * A * E);
            var y = (A * F - D * C) / (2 * A * E - 2 * B * D);

            Point P = new Point() { X = x, Y = y, Distance = 0 };
            return P;
        }
    }

    public class Point
    {
        public double X { get; set; }
        public double Y { get; set; }
        //表示指定点，据此点的距离
        public double Distance { get; set; }
    }