【模块】二维三点定位算法

    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; }  
        }