「C#学习笔记」三点定位算法
自己写的,可能还有很多不足,看着哪里不对的求提出,我改~
先上代码,然后解释我的想法
1 namespace ThreePointOrientation 2 { 3 class Program 4 { 5 struct Point 6 { 7 public int x; 8 public int y; 9 } 10 static Point ThreePointOrientation(Point A, Point B, Point C, int lenAZ, int lenBZ, int lenCZ) 11 { 12 Point A1, B1, C1, Min, B2, Z2, Z1, ZZ1, inA1B1;14 //1 15 if (A.x <= B.x && A.x <= C.x) 16 { A1 = A; B1 = B; C1 = C; } 17 else if (B.x <= A.x && B.x <= C.x) 18 { A1 = B; B1 = C; C1 = A; } 19 else 20 { A1 = C; B1 = A; C1 = B; } 21 Min = A1; 22 //2 23 A1.x = 0; A1.y = 0; 24 B1.x -= Min.x; B1.y -= Min.y; 25 C1.x -= Min.x; C1.y -= Min.y; 26 //3 27 int lenAB = (int)System.Math.Sqrt(B1.x*B1.x + B1.y*B1.y);32 B2.x = lenAB; B2.y = 0; 33 //4 34 Z2.x = (lenAZ * lenAZ + lenAB * lenAB - lenBZ * lenBZ) / (2 * lenAB); 35 Z2.y = (int)System.Math.Sqrt(lenAZ*lenAZ-Z2.x*Z2.x); 36 //5 37 float tanBAB = (float)B1.y / (float)B1.x; 38 float tanCAC = (float)Z2.y / (float)Z2.x; 39 float tanCAB2; 40 if (B1.y >= 0) 41 tanCAB2 = (tanBAB+tanCAC)/(1-tanBAB*tanCAC); 42 else 43 tanCAB2 = (tanCAC-tanBAB)/(1+tanCAC*tanBAB); 44 //6 45 Z1.x = (int)((float)lenAZ / (System.Math.Sqrt(tanCAB2 * tanCAB2 + 1))); 46 Z1.y = (int)(tanCAB2 * Z1.x); 47 //7 48 inA1B1.x = (A1.x + B1.x) / 2; 49 inA1B1.y = (A1.y + B1.y) / 2; 50 //8 51 ZZ1.x = inA1B1.x * 2 - Z1.x; 52 ZZ1.y = inA1B1.y * 2 - Z1.y; 53 //9 54 int lenZZ1C1 = (int)System.Math.Sqrt(System.Math.Pow(ZZ1.x - C1.x, 2) + System.Math.Pow(ZZ1.y - C1.y, 2)); 55 int lenZ1C1 = (int)System.Math.Sqrt(System.Math.Pow(Z1.x - C1.x, 2) + System.Math.Pow(Z1.y - C1.y, 2)); 56 if((lenZZ1C1-lenCZ)<(lenZ1C1-lenCZ)) 57 Z1 = ZZ1; 58 //10 59 Z1.x += Min.x; 60 Z1.y += Min.y; 61 62 return Z1; 63 } 64 static void Main(string[] args) 65 { 66 Point a, b, c, z; 67 int az, bz, cz; 68 a.x = -5; 69 a.y = -5; 70 b.x = 47; 71 b.y = 25; 72 c.x = -5; 73 c.y = 55; 74 az = 34; 75 bz = 34; 76 cz = 34; 77 z = ThreePointOrientation(a, b, c, az, bz, cz); 78 Console.WriteLine("x = {0}, y = {1}\n", z.x, z.y); 79 Console.ReadKey(); 80 } 81 } 82 }
主要就是函数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