public static Quaternion From(this QuaternionMsg self, CoordinateSpaceSelection selection)
{
switch (selection)
{
case CoordinateSpaceSelection.RUF:
return(self.From <RUF>());
case CoordinateSpaceSelection.FLU:
return(self.From <FLU>());
case CoordinateSpaceSelection.FRD:
return(self.From <FRD>());
case CoordinateSpaceSelection.ENU:
return(self.From <ENU>());
case CoordinateSpaceSelection.NED:
return(self.From <NED>());
case CoordinateSpaceSelection.ENULocal:
return(self.From <ENULocal>());
case CoordinateSpaceSelection.NEDLocal:
return(self.From <NEDLocal>());
default:
Debug.LogError("Invalid coordinate space " + selection);
return(self.From <RUF>());
}
}
public static void DrawAxisVectors <C>(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new()
{
Vector3 unityPosition = position.From <C>();
Quaternion unityRotation = rotation.From <C>();
Vector3 x, y, z;
if (drawUnityAxes)
{
x = unityRotation * new Vector3(1, 0, 0) * size;
y = unityRotation * new Vector3(0, 1, 0) * size;
z = unityRotation * new Vector3(0, 0, 1) * size;
}
else
{
x = unityRotation * new Vector3 <C>(1, 0, 0).toUnity *size;
y = unityRotation * new Vector3 <C>(0, 1, 0).toUnity *size;
z = unityRotation * new Vector3 <C>(0, 0, 1).toUnity *size;
}
drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f);
}
