「C#學習筆記」三點定位算法

自己寫的，可能還有很多不足，看着哪里不對的求提出，我改~

先上代碼，然后解釋我的想法

namespace ThreePointOrientation
{
    class Program
    {
        struct Point
        {
            public int x;
            public int y;
        }
        static Point ThreePointOrientation(Point A, Point B, Point C, int lenAZ, int lenBZ, int lenCZ)
        {
            Point A1, B1, C1, Min, B2, Z2, Z1, ZZ1, inA1B1;14             //1
            if (A.x <= B.x && A.x <= C.x)
            { A1 = A; B1 = B; C1 = C; }
            else if (B.x <= A.x && B.x <= C.x)
            { A1 = B; B1 = C; C1 = A; }
            else
            { A1 = C; B1 = A; C1 = B; }
            Min = A1;
            //2
            A1.x = 0; A1.y = 0;
            B1.x -= Min.x; B1.y -= Min.y;
            C1.x -= Min.x; C1.y -= Min.y;
            //3
            int lenAB = (int)System.Math.Sqrt(B1.x*B1.x + B1.y*B1.y);32             B2.x = lenAB; B2.y = 0;
            //4
            Z2.x = (lenAZ * lenAZ + lenAB * lenAB - lenBZ * lenBZ) / (2 * lenAB);
            Z2.y = (int)System.Math.Sqrt(lenAZ*lenAZ-Z2.x*Z2.x);
            //5
            float tanBAB = (float)B1.y / (float)B1.x;
            float tanCAC = (float)Z2.y / (float)Z2.x;
            float tanCAB2;
            if (B1.y >= 0)
                tanCAB2 = (tanBAB+tanCAC)/(1-tanBAB*tanCAC);
            else
                tanCAB2 = (tanCAC-tanBAB)/(1+tanCAC*tanBAB);
            //6
            Z1.x = (int)((float)lenAZ / (System.Math.Sqrt(tanCAB2 * tanCAB2 + 1)));
            Z1.y = (int)(tanCAB2 * Z1.x);
            //7
            inA1B1.x = (A1.x + B1.x) / 2;
            inA1B1.y = (A1.y + B1.y) / 2;
            //8
            ZZ1.x = inA1B1.x * 2 - Z1.x;
            ZZ1.y = inA1B1.y * 2 - Z1.y;
            //9
            int lenZZ1C1 = (int)System.Math.Sqrt(System.Math.Pow(ZZ1.x - C1.x, 2) + System.Math.Pow(ZZ1.y - C1.y, 2));
            int lenZ1C1 = (int)System.Math.Sqrt(System.Math.Pow(Z1.x - C1.x, 2) + System.Math.Pow(Z1.y - C1.y, 2));
            if((lenZZ1C1-lenCZ)<(lenZ1C1-lenCZ))
                Z1 = ZZ1;
            //10
            Z1.x += Min.x;
            Z1.y += Min.y;

            return Z1;
        }
        static void Main(string[] args)
        {
            Point a, b, c, z;
            int az, bz, cz;
            a.x = -5;
            a.y = -5;
            b.x = 47;
            b.y = 25;
            c.x = -5;
            c.y = 55;
            az = 34;
            bz = 34;
            cz = 34;
            z = ThreePointOrientation(a, b, c, az, bz, cz);
            Console.WriteLine("x = {0}, y = {1}\n", z.x, z.y);
            Console.ReadKey();
        }
    }
}

主要就是函數Point ThreePointOrientation(Point A, Point B, Point C, int lenAZ, int lenBZ, int lenCZ);

A,B,C是已知的三點，lenAZ,lenBZ,lenCZ分別是三點到要求的點的距離，函數返回要求的點的坐標。

我的想法......

1.把ABC三點中x值最小的賦值給A1，另外兩點分別給B1和C1，並記錄A1的值給Min

2.把A1B1C1這個三角形平移至A1在原點處（即三個點分別減去Min）

3.計算A1B1距離lenAB，記B2(lenAB, 0)。（相當於把邊A1B1旋轉到與x軸重合，得到A1B2）

4.根據A1B2的坐標和lenAZ、lenBZ的值可以很容易算出Z2的坐標（Z2為經過平移和旋轉之后的要求的點）

5.6.利用公式求得Z1（其實通過A1、B1兩點及AZ、BZ兩條邊能確定的點有兩個，如圖的Z1和ZZ1）

7.8.9.求出另一個點ZZ1，判斷C1Z1、C1ZZ1哪個與CZ更相近，把相近的點賦值給Z1

10.把Z1點平移回去（加上Min）即得Z