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