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); }