/// <summary>
/// Convert Unity coordinates to ROS world lat,long,alt
/// To be used to convert waypint unity coordinates to world lat.long.alt
/// </summary>
/// <param name="unityPosition"></param>
/// <returns>GPSCoordinates</returns>
public static GPSCoordinate UnityCoordToGPSCoord(Vector3 unityPosition)
{
GPSCoordinate gpsCoord = new GPSCoordinate();
gpsCoord.x = (((double)unityPosition.x * FROM_RADIANS / (EARTH_RADIUS * lngCorrection)) + Lng0);
gpsCoord.y = (((double)unityPosition.z * FROM_RADIANS / EARTH_RADIUS) + Lat0);
gpsCoord.z = (((double)unityPosition.y) + Alt0);
return(gpsCoord);
}
/// <summary>
/// Convert the given GPSCoordinate to Unity XYZ space.
/// </summary>
/// <param name="gpsPosition"></param>
/// <returns></returns>
public static Vector3 GPSCoordToUnityCoord(GPSCoordinate gpsPosition)
{
Vector3 unityCoord = Vector3.zero;
unityCoord.z = (float)((gpsPosition.Lat - Lat0) * TO_RADIANS * EARTH_RADIUS);
unityCoord.x = (float)((gpsPosition.Lng - Lng0) * TO_RADIANS * EARTH_RADIUS * lngCorrection);
unityCoord.y = (float)(gpsPosition.Alt - Alt0);
return(unityCoord);
}
