Added CreateTransform implementation for matrix4

technologists-team · Jul 12, 2024 · 2f9db05 · 2f9db05
1 parent e981967
commit 2f9db05
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 6 deletions.
diff --git a/Hypercube.Shared.Math/Matrix/Matrix4X4.cs b/Hypercube.Shared.Math/Matrix/Matrix4X4.cs
@@ -335,6 +335,50 @@ public static Matrix4X4 Transpose(Matrix4X4 matrix4X4)
         return new Matrix4X4(matrix4X4.Column0, matrix4X4.Column1, matrix4X4.Column2, matrix4X4.Column3);
     }
 
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix4X4 CreateTransform(Transform3 transform3)
+    {
+        return CreateTransform(transform3.Position, transform3.Rotation, transform3.Scale);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix4X4 CreateTransform(Vector3 position, Quaternion quaternion, Vector3 scale)
+    {
+        var xx = quaternion.X * quaternion.X;
+        var yy = quaternion.Y * quaternion.Y;
+        var zz = quaternion.Z * quaternion.Z;
+
+        var xy = quaternion.X * quaternion.Y;
+        var wz = quaternion.Z * quaternion.W;
+        var xz = quaternion.Z * quaternion.X;
+        var wy = quaternion.Y * quaternion.W;
+        var yz = quaternion.Y * quaternion.Z;
+        var wx = quaternion.X * quaternion.W;
+
+        var x3 = new Vector3(
+            1.0f - 2.0f * (yy + zz),
+            2.0f * (xy + wz),
+            2.0f * (xz - wy)
+        ) * scale.X;
+        var x = new Vector4(x3, (x3 * position.X).Sum());
+
+        var y3 = new Vector3(
+            2.0f * (xy - wz),
+            1.0f - 2.0f * (zz + xx),
+            2.0f * (yz + wx)
+        )* scale.Y;
+        var y = new Vector4(y3, (y3 * position.Y).Sum());
+
+        var z3 = new Vector3(
+            2.0f * (xz + wy),
+            2.0f * (yz - wx),
+            1.0f - 2.0f * (yy + xx)
+        ) * scale.Z;
+        var z = new Vector4(z3, (z3 * position.Z).Sum());
+
+        return new Matrix4X4(x, y, z, Vector4.UnitW);
+    }
+
     /// <summary>
     /// Creating scale matrix
     /// <code>

diff --git a/Hypercube.Shared.Math/Transform/Transform2.cs b/Hypercube.Shared.Math/Transform/Transform2.cs
@@ -55,8 +55,6 @@ public Transform2 SetScale(Vector2 scale)
 
     private void UpdateMatrix()
     {
-        Matrix = Matrix4X4.CreateTranslation(Position) *
-                 Matrix4X4.CreateRotationZ((float)Rotation) *
-                 Matrix4X4.CreateScale(Scale);
+        Matrix = Matrix4X4.CreateTransform(this);
     }
 }
diff --git a/Hypercube.Shared.Math/Transform/Transform3.cs b/Hypercube.Shared.Math/Transform/Transform3.cs
@@ -61,8 +61,6 @@ public Transform3 SetScale(Vector3 scale)
 
     private void UpdateMatrix()
     {
-        Matrix = Matrix4X4.CreateTranslation(Position) *
-                 Matrix4X4.CreateRotation(Rotation) *
-                 Matrix4X4.CreateScale(Scale);
+        Matrix = Matrix4X4.CreateTransform(this);
     }
 }
diff --git a/Hypercube.Shared.Math/Vector/Vector4.cs b/Hypercube.Shared.Math/Vector/Vector4.cs
@@ -56,6 +56,10 @@ public Vector4(float value) : this(value, value, value, value)
     public Vector4(Vector2 vector2, float z, float w) : this(vector2.X, vector2.Y, z, w)
     {
     }
+
+    public Vector4(Vector3 vector3, float w) : this(vector3.X, vector3.Y, vector3.Z, w)
+    {
+    }
 
     public Vector4(Vector4 vector4, float w) : this(vector4.X, vector4.Y, vector4.Z, w)
     {