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