IEnumerator Rotate(CoordinateFrame unityFrame, CoordinateFrame frame2, Vector3 unityAngles)
{
yield return(new WaitForSeconds(1));
CoordinateFrameConverter conv = new CoordinateFrameConverter(unityFrame, frame2);
EulerAngles eu1 = new EulerAngles(unityAngles.z, unityAngles.x, unityAngles.y);
EulerAngles eu2 = conv.ConvertEulerAngles(eu1);
int steps = 40;
EulerAngles teu1 = new EulerAngles(0, 0, 0);
EulerAngles teu2 = new EulerAngles(0, 0, 0);
for (int e = 0; e < 3; e++)
{
for (int i = 0; i < steps; i++)
{
teu1[e] = teu1[e] + eu1[e] * 1.0f / steps;
teu2[e] = teu2[e] + eu2[e] * 1.0f / steps;
target1.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, teu1);
target2.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, conv.inverse.ConvertEulerAngles(teu2));
yield return(null);
}
}
}
// Run back and forth conversion tests against a sampling of
// coordinate frames
IEnumerator RunCoordinateFrameTests(List <CoordinateFrame> candidateFrames, int framesToTest)
{
framesToTest = Mathf.Min(candidateFrames.Count, framesToTest);
List <CoordinateFrame> remainingFrames = new List <CoordinateFrame>(candidateFrames);
List <CoordinateFrame> frames = new List <CoordinateFrame>();
for (int i = 0; i < framesToTest; i++)
{
int index = Mathf.FloorToInt(Random.value * (remainingFrames.Count)) % remainingFrames.Count;
frames.Add(remainingFrames[index]);
remainingFrames.RemoveAt(index);
}
int issues = 0;
foreach (var fr1 in frames)
{
foreach (var fr2 in frames)
{
// Verify that the inverse frames work as expected
var cfc = new CoordinateFrameConverter(fr1, fr2);
if (cfc != cfc.inverse.inverse || cfc.from != cfc.inverse.to || cfc.to != cfc.inverse.from)
{
issues++;
}
// Position Tests
Vector3 v = new Vector3(1, 2, 3);
Vector3 to = fr1.ConvertPosition(fr2, v);
Vector3 back = fr2.ConvertPosition(fr1, to);
if (!AreVectorsEquivalent(v, back))
{
issues++;
}
to = cfc.ConvertPosition(v);
back = cfc.inverse.ConvertPosition(to);
if (!AreVectorsEquivalent(v, back))
{
issues++;
}
// Rotation Conversions
EulerAngles e = GetRandomAngles();
EulerAngles eTo = Conversions.ConvertEulerAngles(fr1, fr2, e);
EulerAngles eBack = Conversions.ConvertEulerAngles(fr2, fr1, eTo);
bool equal = AreRotationsEquivalent(fr1.Axes, fr1.RotationOrder, e, eBack);
if (!equal)
{
Debug.LogError("Error converting " + fr1.ToString(true) + " > " + fr2.ToString(true));
Debug.Log(e.ToString("0.00000") + " > " + eTo.ToString("0.00000") + " > " + eBack.ToString("0.00000"));
AreRotationsEquivalent(fr1.Axes, fr1.RotationOrder, e, eBack, true);
issues++;
}
}
yield return(null);
}
Debug.Log("Ran into " + issues + " issues when doing coordinate frame conversions");
}
