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"); }