feat(raymath): MatrixDecompose

raylib/raymath.go

@@ -1792,3 +1792,56 @@ func QuaternionEquals(p, q Quaternion) bool {
 			math.Abs(float64(p.Z+q.Z)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Z)), math.Abs(float64(q.Z)))) &&
 			math.Abs(float64(p.W+q.W)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.W)), math.Abs(float64(q.W)))))
 }
+
+// MatrixDecompose - Decompose a transformation matrix into its rotational, translational and scaling components
+func MatrixDecompose(mat Matrix, translation *Vector3, rotation *Quaternion, scale *Vector3) {
+	// Extract translation.
+	translation.X = mat.M12
+	translation.Y = mat.M13
+	translation.Z = mat.M14
+
+	// Extract upper-left for determinant computation
+	a := mat.M0
+	b := mat.M4
+	c := mat.M8
+	d := mat.M1
+	e := mat.M5
+	f := mat.M9
+	g := mat.M2
+	h := mat.M6
+	i := mat.M10
+	A := e*i - f*h
+	B := f*g - d*i
+	C := d*h - e*g
+
+	// Extract scale
+	det := a*A + b*B + c*C
+	abc := NewVector3(a, b, c)
+	def := NewVector3(d, e, f)
+	ghi := NewVector3(g, h, i)
+
+	scalex := Vector3Length(abc)
+	scaley := Vector3Length(def)
+	scalez := Vector3Length(ghi)
+	s := NewVector3(scalex, scaley, scalez)
+
+	if det < 0 {
+		s = Vector3Negate(s)
+	}
+
+	*scale = s
+
+	// Remove scale from the matrix if it is not close to zero
+	clone := mat
+	if !FloatEquals(det, 0) {
+		clone.M0 /= s.X
+		clone.M5 /= s.Y
+		clone.M10 /= s.Z
+
+		// Extract rotation
+		*rotation = QuaternionFromMatrix(clone)
+	} else {
+		// Set to identity if close to zero
+		*rotation = QuaternionIdentity()
+	}
+}

raylib/raymath.h

@@ -2524,4 +2524,59 @@ RMAPI int QuaternionEquals(Quaternion p, Quaternion q)
     return result;
 }
 
+// Decompose a transformation matrix into its rotational, translational and scaling components
+RMAPI void MatrixDecompose(Matrix mat, Vector3 *translation, Quaternion *rotation, Vector3 *scale)
+{
+    // Extract translation.
+    translation->x = mat.m12;
+    translation->y = mat.m13;
+    translation->z = mat.m14;
+
+    // Extract upper-left for determinant computation
+    const float a = mat.m0;
+    const float b = mat.m4;
+    const float c = mat.m8;
+    const float d = mat.m1;
+    const float e = mat.m5;
+    const float f = mat.m9;
+    const float g = mat.m2;
+    const float h = mat.m6;
+    const float i = mat.m10;
+    const float A = e*i - f*h;
+    const float B = f*g - d*i;
+    const float C = d*h - e*g;
+
+    // Extract scale
+    const float det = a*A + b*B + c*C;
+    Vector3 abc = { a, b, c };
+    Vector3 def = { d, e, f };
+    Vector3 ghi = { g, h, i };
+
+    float scalex = Vector3Length(abc);
+    float scaley = Vector3Length(def);
+    float scalez = Vector3Length(ghi);
+    Vector3 s = { scalex, scaley, scalez };
+
+    if (det < 0) s = Vector3Negate(s);
+
+    *scale = s;
+
+    // Remove scale from the matrix if it is not close to zero
+    Matrix clone = mat;
+    if (!FloatEquals(det, 0))
+    {
+        clone.m0 /= s.x;
+        clone.m5 /= s.y;
+        clone.m10 /= s.z;
+
+        // Extract rotation
+        *rotation = QuaternionFromMatrix(clone);
+    }
+    else
+    {
+        // Set to identity if close to zero
+        *rotation = QuaternionIdentity();
+    }
+}
+
 #endif  // RAYMATH_H