public void Should_SetRotationMatrixFromQuaternion(tf.Quaternion quaternion, tf.Vector3 vector, tf.Vector3 expectedResult = null)
{
//Utils.WaitForDebugger();
Console.WriteLine("");
Console.WriteLine("Test: SetRotationMatrixFromQuaternion");
Console.WriteLine("=====================================");
//var matrix = new tf.Matrix3x3(quaternion);
var matrix = new tf.Matrix3x3();
matrix.SetRotation(quaternion);
Console.WriteLine("Quaternion for rotation:");
Console.WriteLine(quaternion.ToString());
Console.WriteLine("Corresponding rotation matrix:");
Console.WriteLine(matrix.ToString());
Console.WriteLine("Vector to be rotated:");
Console.WriteLine(vector.ToString());
Console.WriteLine("Result of rotation via quaternion-vector-mult (qpq^-1):");
var resultQuaternion = quaternion * vector;
//Console.WriteLine(resultQuaternion.ToString());
var resultVector1 = new tf.Vector3(resultQuaternion.X, resultQuaternion.Y, resultQuaternion.Z);
Console.WriteLine(resultVector1.ToString());
Console.WriteLine("Result of rotation via quaternion conversion into rotation matrix:");
var resultVector2 = matrix * vector;
Console.WriteLine(resultVector2.ToString());
var tolerance = 1E-05;
Assert.InRange(Math.Abs(resultVector1.X - resultVector2.X), 0.0, tolerance);
Assert.InRange(Math.Abs(resultVector1.Y - resultVector2.Y), 0.0, tolerance);
Assert.InRange(Math.Abs(resultVector1.Z - resultVector2.Z), 0.0, tolerance);
if (expectedResult != null)
{
Assert.InRange(Math.Abs(resultVector1.X - expectedResult.X), 0.0, tolerance);
Assert.InRange(Math.Abs(resultVector1.Y - expectedResult.Y), 0.0, tolerance);
Assert.InRange(Math.Abs(resultVector1.Z - expectedResult.Z), 0.0, tolerance);
}
}
public void Should_QuaternionToRPYToQuaternion(tf.Quaternion quaternion, tf.Vector3 expectedResult = null)
{
//Utils.WaitForDebugger();
Console.WriteLine("");
Console.WriteLine("Test: QuaternionToRPYToQuaternion");
Console.WriteLine("=================================");
var rpy = quaternion.RPY;
var resultQuaternion = tf.Quaternion.FromRPY(rpy);
Console.WriteLine("Input quaternion:");
Console.WriteLine(quaternion.ToString());
Console.WriteLine("Roll, Pitch, Yaw:");
Console.WriteLine(rpy.ToString());
Console.WriteLine("Output quaternion:");
Console.WriteLine(resultQuaternion.ToString());
// The output of conversion to rpy and back is a unit quaternion.
// Hence, for comparison of input and output quaternion,
// the input quaternion has to be normalized.
var unitQuaternion = quaternion / quaternion.Abs;
Console.WriteLine("Normalized input quaternion:");
Console.WriteLine(unitQuaternion.ToString());
var tolerance = 1E-05;
Assert.InRange(Math.Abs(unitQuaternion.X - resultQuaternion.X), 0.0, tolerance);
Assert.InRange(Math.Abs(unitQuaternion.Y - resultQuaternion.Y), 0.0, tolerance);
Assert.InRange(Math.Abs(unitQuaternion.Z - resultQuaternion.Z), 0.0, tolerance);
Assert.InRange(Math.Abs(unitQuaternion.W - resultQuaternion.W), 0.0, tolerance);
if (expectedResult != null)
{
Assert.InRange(Math.Abs(rpy.X - expectedResult.X), 0.0, tolerance);
Assert.InRange(Math.Abs(rpy.Y - expectedResult.Y), 0.0, tolerance);
Assert.InRange(Math.Abs(rpy.Z - expectedResult.Z), 0.0, tolerance);
}
}