// Transform by matrix
public void PlaneTransformTest1()
{
PlaneD target = new PlaneD(1, 2, 3, 4);
target = PlaneD.Normalize(target);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
PlaneD expected = new PlaneD();
Matrix4x4D.Invert(m, out var inv);
Matrix4x4D itm = Matrix4x4D.Transpose(inv);
double x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3D(
x * itm.M11 + y * itm.M21 + z * itm.M31 + w * itm.M41,
x * itm.M12 + y * itm.M22 + z * itm.M32 + w * itm.M42,
x * itm.M13 + y * itm.M23 + z * itm.M33 + w * itm.M43);
expected.D = x * itm.M14 + y * itm.M24 + z * itm.M34 + w * itm.M44;
PlaneD actual;
actual = PlaneD.Transform(target, m);
Assert.True(MathHelper.Equal(expected, actual), "PlaneD.Transform did not return the expected value.");
}
// Transform by QuaternionD
public void PlaneTransformTest2()
{
PlaneD target = new PlaneD(1, 2, 3, 4);
target = PlaneD.Normalize(target);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
PlaneD expected = new PlaneD();
double x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3D(
x * m.M11 + y * m.M21 + z * m.M31 + w * m.M41,
x * m.M12 + y * m.M22 + z * m.M32 + w * m.M42,
x * m.M13 + y * m.M23 + z * m.M33 + w * m.M43);
expected.D = x * m.M14 + y * m.M24 + z * m.M34 + w * m.M44;
PlaneD actual;
actual = PlaneD.Transform(target, q);
Assert.True(MathHelper.Equal(expected, actual), "PlaneD.Transform did not return the expected value.");
}
public void Matrix3x3DDeterminantTest()
{
Matrix3x3D target = As3x3(
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0)));
double val = 1.0;
double det = target.GetDeterminant();
Assert.AreEqual(val, det, 1e-15, "Matrix3x3D.Determinant was not set correctly.");
}
public void Vector3TransformByQuaternionTest()
{
Vector3D v = new Vector3D(1.0f, 2.0f, 3.0f);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
Vector3D expected = Vector3D.Transform(v, m);
Vector3D actual = Vector3D.Transform(v, q);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D.Transform did not return the expected value.");
}
public void Vector2TransformByQuaternionTest()
{
Vector2D v = new Vector2D(1.0, 2.0);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
Vector2D expected = Vector2D.Transform(v, m);
Vector2D actual = Vector2D.Transform(v, q);
Assert.That(actual, Is.EqualTo(expected).Using <Vector2D>((a, b) => a.Equals(b, 1e-15)), "Vector2D.Transform did not return the expected value.");
}
public void QuaternionDFromRotationMatrixWithScaledMatrixTest2()
{
double angle = MathHelper.ToRadians(180.0f);
Matrix4x4D matrix = Matrix4x4D.CreateRotationX(angle) * Matrix4x4D.CreateRotationZ(angle);
QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, 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 Vector3TransformTest()
{
Vector3D v = new Vector3D(1.0f, 2.0f, 3.0f);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
Vector3D expected = new Vector3D(12.191987f, 21.533493f, 32.616024f);
Vector3D actual;
actual = Vector3D.Transform(v, m);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D.Transform did not return the expected value.");
}
public void Vector2TransformTest()
{
Vector2D v = new Vector2D(1.0, 2.0);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0));
m.M41 = 10.0;
m.M42 = 20.0;
m.M43 = 30.0;
Vector2D expected = new Vector2D(10.316987298107781, 22.18301270189222);
Vector2D actual;
actual = Vector2D.Transform(v, m);
Assert.AreEqual(expected, actual, "Vector2D.Transform did not return the expected value.");
}
