private void getMatrixAndVector(Beacon beacons, int rows, out double[,] m, out double[] b)
{
m = new double[rows, 3];
b = new double[rows];
double x1 = beacons.xpos(0);
double y1 = beacons.ypos(0);
double z1 = beacons.zpos(0);
double r1 = beacons.rpos(0);
for (int i = 1; i < beacons.size(); i++)
{
double x2 = beacons.xpos(i);
double y2 = beacons.ypos(i);
double z2 = beacons.zpos(i);
double r2 = beacons.rpos(i);
m[i - 1, 0] = x2 - x1;
m[i - 1, 1] = y2 - y1;
m[i - 1, 2] = z2 - z1;
b[i - 1] = (Math.Pow(r1, 2) - Math.Pow(r2, 2) + Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2) + Math.Pow(z2 - z1, 2)) / 2;
}
}
private void JacobiMethod(Beacon beacons, double[,] m, double[] b, out double x, out double y, out double z)
{
double x1 = beacons.xpos(0);
double y1 = beacons.ypos(0);
double z1 = beacons.zpos(0);
double[,] mt = new double[m.GetLength(1), m.GetLength(0)];
double[,] multiplied = new double[mt.GetLength(0), m.GetLength(1)];
mt = transpose(m);
multiplied = multiply(mt, m);
double det = determinant(multiplied);
double[,] A = inverse(multiplied, det);
double[,] d = multiply(A, mt);
double[] q = multbyvector(d, b);
x = q[0] + x1;
y = q[1] + y1;
z = q[2] + z1;
}
public double[] test(Beacon beacons, double[] coor, int rows)
{
double[] dest = new double[beacons.size()];
double[] res = new double[beacons.size()];
Beacon beaconr = beacons;
int c = 0;
while (c != 100)
{
for (int i = 0; i < beacons.size(); i++)
{
dest[i] = Math.Sqrt(Math.Pow(beaconr.xpos(i) - coor[0], 2) + Math.Pow(beaconr.ypos(i) - coor[1], 2) + Math.Pow(beaconr.zpos(i) - coor[2], 2));
res[i] = Math.Abs(beacons.rpos(i) - dest[i]);
}
if (res[0] <= 1E-10 && res[1] <= 1E-10 & res[2] <= 1E-10)
{
c = 99;
}
else
{
for (int i = 0; i < beaconr.size(); i++)
{
double[,] m = new double[rows, 3];
double x, y, z;
double[] b = new double[rows];
beaconr.to(i, beaconr.xpos(i), beaconr.ypos(i), beaconr.zpos(i), newr(beaconr.rpos(i), dest[i]));
getMatrixAndVector(beaconr, rows, out m, out b);
JacobiMethod(beaconr, m, b, out x, out y, out z);
coor[0] = x;
coor[1] = y;
coor[2] = z;
}
Console.WriteLine(c + ": " + res[0] + " " + res[1] + " " + res[2] + " " + res[3]);
}
c++;
}
Console.WriteLine("Final: " + res[0] + " " + res[1] + " " + res[2] + " " + res[3]);
return(coor);
}