// Transform by quaternion
public void PlaneTransformTest2()
{
Plane target = new Plane(1, 2, 3, 4);
target = Plane.Normalize(target);
Matrix4x4 m =
Matrix4x4.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationZ(MathHelper.ToRadians(30.0f));
Quaternion q = Quaternion.CreateFromRotationMatrix(m);
Plane expected = new Plane();
Single x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3(
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;
Plane actual;
actual = Plane.Transform(target, q);
Assert.True(MathHelper.Equal(expected, actual), "Plane.Transform did not return the expected value.");
}
// Transform by matrix
public void PlaneTransformTest1()
{
Plane target = new Plane(1, 2, 3, 4);
target = Plane.Normalize(target);
Matrix4x4 m =
Matrix4x4.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
Plane expected = new Plane();
Matrix4x4 inv;
Matrix4x4.Invert(m, out inv);
Matrix4x4 itm = Matrix4x4.Transpose(inv);
Single x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3(
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;
Plane actual;
actual = Plane.Transform(target, m);
Assert.True(MathHelper.Equal(expected, actual), "Plane.Transform did not return the expected value.");
}
public static Vector3 TransformNormal(Vector3 normal, Matrix4x4 matrix)
{
return(new Vector3(
normal.X * matrix.M11 + normal.Y * matrix.M21 + normal.Z * matrix.M31,
normal.X * matrix.M12 + normal.Y * matrix.M22 + normal.Z * matrix.M32,
normal.X * matrix.M13 + normal.Y * matrix.M23 + normal.Z * matrix.M33));
}
public static Vector3 Transform(Vector3 position, Matrix4x4 matrix)
{
return(new Vector3(
position.X * matrix.M11 + position.Y * matrix.M21 + position.Z * matrix.M31 + matrix.M41,
position.X * matrix.M12 + position.Y * matrix.M22 + position.Z * matrix.M32 + matrix.M42,
position.X * matrix.M13 + position.Y * matrix.M23 + position.Z * matrix.M33 + matrix.M43));
}
public void Matrix3x2MultiplyTest3()
{
Matrix3x2 a = GenerateMatrixNumberFrom1To6();
Matrix3x2 b = GenerateMatrixNumberFrom1To6();
Matrix3x2 expected = new Matrix3x2();
expected.M11 = a.M11 * b.M11 + a.M12 * b.M21;
expected.M12 = a.M11 * b.M12 + a.M12 * b.M22;
expected.M21 = a.M21 * b.M11 + a.M22 * b.M21;
expected.M22 = a.M21 * b.M12 + a.M22 * b.M22;
expected.M31 = a.M31 * b.M11 + a.M32 * b.M21 + b.M31;
expected.M32 = a.M31 * b.M12 + a.M32 * b.M22 + b.M32;
Matrix3x2 actual;
actual = Matrix3x2.Multiply(a, b);
Assert.Equal(expected, actual);
// Sanity check by comparison with 4x4 multiply.
a = Matrix3x2.CreateRotation(MathHelper.ToRadians(30)) * Matrix3x2.CreateTranslation(23, 42);
b = Matrix3x2.CreateScale(3, 7) * Matrix3x2.CreateTranslation(666, -1);
actual = Matrix3x2.Multiply(a, b);
Matrix4x4 a44 = new Matrix4x4(a);
Matrix4x4 b44 = new Matrix4x4(b);
Matrix4x4 expected44 = Matrix4x4.Multiply(a44, b44);
Matrix4x4 actual44 = new Matrix4x4(actual);
Assert.True(MathHelper.Equal(expected44, actual44), "Matrix3x2.Multiply did not return the expected value.");
}
public static Vector4 Transform(Vector4 vector, Matrix4x4 matrix)
{
return(new Vector4(
vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31 + vector.W * matrix.M41,
vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32 + vector.W * matrix.M42,
vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33 + vector.W * matrix.M43,
vector.X * matrix.M14 + vector.Y * matrix.M24 + vector.Z * matrix.M34 + vector.W * matrix.M44));
}
public static Vector4 Transform(Vector2 position, Matrix4x4 matrix)
{
return(new Vector4(
position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M41,
position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M42,
position.X * matrix.M13 + position.Y * matrix.M23 + matrix.M43,
position.X * matrix.M14 + position.Y * matrix.M24 + matrix.M44));
}
public static bool Equal(Matrix4x4 a, Matrix4x4 b)
{
return
(Equal(a.M11, b.M11) && Equal(a.M12, b.M12) && Equal(a.M13, b.M13) && Equal(a.M14, b.M14) &&
Equal(a.M21, b.M21) && Equal(a.M22, b.M22) && Equal(a.M23, b.M23) && Equal(a.M24, b.M24) &&
Equal(a.M31, b.M31) && Equal(a.M32, b.M32) && Equal(a.M33, b.M33) && Equal(a.M34, b.M34) &&
Equal(a.M41, b.M41) && Equal(a.M42, b.M42) && Equal(a.M43, b.M43) && Equal(a.M44, b.M44));
}
public static Plane Transform(Plane plane, Matrix4x4 matrix)
{
Matrix4x4 m;
Matrix4x4.Invert(matrix, out m);
Single x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z, w = plane.D;
return(new Plane(
x * m.M11 + y * m.M12 + z * m.M13 + w * m.M14,
x * m.M21 + y * m.M22 + z * m.M23 + w * m.M24,
x * m.M31 + y * m.M32 + z * m.M33 + w * m.M34,
x * m.M41 + y * m.M42 + z * m.M43 + w * m.M44));
}
public void Vector3TransformByQuaternionTest()
{
Vector3 v = new Vector3(1.0f, 2.0f, 3.0f);
Matrix4x4 m =
Matrix4x4.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationZ(MathHelper.ToRadians(30.0f));
Quaternion q = Quaternion.CreateFromRotationMatrix(m);
Vector3 expected = Vector3.Transform(v, m);
Vector3 actual = Vector3.Transform(v, q);
Assert.True(MathHelper.Equal(expected, actual), "Vector3f.Transform did not return the expected value.");
}
/// <summary>
/// Creates a Quaternion from the given rotation matrix.
/// </summary>
/// <param name="matrix">The rotation matrix.</param>
/// <returns>The created Quaternion.</returns>
public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix)
{
Single trace = matrix.M11 + matrix.M22 + matrix.M33;
Quaternion q = new Quaternion();
if (trace > 0.0f)
{
Single s = MathF.Sqrt(trace + 1.0f);
q.W = s * 0.5f;
s = 0.5f / s;
q.X = (matrix.M23 - matrix.M32) * s;
q.Y = (matrix.M31 - matrix.M13) * s;
q.Z = (matrix.M12 - matrix.M21) * s;
}
else
{
if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
{
Single s = MathF.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
Single invS = 0.5f / s;
q.X = 0.5f * s;
q.Y = (matrix.M12 + matrix.M21) * invS;
q.Z = (matrix.M13 + matrix.M31) * invS;
q.W = (matrix.M23 - matrix.M32) * invS;
}
else if (matrix.M22 > matrix.M33)
{
Single s = MathF.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
Single invS = 0.5f / s;
q.X = (matrix.M21 + matrix.M12) * invS;
q.Y = 0.5f * s;
q.Z = (matrix.M32 + matrix.M23) * invS;
q.W = (matrix.M31 - matrix.M13) * invS;
}
else
{
Single s = MathF.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
Single invS = 0.5f / s;
q.X = (matrix.M31 + matrix.M13) * invS;
q.Y = (matrix.M32 + matrix.M23) * invS;
q.Z = 0.5f * s;
q.W = (matrix.M12 - matrix.M21) * invS;
}
}
return(q);
}
public void QuaternionFromRotationMatrixTest1()
{
Matrix4x4 matrix = Matrix4x4.Identity;
Quaternion expected = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
Quaternion actual = Quaternion.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.Equal(expected, actual),
$"Quaternion.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4 m2 = Matrix4x4.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"Quaternion.CreateFromQuaternion did not return the expected value: matrix {matrix} m2 {m2}");
}
public void QuaternionFromRotationMatrixWithScaledMatrixTest3()
{
Single angle = MathHelper.ToRadians(180.0f);
Matrix4x4 matrix = Matrix4x4.CreateRotationX(angle) * Matrix4x4.CreateRotationY(angle);
Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitY, angle) * Quaternion.CreateFromAxisAngle(Vector3.UnitX, angle);
Quaternion actual = Quaternion.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"Quaternion.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4 m2 = Matrix4x4.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"Quaternion.CreateFromQuaternion did not return the expected value: matrix {matrix} m2 {m2}");
}
public void QuaternionFromRotationMatrixTest4()
{
for (Single angle = 0.0f; angle < 720.0f; angle += 10.0f)
{
Matrix4x4 matrix = Matrix4x4.CreateRotationZ(angle);
Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, angle);
Quaternion actual = Quaternion.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"Quaternion.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.
Matrix4x4 m2 = Matrix4x4.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"Quaternion.CreateFromQuaternion angle:{angle} did not return the expected value: matrix {matrix} m2 {m2}");
}
}
public void Vector2TransformNormalTest()
{
Vector2 v = new Vector2(1.0f, 2.0f);
Matrix4x4 m =
Matrix4x4.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
Vector2 expected = new Vector2(0.3169873f, 2.18301272f);
Vector2 actual;
actual = Vector2.TransformNormal(v, m);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Tranform did not return the expected value.");
}
public void Vector3TransformNormalTest()
{
Vector3 v = new Vector3(1.0f, 2.0f, 3.0f);
Matrix4x4 m =
Matrix4x4.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
Vector3 expected = new Vector3(2.19198728f, 1.53349364f, 2.61602545f);
Vector3 actual;
actual = Vector3.TransformNormal(v, m);
Assert.True(MathHelper.Equal(expected, actual), "Vector3f.TransformNormal did not return the expected value.");
}
public static Vector2 TransformNormal(Vector2 normal, Matrix4x4 matrix)
{
return(new Vector2(
normal.X * matrix.M11 + normal.Y * matrix.M21,
normal.X * matrix.M12 + normal.Y * matrix.M22));
}
public static Vector2 Transform(Vector2 position, Matrix4x4 matrix)
{
return(new Vector2(
position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M41,
position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M42));
}
