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

最新推荐文章于 2025-10-14 16:27:13 发布

转载最新推荐文章于 2025-10-14 16:27:13 发布 · 6.3k 阅读

文章标签：

#三点定位

本文介绍了一种在C#中实现的三边定位算法，该算法通过解析三个已知位置及其距离信息来确定未知点的位置坐标。由于C#中缺乏类似MATLAB的solve函数，故采用数学公式直接求解。

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

[csharp]view plain copy
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; }  
    }  