三点定位算法--用于平面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; }
    }


评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值