// Outputs euler angles in degrees
// Extracts the rotation in Euler degrees in the
// given order
internal static EulerAngles ExtractEulerAngles(AxisSet order, Quaternion quat)
{
AngleExtraction.EulerResult eu;
EulerAngles res = extractionFunctions[order.ToString()](quat, out eu);
for (int i = 0; i < 3; i++)
{
if (order[i].negative)
{
res[i] *= -1;
}
}
return(res);
}
// Takes euler angles in degrees
// Returns the rotation as a quaternion that results from
// applying the rotations in the provided order
// TODO: This function basically requires that angles be
// in Unity's frame in order to really work, does it make sense to expose
// it or use it?
internal static Quaternion ToQuaternion(AxisSet order, EulerAngles euler)
{
Quaternion res = Quaternion.identity;
for (int i = 0; i < 3; i++)
{
Axis axis = order[i];
Vector3 angles = Vector3.zero;
angles[axis.xyzIndex] = axis.negative ? -euler[i] : euler[i];
// Unity's default rotation direction is negative
angles *= -1;
res = Quaternion.Euler(angles) * res;
}
return(res);
}
public static EulerAngles ConvertEulerAngles(AxisSet axes1, AxisSet rot1, AxisSet axes2, AxisSet rot2, EulerAngles eulerAngles)
{
// ToQuaternion doesn't know anything about the axis conventions
// about which the the rotations order apply, so it assumes
// XYZ are up right left. We use a consistent frame when converting
// so the conventions when going back and forth are consistent
// TODO: Should ToQuaternion be changed to account for this? At the
// moment it assumes the Unity direction convention.
// TODO: Using anything other than XY-Z (unity's frame) here causes
// some oddities in the way rotations are applied
AxisSet intermediateAxes = new AxisSet("XY-Z", false);
AxisSet order1InInter = ToEquivelentRotationOrder(axes1, intermediateAxes, rot1);
AxisSet order2InInter = ToEquivelentRotationOrder(axes2, intermediateAxes, rot2);
Quaternion intermediateQuat = ToQuaternion(order1InInter, eulerAngles);
EulerAngles resultantAngles = ExtractEulerAngles(order2InInter, intermediateQuat);
return(resultantAngles);
}
// Draw the frame with the position and rotation order axes
public static void DrawFrame(AxisSet axes, AxisSet rotationOrder, EulerAngles stAngles, EulerAngles endAngles, float scale = 1)
{
DrawFrame(axes, scale);
// Associate axes to the xyz index
Vector3[] dir = new Vector3[] { Vector3.right, Vector3.up, -Vector3.forward };
for (int i = 0; i < 3; i++)
{
Axis rotAxis = rotationOrder[i];
int index = axes[rotAxis.name];
Axis posAxis = axes[index];
Gizmos.color = posAxis.name == "X" ? Color.red : posAxis.name == "Y" ? Color.green : Color.blue;
Vector3 center = dir[index];
if (posAxis.negative)
{
center *= -1;
}
DrawRotationDirection(center * (0.45f + i * 0.05f) * scale, dir[(index + 1) % 3], stAngles[i], endAngles[i], scale * 0.25f, rotAxis.negative);
}
}
public EulerAngles ConvertEulerAngles(CoordinateFrame other, EulerAngles eulerAngles)
{
return(Conversions.ConvertEulerAngles(this, other, eulerAngles));
}
public static EulerAngles ConvertEulerAngles(CoordinateFrame cf1, CoordinateFrame cf2, EulerAngles eulerAngles)
{
return(ConvertEulerAngles(cf1.Axes, cf1.RotationOrder, cf2.Axes, cf2.RotationOrder, eulerAngles));
}
// Convert the provided euler angles from an euler rotation in frame 1 to
// frame 2 given the direction and rotation conventions
public static EulerAngles ConvertEulerAngles(string fromAxes, string fromRotorder, string toAxes, string toRotOrder, EulerAngles eulerAngles)
{
return(ConvertEulerAngles(new AxisSet(fromAxes, true), new AxisSet(fromRotorder, true), new AxisSet(toAxes, true), new AxisSet(toRotOrder, true), eulerAngles));
}
public static EulerAngles ConvertEulerOrder(AxisSet from, AxisSet to, EulerAngles eulerAngles)
{
Quaternion quat = ToQuaternion(from, eulerAngles);
return(ExtractEulerAngles(to, quat));
}
// Convert between the provided euler orders
public static EulerAngles ConvertEulerOrder(string from, string to, EulerAngles eulerAngles)
{
return(ConvertEulerOrder(new AxisSet(from, false), new AxisSet(to, false), eulerAngles));
}
public EulerAngles ConvertEulerAngles(EulerAngles euler)
{
return(_fromFrame.ConvertEulerAngles(_toFrame, euler));
}
