public static void trilaterate2(double[] P1, double[] P2, double[] P3, double[] L)
{
//define station points and distances
double x1 = P1[0];
double y1 = P1[1];
double z1 = P1[2];
double x2 = P2[0];
double y2 = P2[1];
double z2 = P2[2];
double x3 = P3[0];
double y3 = P3[1];
double z3 = P3[2];
double L1 = L[0];
double L2 = L[1];
double L3 = L[2];
//caluculate coords in plane of stations
double LB1 = Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1));
double LB2 = Math.Sqrt((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2) + (z3 - z2) * (z3 - z2));
double LB3 = Math.Sqrt((x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3) + (z1 - z3) * (z1 - z3));
double X = (L1*L1 - L2*L2 + LB1*LB1)/(2*LB1 );
double C1 = Math.Sqrt (L1*L1 - X*X);
if (L1*L1 - X*X < 0){Console.Write("no solution");}
double XB = (LB3*LB3 - LB2*LB2 + LB1*LB1 )/(2*LB1 );
if (LB3*LB3 - XB* XB < 0 ){Console.Write("no solution");}
double CB= Math.Sqrt(LB3*LB3 - XB* XB );
if (C1*C1+(XB - X)*(XB - X)< 0){Console.Write("no solution");}
double D1 = Math.Sqrt(C1*C1+(XB - X)*(XB - X));
double Y = (D1*D1 - L3*L3 + CB*CB )/(2*CB );
if (C1*C1 - Y*Y < 0){Console.Write("no solution");}
double Z = Math.Sqrt(C1 * C1 - Y * Y);
//Now transform X,Y,Z to x,y,z
//Find the unit vectors in X,Y,Z
double Xx = (x2-x1);
double Xy = (y2-y1);
double Xz = (z2-z1);
double Xl = Math.Sqrt(Xx*Xx+Xy*Xy+Xz*Xz);
Xx = Xx / Xl;
Xy = Xy / Xl;
Xz = Xz / Xl;
double t =- ((x1-x3)*(x2-x1)+(y1-y3)*(y2-y1)+(z1-z3)*(z2-z1))/(LB1*LB1);
double Yx = (x1+(x2-x1)*t-x3);
double Yy = (y1+(y2-y1)*t-y3);
double Yz = (z1+(z2-z1)*t-z3);
double Yl = Math.Sqrt(Yx*Yx+Yy*Yy+Yz*Yz);
Yx =- (Yx/Yl);
Yy =- (Yy/Yl);
Yz =- (Yz/Yl);
double Zx = (Xy * Yz - Xz * Yy);
double Zy = (Xz * Yx - Xx * Yz);
double Zz = (Xx * Yy - Xy * Yx);
//document.write(' Zx='+Zx.toFixed(5)+' Zy='+Zy.toFixed(5)+' Zz='+Zz.toFixed(5)+'<br>')
double x = (x1 + X * Xx + Y * Yx + Z * Zx);
double y = (y1 + X * Xy + Y * Yy + Z * Zy);
double z = (z1 + X * Xz + Y * Yz + Z * Zz);
x = (x1 + X * Xx + Y * Yx - Z * Zx);
y = (y1 + X * Xy + Y * Yy - Z * Zy);
z = (z1 + X * Xz + Y * Yz - Z * Zz);
Console.Write(x + " " + y + " " + z);
}
public static double[] L = new double[3] { 1782, 1795, 1491 };
public static double[] P1 = new double[3] { 1000, 1000, 0 };
public static double[] P2 = new double[3] { 1000, 2000, 0 };
public static double[] P3 = new double[3] { 2000, 1000, 0 };
static void Main(string[] args)
{
MathTest.trilaterate2(P1,P2,P3,L);
Console.ReadLine();
}
If you are experiencing a similar issue, please ask a related question
Title | # Comments | Views | Activity |
---|---|---|---|
How can i use linq for parsing string datetime to real datetime? | 16 | 46 | |
updating order of the rows, N+1 query issue, C#, asp.net, MVC, SQL Server | 3 | 27 | |
insert value of checklistbox checked | 4 | 27 | |
Problem with SelectList in .NET MVC application | 1 | 18 |
Join the community of 500,000 technology professionals and ask your questions.