fix mat4 getRotation (cocos#14183)

cocos/core/math/mat4.ts

@@ -2024,7 +2024,7 @@ export class Mat4 extends ValueType {
         out.x = Math.sqrt(m00 * m00 + m01 * m01 + m02 * m02);
         out.y = Math.sqrt(m04 * m04 + m05 * m05 + m06 * m06);
         out.z = Math.sqrt(m08 * m08 + m09 * m09 + m10 * m10);
-        // account for refections
+        // account for reflections
         if (Mat3.determinant(m3_1) < 0) { out.x *= -1; }
         return out;
     }
@@ -2035,36 +2035,27 @@ export class Mat4 extends ValueType {
      * @param out Vector to receive rotation component
      */
     public getRotation (out: Quat) {
-        const trace = this.m00 + this.m05 + this.m10;
-        let S = 0;
-
-        if (trace > 0) {
-            S = Math.sqrt(trace + 1.0) * 2;
-            out.w = 0.25 * S;
-            out.x = (this.m06 - this.m09) / S;
-            out.y = (this.m08 - this.m02) / S;
-            out.z = (this.m01 - this.m04) / S;
-        } else if ((this.m00 > this.m05) && (this.m00 > this.m10)) {
-            S = Math.sqrt(1.0 + this.m00 - this.m05 - this.m10) * 2;
-            out.w = (this.m06 - this.m09) / S;
-            out.x = 0.25 * S;
-            out.y = (this.m01 + this.m04) / S;
-            out.z = (this.m08 + this.m02) / S;
-        } else if (this.m05 > this.m10) {
-            S = Math.sqrt(1.0 + this.m05 - this.m00 - this.m10) * 2;
-            out.w = (this.m08 - this.m02) / S;
-            out.x = (this.m01 + this.m04) / S;
-            out.y = 0.25 * S;
-            out.z = (this.m06 + this.m09) / S;
-        } else {
-            S = Math.sqrt(1.0 + this.m10 - this.m00 - this.m05) * 2;
-            out.w = (this.m01 - this.m04) / S;
-            out.x = (this.m08 + this.m02) / S;
-            out.y = (this.m06 + this.m09) / S;
-            out.z = 0.25 * S;
+        // Extract rotation matrix first
+        const sx = Vec3.set(v3_1, this.m00, this.m01, this.m02).length();
+        const sy = Vec3.set(v3_1, this.m04, this.m05, this.m06).length();
+        const sz = Vec3.set(v3_1, this.m08, this.m09, this.m10).length();
+        m3_1.m00 = this.m00 / sx;
+        m3_1.m01 = this.m01 / sx;
+        m3_1.m02 = this.m02 / sx;
+        m3_1.m03 = this.m04 / sy;
+        m3_1.m04 = this.m05 / sy;
+        m3_1.m05 = this.m06 / sy;
+        m3_1.m06 = this.m08 / sz;
+        m3_1.m07 = this.m09 / sz;
+        m3_1.m08 = this.m10 / sz;
+        const det = Mat3.determinant(m3_1);
+        if (det < 0) {
+            m3_1.m00 *= -1;
+            m3_1.m01 *= -1;
+            m3_1.m02 *= -1;
         }
 
-        return out;
+        return Quat.fromMat3(out, m3_1);
     }
 
     /**

cocos/core/math/quat.ts

@@ -396,8 +396,8 @@ export class Quat extends ValueType {
     }
 
     /**
-     * @en Normalize the given quaternion, returns a zero quaternion if input is a zero quaternion
-     * @zh 归一化四元数，输入零四元数将会返回零四元数
+     * @en Normalize the given quaternion, returns a zero quaternion if input is a zero quaternion.
+     * @zh 归一化四元数，输入零四元数将会返回零四元数。
      */
     public static normalize<Out extends IQuatLike> (out: Out, a: Out) {
         let len = a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w;
@@ -457,7 +457,7 @@ export class Quat extends ValueType {
      * @en Calculates the quaternion with the three-dimensional transform matrix, considering no scale included in the matrix
      * @zh 根据三维矩阵信息计算四元数，默认输入矩阵不含有缩放信息
      */
-    public static fromMat3<Out extends IQuatLike> (out: Out, m: Mat3) {
+    public static  fromMat3<Out extends IQuatLike> (out: Out, m: Mat3) {
         const {
             m00, m03: m01, m06: m02,
             m01: m10, m04: m11, m07: m12,
@@ -474,26 +474,26 @@ export class Quat extends ValueType {
             out.y = (m02 - m20) * s;
             out.z = (m10 - m01) * s;
         } else if ((m00 > m11) && (m00 > m22)) {
-            const s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22);
+            const s = 0.5 / Math.sqrt(1.0 + m00 - m11 - m22);
 
-            out.w = (m21 - m12) / s;
-            out.x = 0.25 * s;
-            out.y = (m01 + m10) / s;
-            out.z = (m02 + m20) / s;
+            out.w = (m21 - m12) * s;
+            out.x = 0.25 / s;
+            out.y = (m01 + m10) * s;
+            out.z = (m02 + m20) * s;
         } else if (m11 > m22) {
-            const s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22);
+            const s = 0.5 / Math.sqrt(1.0 + m11 - m00 - m22);
 
-            out.w = (m02 - m20) / s;
-            out.x = (m01 + m10) / s;
-            out.y = 0.25 * s;
-            out.z = (m12 + m21) / s;
+            out.w = (m02 - m20) * s;
+            out.x = (m01 + m10) * s;
+            out.y = 0.25 / s;
+            out.z = (m12 + m21) * s;
         } else {
-            const s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11);
+            const s = 0.5 / Math.sqrt(1.0 + m22 - m00 - m11);
 
-            out.w = (m10 - m01) / s;
-            out.x = (m02 + m20) / s;
-            out.y = (m12 + m21) / s;
-            out.z = 0.25 * s;
+            out.w = (m10 - m01) * s;
+            out.x = (m02 + m20) * s;
+            out.y = (m12 + m21) * s;
+            out.z = 0.25 / s;
         }
 
         return out;

cocos/core/math/vec2.ts

@@ -265,8 +265,8 @@ export class Vec2 extends ValueType {
     }
 
     /**
-     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector
-     * @zh 归一化向量，输入零向量将会返回零向量
+     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector.
+     * @zh 归一化向量，输入零向量将会返回零向量。
      */
     public static normalize <Out extends IVec2Like, Vec2Like extends IVec2Like> (out: Out, a: Vec2Like) {
         const x = a.x;
@@ -415,15 +415,14 @@ export class Vec2 extends ValueType {
     }
 
     /**
-     * @en Calculates the radian angle between two vectors, returns zero if either vector is a zero vector
-     * @zh 求两向量夹角弧度，任意一个向量是零向量则返回零
+     * @en Calculates the radian angle between two vectors, returns zero if either vector is a zero vector.
+     * @zh 求两向量夹角弧度，任意一个向量是零向量则返回零。
      */
     public static angle <Out extends IVec2Like> (a: Out, b: Out) {
         const magSqr1 = a.x * a.x + a.y * a.y;
         const magSqr2 = b.x * b.x + b.y * b.y;
 
         if (magSqr1 === 0 || magSqr2 === 0) {
-            console.warn('Can\'t get angle between zero vector');
             return 0.0;
         }
 
@@ -756,17 +755,16 @@ export class Vec2 extends ValueType {
     }
 
     /**
-     * @en Calculates radian angle between two vectors, returns zero if either vector is a zero vector
+     * @en Calculates radian angle between two vectors, returns zero if either vector is a zero vector.
      * @zh 获取当前向量和指定向量之间的角度，任意一个向量是零向量则返回零。
-     * @param other specified vector
+     * @param other specified vector.
      * @return The angle between the current vector and the specified vector.
      */
     public angle (other: Vec2) {
         const magSqr1 = this.lengthSqr();
         const magSqr2 = other.lengthSqr();
 
         if (magSqr1 === 0 || magSqr2 === 0) {
-            console.warn('Can\'t get angle between zero vector');
             return 0.0;
         }
 

cocos/core/math/vec3.ts

@@ -305,8 +305,8 @@ export class Vec3 extends ValueType {
     }
 
     /**
-     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector
-     * @zh 归一化向量，输入零向量将会返回零向量
+     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector.
+     * @zh 归一化向量，输入零向量将会返回零向量。
      */
     public static normalize<Out extends IVec3Like> (out: Out, a: IVec3Like) {
         const x = a.x;
@@ -707,7 +707,6 @@ export class Vec3 extends ValueType {
         const magSqr2 = b.x * b.x + b.y * b.y + b.z * b.z;
 
         if (magSqr1 === 0 || magSqr2 === 0) {
-            console.warn('Can\'t get angle between zero vector');
             return 0.0;
         }
 

cocos/core/math/vec4.ts

@@ -313,8 +313,8 @@ export class Vec4 extends ValueType {
     }
 
     /**
-     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector
-     * @zh 归一化向量，输入零向量将会返回零向量
+     * @en Sets the normalized vector to the out vector, returns a zero vector if input is a zero vector.
+     * @zh 归一化向量，输入零向量将会返回零向量。
      */
     public static normalize <Out extends IVec4Like> (out: Out, a: Out) {
         const x = a.x;

native/cocos/math/Mat4.cpp

@@ -555,7 +555,7 @@ bool Mat4::decompose(Vec3 *scale, Quaternion *rotation, Vec3 *translation) const
     // In this case, we simply negate a single axis of the scale.
     float det = determinant();
     if (det < 0) {
-        scaleZ = -scaleZ;
+        scaleX = -scaleX;
     }
 
     if (scale) {
@@ -593,10 +593,10 @@ bool Mat4::decompose(Vec3 *scale, Quaternion *rotation, Vec3 *translation) const
     zaxis.z *= rn;
 
     // Now calculate the rotation from the resulting matrix (axes).
-    float trace = xaxis.x + yaxis.y + zaxis.z + 1.0F;
+    float trace = xaxis.x + yaxis.y + zaxis.z;
 
-    if (trace > MATH_EPSILON) {
-        float s = 0.5F / std::sqrt(trace);
+    if (trace > 0.0F) {
+        float s = 0.5F / std::sqrt(trace + 1.0F);
         rotation->w = 0.25F / s;
         rotation->x = (yaxis.z - zaxis.y) * s;
         rotation->y = (zaxis.x - xaxis.z) * s;

tests/core/math/mat4.test.ts

@@ -0,0 +1,26 @@
+import { Mat4, Quat, Vec3 } from "../../../cocos/core";
+
+test('Conversion between TRS', () => {
+    const t = new Vec3(1., 2., 3.);
+    const r = new Quat(4., 5., 6., 7.);
+    Quat.normalize(r, r);
+    const s = new Vec3(8., 9., 10.);
+
+    const mat4 = new Mat4();
+    expect(Mat4.fromRTS(
+        mat4,
+        r,
+        t,
+        s,
+    )).toBe(mat4);
+    const t2 = new Vec3();
+    const r2 = new Quat();
+    const s2 = new Vec3();
+    Mat4.toRTS(mat4, r2, t2, s2);
+    expect(Vec3.equals(t2, t)).toBe(true);
+    expect(Vec3.equals(s2, s)).toBe(true);
+    expect(Quat.equals(r2, r)).toBe(true);
+    expect(Vec3.equals(mat4.getTranslation(new Vec3()), t)).toBe(true);
+    expect(Vec3.equals(mat4.getScale(new Vec3()), s)).toBe(true);
+    expect(Quat.equals(mat4.getRotation(new Quat()), r)).toBe(true);
+});