// Convert the provided rotation order in frame 1 to the rotation order
// that would yield the same rotation in frame 2, given the direction
// conventions
internal static AxisSet ToEquivelentRotationOrder(AxisSet axes1, AxisSet axes2, AxisSet rotOrder)
{
// State the axes to rotate about
var r0 = rotOrder[0];
var r1 = rotOrder[1];
var r2 = rotOrder[2];
// 1. Swap the rotation direction on each axis
// based on each axis in the original frame
// 2. Then convert the axis names into the new
// frame and change rotation order based on that
// Originally two operations
// r0 = new Axis(r0.name, r0.negative != axes1[axes1[r0.name]].negative);
// r0 = new Axis(axes2[axes1[r0.name]].name, r0.negative != axes2[axes1[r0.name]].negative);
// Change the rotation direction based on the axis direction
r0 = new Axis(
axes2[axes1[r0.name]].name,
(r0.negative != axes1[axes1[r0.name]].negative) != axes2[axes1[r0.name]].negative
);
r1 = new Axis(
axes2[axes1[r1.name]].name,
(r1.negative != axes1[axes1[r1.name]].negative) != axes2[axes1[r1.name]].negative
);
r2 = new Axis(
axes2[axes1[r2.name]].name,
(r2.negative != axes1[axes1[r2.name]].negative) != axes2[axes1[r2.name]].negative
);
return(new AxisSet(r0, r1, r2, true));
}
public static string ToRotationOrderFrameString(AxisSet axes, AxisSet rotOrder)
{
string str = ToCoordinateFrameString(axes);
return(str
.Replace(" " + rotOrder[0].name, rotOrder[0].negative ? "-1" : "+1")
.Replace(" " + rotOrder[1].name, rotOrder[1].negative ? " -2" : " +2")
.Replace(" " + rotOrder[2].name, rotOrder[2].negative ? "-3" : "+3"));
}
public override bool Equals(object obj)
{
if (ReferenceEquals(obj, null) || GetType() != obj.GetType())
{
return(false);
}
AxisSet other = (AxisSet)obj;
return(_axes[0] == other._axes[0] && _axes[1] == other._axes[1] && _axes[2] == other._axes[2]);
}
// 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 static string ToCoordinateFrameString(AxisSet axes)
{
string r = axes[0].negative ? " " : "-";
string l = axes[0].negative ? "-" : " ";
string u = axes[1].negative ? " " : "|";
string d = axes[1].negative ? "|" : " ";
string f = axes[2].negative ? " " : "／";
string b = axes[2].negative ? "／" : " ";
string template =
" U \n" +
" u B\n" +
" u b \n" +
" Llllll.rrrrr R\n" +
" f d \n" +
" F d \n" +
" D \n";
return(template
.Replace("R", axes[0].negative ? " " : axes[0].ToString())
.Replace("r", r)
.Replace("L", axes[0].negative ? axes[0].ToString() : " ")
.Replace("l", l)
.Replace("U", axes[1].negative ? " " : axes[1].ToString())
.Replace("u", u)
.Replace("D", axes[1].negative ? axes[1].ToString() : " ")
.Replace("d", d)
.Replace("F", axes[2].negative ? " " : axes[2].ToString())
.Replace("f", f)
.Replace("B", axes[2].negative ? axes[2].ToString() : " ")
.Replace("b", b));
}
public static Vector3 ConvertPosition(AxisSet from, AxisSet to, Vector3 v)
{
Vector3 res = new Vector3();
// Iterate over right, then up, then forward
for (int i = 0; i < 3; i++)
{
// Get the axis name associated with the
// given direction
int directionIndex = i; // the direction
// Associated axes
Axis fromAxis = from[directionIndex];
Axis toAxis = to[directionIndex];
// The vector index to take the value out of
// and put it in to
int fromIndex = fromAxis.xyzIndex;
int toIndex = toAxis.xyzIndex;
res[toIndex] = fromAxis.negative == toAxis.negative ? v[fromIndex] : -v[fromIndex];
}
return(res);
}
public CoordinateFrame(string rufAxes, string rotationOrder)
{
_axes = new AxisSet(rufAxes, false);
_rotationOrder = new AxisSet(rotationOrder, true);
}
// Draws the coordinate frame representing the provided axis set
public static void DrawFrame(AxisSet axes, float scale = 1)
{
DrawAxis(axes[0], Vector3.right * scale);
DrawAxis(axes[1], Vector3.up * scale);
DrawAxis(axes[2], -Vector3.forward * scale);
}
public static void DrawFrame(AxisSet axes, AxisSet rotationOrder, float scale = 1)
{
DrawFrame(axes, rotationOrder, new EulerAngles(0, 0, 0), new EulerAngles(120, 120, 120), scale);
}
public static EulerAngles ConvertEulerOrder(AxisSet from, AxisSet to, EulerAngles eulerAngles)
{
Quaternion quat = ToQuaternion(from, eulerAngles);
return(ExtractEulerAngles(to, quat));
}
