/// <summary> /// Converts XYZ Tait-Bryan angles to a <see cref="Quaternion"/>. /// </summary> /// <param name="xyzAngles">The XYZ angles in degrees</param> /// <returns>The created <see cref="Quaternion"/></returns> public static Quaternion XYZtoQuaternion(Vector3 xyzAngles) { RotationMatrix rotmat = new RotationMatrix(xyzAngles.x, xyzAngles.y, xyzAngles.z); IRescue.Core.DataTypes.Vector3 forward = new IRescue.Core.DataTypes.Vector3(0, 0, 1); IRescue.Core.DataTypes.Vector3 upward = new IRescue.Core.DataTypes.Vector3(0, 1, 0); rotmat.Multiply(forward, forward); rotmat.Multiply(upward, upward); return(Quaternion.LookRotation(IRescueVec3ToUnityVec3(forward), IRescueVec3ToUnityVec3(upward))); }
/// <summary> /// Test that rotations are equal. As rotations can be achieved by different rotations in /// different orders around different axes the method creates a rotation matrix for the expected /// and actual _values and compares the result of a multiplication with a reference vector. When /// the output of the multiplication is the same the orientation is also similar. /// </summary> /// <param name="expected">The expected rotation.</param> /// <param name="actual">The actual rotation.</param> /// <param name="epsilon">The epsilon to use in floating point comparison.</param> private void AssertRotationAreEqual(Vector3 expected, Vector3 actual, double epsilon) { RotationMatrix rotExpected = new RotationMatrix(expected.X, expected.Y, expected.Z); RotationMatrix rotActual = new RotationMatrix(actual.X, actual.Y, actual.Z); Vector3 vecExpected = new Vector3(1, 1, 1); Vector3 vecActual = new Vector3(1, 1, 1); rotExpected.Multiply(vecExpected, vecExpected); rotActual.Multiply(vecActual, vecActual); this.AssertVectorAreEqual(vecExpected, vecActual, epsilon); }
/// <summary> /// Rotates the specified vector with the specified rotation matrix and stores the result in result vector. /// </summary> /// <param name="vector">The vector to rotate.</param> /// <param name="rotation">The rotation matrix.</param> /// <param name="result">Vector where the result is stored.</param> public static void RotateVector(Vector3 vector, RotationMatrix rotation, Vector3 result) { rotation.Multiply(vector, result); }