三边定位 c#

MATLAB是美国MathWorks公司出品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境，主要包括MATLAB和Simulink两大部分。

项目中用到三边定位，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; }

    }