Want to avoid the missteps to gaining all the benefits of the cloud? Learn more about the different assessment options from our Cloud Advisory team.
Add your voice to the tech community where 5M+ people just like you are talking about what matters.
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
Join the community of 500,000 technology professionals and ask your questions.