public XYCoordinates GetLocation(List <float> distancesFromSatellites)
{
XYCoordinates Kenobi = new XYCoordinates(-500, -200);
XYCoordinates Skywalker = new XYCoordinates(100, -100);
XYCoordinates Sato = new XYCoordinates(500, 100);
return(BaseTools.TrilaterationSolver(Kenobi, distancesFromSatellites[0], Skywalker, distancesFromSatellites[1], Sato, distancesFromSatellites[2]));
}
public void ExecuteLocator()
{
List <float> distanceFromSatellites = new List <float>();
string finishDesition = "";
distanceFromSatellites = _communicationSystem.GetDistanceFromSatellites();
XYCoordinates shipLocation = shipLocator.GetLocation(distanceFromSatellites);
Console.WriteLine("Ship Location: (" + shipLocation.GetCoordinateX() + ";" + shipLocation.GetCoordinateY() + ")");
}
/// <summary>
/// This Method determine de position of a point in a 2D Dimension from the 3 distances to 3 known points.
/// TODO: It can no determine when distances and known points given are incorrect. The results from wrong given points will be not correct.
/// </summary>
/// <param name="knownPoint1"></param>
/// <param name="distance1"></param>
/// <param name="knownPoint2"></param>
/// <param name="distance2"></param>
/// <param name="knownPoint3"></param>
/// <param name="distance3"></param>
/// <returns></returns>
public static XYCoordinates TrilaterationSolver(
XYCoordinates knownPoint1, float distance1,
XYCoordinates knownPoint2, float distance2,
XYCoordinates knownPoint3, float distance3)
{
float[] point1 = new float[2];
float[] point2 = new float[2];
float[] point3 = new float[2];
float[] ex = new float[2];
float[] ey = new float[2];
float[] p3p1 = new float[2];
float temp = 0;
float temp2;
float p3p1i = 0;
float xvalue;
float yvalue;
float jvalue = 0;
float ivalue = 0;
float exx;
float d;
float eyy;
float finalPointX;
float finalPointY;
//Get vectors from points
point1[0] = knownPoint1.GetCoordinateX();
point1[1] = knownPoint1.GetCoordinateY();
point2[0] = knownPoint2.GetCoordinateX();
point2[1] = knownPoint2.GetCoordinateY();
point3[0] = knownPoint3.GetCoordinateX();
point3[1] = knownPoint3.GetCoordinateY();
for (int i = 0; i < point1.Length; i++)
{
temp2 = point2[i] - point1[i];
temp += (temp2 * temp2);
}
d = (float)Math.Sqrt(temp);
for (int i = 0; i < point1.Length; i++)
{
exx = (float)(point2[i] - point1[i]) / (float)(Math.Sqrt(temp));
ex[i] = exx;
}
for (int i = 0; i < point3.Length; i++)
{
p3p1[i] = point3[i] - point1[i];
}
for (int i = 0; i < ex.Length; i++)
{
ivalue += (ex[i] * p3p1[i]);
}
for (int i = 0; i < point3.Length; i++)
{
temp2 = point3[i] - point1[i] - (ex[i] * ivalue);
p3p1i += (temp2 * temp2);
}
for (int i = 0; i < point3.Length; i++)
{
eyy = (point3[i] - point1[i] - (ex[i] * ivalue)) / (float)Math.Sqrt(p3p1i);
ey[i] = eyy;
}
for (int i = 0; i < ey.Length; i++)
{
jvalue += (ey[i] * p3p1[i]);
}
xvalue = (float)(Math.Pow(distance1, 2) - Math.Pow(distance2, 2) + Math.Pow(d, 2)) / (2 * d);
yvalue = (float)((Math.Pow(distance1, 2) - Math.Pow(distance3, 2) + Math.Pow(ivalue, 2) + Math.Pow(jvalue, 2)) / (2 * jvalue)) - ((ivalue / jvalue) * xvalue);
finalPointX = knownPoint1.GetCoordinateX() + (ex[0] * xvalue) + (ey[0] * yvalue);
finalPointY = knownPoint1.GetCoordinateY() + (ex[1] * xvalue) + (ey[1] * yvalue);
XYCoordinates location = new XYCoordinates(finalPointX, finalPointY);
return(location);
}
