public void QuaternionDCreateFromYawPitchRollTest2()
{
const double step = 35.0f;
for (double yawAngle = -720.0f; yawAngle <= 720.0f; yawAngle += step)
{
for (double pitchAngle = -720.0f; pitchAngle <= 720.0f; pitchAngle += step)
{
for (double rollAngle = -720.0f; rollAngle <= 720.0f; rollAngle += step)
{
double yawRad = MathHelper.ToRadians(yawAngle);
double pitchRad = MathHelper.ToRadians(pitchAngle);
double rollRad = MathHelper.ToRadians(rollAngle);
QuaternionD yaw = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, yawRad);
QuaternionD pitch = QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, pitchRad);
QuaternionD roll = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, rollRad);
QuaternionD expected = yaw * pitch * roll;
QuaternionD actual = QuaternionD.CreateFromYawPitchRoll(yawRad, pitchRad, rollRad);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.QuaternionDCreateFromYawPitchRollTest2 Yaw:{yawAngle} Pitch:{pitchAngle} Roll:{rollAngle} did not return the expected value: expected {expected} actual {actual}");
}
}
}
}
public void QuaternionDCreateFromAxisAngleTest()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0, 2.0, 3.0));
double angle = MathHelper.ToRadians(30.0);
QuaternionD expected = new QuaternionD(0.0691723, 0.1383446, 0.207516879, 0.9659258);
QuaternionD actual;
actual = QuaternionD.CreateFromAxisAngle(axis, angle);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.CreateFromAxisAngle did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDCreateFromAxisAngleTest2()
{
Vector3D axis = new Vector3D(1, 0, 0);
double angle1 = MathHelper.ToRadians(30.0f);
double angle2 = MathHelper.ToRadians(750.0f);
QuaternionD actual1 = QuaternionD.CreateFromAxisAngle(axis, angle1);
QuaternionD actual2 = QuaternionD.CreateFromAxisAngle(axis, angle2);
Assert.True(MathHelper.Equal(actual1, actual2), $"QuaternionD.CreateFromAxisAngle did not return the expected value: actual1 {actual1} actual2 {actual2}");
}
public void QuaternionDCreateFromAxisAngleTest1()
{
Vector3D axis = new Vector3D();
double angle = MathHelper.ToRadians(-30.0f);
double cos = (double)System.Math.Cos(angle / 2.0f);
QuaternionD actual = QuaternionD.CreateFromAxisAngle(axis, angle);
Assert.True(actual.X == 0.0f && actual.Y == 0.0f && actual.Z == 0.0f && MathHelper.Equal(cos, actual.W)
, "QuaternionD.CreateFromAxisAngle did not return the expected value.");
}
public void QuaternionDSlerpTest4()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0, 2.0, 3.0));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0));
QuaternionD b = -QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(30.0));
double t = 0.0;
QuaternionD expected = new QuaternionD(a.X, a.Y, a.Z, a.W);
QuaternionD actual = QuaternionD.Slerp(a, b, t);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.Slerp did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDLerpTest2()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0f, 2.0f, 3.0f));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0f));
QuaternionD b = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(30.0f));
double t = 1.0f;
QuaternionD expected = new QuaternionD(b.X, b.Y, b.Z, b.W);
QuaternionD actual = QuaternionD.Lerp(a, b, t);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.Lerp did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDLerpTest3()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0f, 2.0f, 3.0f));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0f));
QuaternionD b = QuaternionD.Negate(a);
double t = 1.0f;
QuaternionD actual = QuaternionD.Lerp(a, b, t);
// Note that in QuaternionD world, Q == -Q. In the case of QuaternionDs dot product is zero,
// one of the QuaternionD will be flipped to compute the shortest distance. When t = 1, we
// expect the result to be the same as QuaternionD b but flipped.
Assert.True(actual == a, $"QuaternionD.Lerp did not return the expected value: expected {a} actual {actual}");
}
public void QuaternionDCreateFromYawPitchRollTest1()
{
double yawAngle = MathHelper.ToRadians(30.0f);
double pitchAngle = MathHelper.ToRadians(40.0f);
double rollAngle = MathHelper.ToRadians(50.0f);
QuaternionD yaw = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, yawAngle);
QuaternionD pitch = QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, pitchAngle);
QuaternionD roll = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, rollAngle);
QuaternionD expected = yaw * pitch * roll;
QuaternionD actual = QuaternionD.CreateFromYawPitchRoll(yawAngle, pitchAngle, rollAngle);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.QuaternionDCreateFromYawPitchRollTest1 did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDSlerpTest3()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0, 2.0, 3.0));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0));
QuaternionD b = -a;
double t = 1.0;
QuaternionD expected = a;
QuaternionD actual = QuaternionD.Slerp(a, b, t);
// Note that in QuaternionD world, Q == -Q. In the case of QuaternionDs dot product is zero,
// one of the QuaternionD will be flipped to compute the shortest distance. When t = 1, we
// expect the result to be the same as QuaternionD b but flipped.
Assert.That(actual, Is.EqualTo(expected).Using <QuaternionD>((a, b) => a.Equals(b, 1e-15)), $"QuaternionD.Slerp did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDFromRotationMatrixWithScaledMatrixTest3()
{
double angle = MathHelper.ToRadians(180.0f);
Matrix4x4D matrix = Matrix4x4D.CreateRotationX(angle) * Matrix4x4D.CreateRotationY(angle);
QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, angle) * QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, angle);
QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"QuaternionD.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"QuaternionD.CreateFromQuaternionD did not return the expected value: matrix {matrix} m2 {m2}");
}
public void QuaternionDFromRotationMatrixTest4()
{
for (double angle = 0.0f; angle < 720.0f; angle += 10.0f)
{
Matrix4x4D matrix = Matrix4x4D.CreateRotationZ(angle);
QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, angle);
QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"QuaternionD.CreateFromRotationMatrix angle:{angle} did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"QuaternionD.CreateFromQuaternionD angle:{angle} did not return the expected value: matrix {matrix} m2 {m2}");
}
}
public void QuaternionDSlerpTest()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0, 2.0, 3.0));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0));
QuaternionD b = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(30.0));
double t = 0.5;
QuaternionD expected = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(20.0));
QuaternionD actual;
actual = QuaternionD.Slerp(a, b, t);
Assert.That(actual, Is.EqualTo(expected).Using <QuaternionD>((a, b) => a.Equals(b, 1e-15)), $"QuaternionD.Slerp did not return the expected value: expected {expected} actual {actual}");
// Case a and b are same.
expected = a;
actual = QuaternionD.Slerp(a, a, t);
Assert.AreEqual(expected, actual, $"QuaternionD.Slerp did not return the expected value: expected {expected} actual {actual}");
}
public void QuaternionDLerpTest()
{
Vector3D axis = Vector3D.Normalize(new Vector3D(1.0f, 2.0f, 3.0f));
QuaternionD a = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0f));
QuaternionD b = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(30.0f));
double t = 0.5f;
QuaternionD expected = QuaternionD.CreateFromAxisAngle(axis, MathHelper.ToRadians(20.0f));
QuaternionD actual;
actual = QuaternionD.Lerp(a, b, t);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.Lerp did not return the expected value: expected {expected} actual {actual}");
// Case a and b are same.
expected = a;
actual = QuaternionD.Lerp(a, a, t);
Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.Lerp did not return the expected value: expected {expected} actual {actual}");
}
