public static Vector3 OrientationToUnityEuler(Vector3Float orientation, LatLonCoord latLon)
{
float lat = (float)latLon.Lat * Mathf.Deg2Rad;
float lon = (float)latLon.Lon * Mathf.Deg2Rad;
float sinlat = Mathf.Sin(lat);
float coslat = Mathf.Cos(lat);
float sinlon = Mathf.Sin(lon);
float coslon = Mathf.Cos(lon);
float cosPsi = Mathf.Cos(orientation.X);
float sinPsi = Mathf.Sin(orientation.X);
float sinTheta = Mathf.Sin(orientation.Y);
float cosTheta = Mathf.Cos(orientation.Y);
float sinPhi = Mathf.Sin(orientation.Z);
float cosPhi = Mathf.Cos(orientation.Z);
float sinsin = sinlat * sinlon;
float cosThetaCosPsi = cosTheta * cosPsi;
float cosThetaSinPsi = cosTheta * sinPsi;
float sincos = sinlat * coslon;
float b11 = -sinlon * cosThetaCosPsi + coslon * cosThetaSinPsi;
float b12 = -sincos * cosThetaCosPsi - sinsin * cosThetaSinPsi - coslat * sinTheta;
float cosLatCosLon = coslat * coslon;
float cosLatSinLon = coslat * sinlon;
float sinPhiSinTheta = sinPhi * sinTheta;
float cosPhiSinTheta = cosPhi * sinTheta;
float b23 = cosLatCosLon * (-cosPhi * sinPsi + sinPhiSinTheta * cosPsi) +
cosLatSinLon * (cosPhi * cosPsi + sinPhiSinTheta * sinPsi) +
sinlat * (sinPhi * cosTheta);
float b33 = cosLatCosLon * (sinPhi * sinPsi + cosPhiSinTheta * cosPsi) +
cosLatSinLon * (-sinPhi * cosPsi + cosPhiSinTheta * sinPsi) +
sinlat * (cosPhi * cosTheta);
return(new Vector3(
-Mathf.Asin(cosLatCosLon * cosTheta * cosPsi + cosLatSinLon * cosTheta * sinPsi - sinlat * sinTheta) * Mathf.Rad2Deg,
Mathf.Atan2(b11, b12) * Mathf.Rad2Deg,
-Mathf.Atan2(-b23, -b33) * Mathf.Rad2Deg
));
}
public static Vector3Float UnityEulerToOrientation(Vector3 unityEuler, LatLonCoord latLon)
{
float lat = (float)latLon.Lat * Mathf.Deg2Rad;
float lon = (float)latLon.Lon * Mathf.Deg2Rad;
float yaw = unityEuler.y * Mathf.Deg2Rad;
float pitch = -unityEuler.x * Mathf.Deg2Rad;
float roll = -unityEuler.z * Mathf.Deg2Rad;
float sinLat = Mathf.Sin(lat);
float cosLat = Mathf.Cos(lat);
float sinLon = Mathf.Sin(lon);
float cosLon = Mathf.Cos(lon);
float sinYaw = Mathf.Sin(yaw);
float cosYaw = Mathf.Cos(yaw);
float sinPitch = Mathf.Sin(pitch);
float cosPitch = Mathf.Cos(pitch);
float sinRoll = Mathf.Sin(roll);
float cosRoll = Mathf.Cos(roll);
float cosLatCosLon = cosLat * cosLon;
float cosLatSinLon = cosLat * sinLon;
float sinLatCosLon = sinLat * cosLon;
float sinLatSinLon = sinLat * sinLon;
float a_11 = -sinLon * sinYaw * cosPitch - sinLatCosLon * cosYaw * cosPitch + cosLatCosLon * sinPitch;
float a_12 = cosLon * sinYaw * cosPitch - sinLatSinLon * cosYaw * cosPitch + cosLatSinLon * sinPitch;
float a_23 = cosLat * (-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll) - sinLat * cosPitch * sinRoll;
float a_33 = cosLat * (sinYaw * sinRoll + cosYaw * sinPitch * cosRoll) - sinLat * cosPitch * cosRoll;
return(new Vector3Float(
Mathf.Atan2(a_12, a_11),
Mathf.Asin(-cosLat * cosYaw * cosPitch - sinLat * sinPitch),
Mathf.Atan2(a_23, a_33)
));
}
public static void SetRefLatLon(LatLonCoord refLatLon)
{
_refLatLon = refLatLon;
}