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