[fix] physics-2d box2d syncRotationToPhysics not calculate Euler angle right #15398 (#14048)

cocos/core/math/mat3.ts

@@ -26,7 +26,7 @@ import { CCClass } from '../data/class';
 import { ValueType } from '../value-types/value-type';
 import { Quat } from './quat';
 import { IMat3Like, IMat4Like, IQuatLike, IVec2Like, IVec3Like } from './type-define';
-import { EPSILON } from './utils';
+import { EPSILON, HALF_PI } from './utils';
 import { Vec3 } from './vec3';
 import { legacyCC } from '../global-exports';
 
@@ -623,6 +623,41 @@ export class Mat3 extends ValueType {
         );
     }
 
+    /**
+     * @en Convert Matrix to euler angle, resulting angle y, z in the range of [-PI, PI],
+     *  x in the range of [-PI/2, PI/2], the rotation order is YXZ.
+     * @zh 将矩阵转换成欧拉角, 返回角度 y,z 在 [-PI, PI] 区间内, x 在 [-PI/2, PI/2] 区间内，旋转顺序为 YXZ.
+     */
+    public static toEuler (matrix: Mat3,  v: Vec3): boolean {
+        //a[col][row]
+        const a00 = matrix.m00; const a01 = matrix.m01; const a02 = matrix.m02;
+        const a10 = matrix.m03; const a11 = matrix.m04; const a12 = matrix.m05;
+        const a20 = matrix.m06; const a21 = matrix.m07; const a22 = matrix.m08;
+
+        // from http://www.geometrictools.com/Documentation/EulerAngles.pdf
+        // YXZ order
+        if (a21 < 0.999) {
+            if (a21 > -0.999) {
+                v.x = Math.asin(-a21);
+                v.y = Math.atan2(a20, a22);
+                v.z = Math.atan2(a01, a11);
+                return true;
+            } else {
+                // Not unique.  YA - ZA = atan2(r01,r00)
+                v.x = HALF_PI;
+                v.y = Math.atan2(a10, a00);
+                v.z = 0.0;
+                return false;
+            }
+        } else {
+            // Not unique.  YA + ZA = atan2(-r01,r00)
+            v.x = -HALF_PI;
+            v.y = Math.atan2(-a10, a00);
+            v.z = 0.0;
+            return false;
+        }
+    }
+
     /**
      * @en Value at column 0 row 0 of the matrix.
      * @zh 矩阵第 0 列第 0 行的元素。

cocos/core/math/quat.ts

@@ -613,6 +613,18 @@ export class Quat extends ValueType {
         return out;
     }
 
+    /**
+     * @en Converts the quaternion to euler angles, result angle y, z in the range of [-180, 180], x in the range of [-90, 90], the rotation order is YXZ
+     * @zh 根据四元数计算欧拉角，返回角度 yz 在 [-180, 180], x 在 [-90, 90]，旋转顺序为 YXZ
+     */
+    public static toEulerInYXZOrder (out: Vec3, q: IQuatLike) {
+        Mat3.fromQuat(m3_1, q);
+        Mat3.toEuler(m3_1, out);
+        out.x = toDegree(out.x);
+        out.y = toDegree(out.y);
+        out.z = toDegree(out.z);
+    }
+
     /**
      * @en Converts quaternion to an array
      * @zh 四元数转数组

cocos/core/math/utils.ts

@@ -31,6 +31,9 @@ const _d2r = Math.PI / 180.0;
 
 const _r2d = 180.0 / Math.PI;
 
+export const HALF_PI = Math.PI * 0.5;
+export const TWO_PI = Math.PI * 2.0;
+
 export const EPSILON = 0.000001;
 
 /**

cocos/physics-2d/box2d/rigid-body.ts

@@ -27,7 +27,7 @@ import { IRigidBody2D } from '../spec/i-rigid-body';
 import { RigidBody2D } from '../framework/components/rigid-body-2d';
 import { PhysicsSystem2D } from '../framework/physics-system';
 import { b2PhysicsWorld } from './physics-world';
-import { Vec2, toRadian, Vec3, Quat, IVec2Like, toDegree } from '../../core';
+import { Vec2, toRadian, Vec3, Quat, IVec2Like, toDegree, TWO_PI, HALF_PI } from '../../core';
 import { PHYSICS_2D_PTM_RATIO, ERigidBody2DType } from '../framework/physics-types';
 
 import { Node } from '../../scene-graph/node';
@@ -132,8 +132,21 @@ export class b2RigidBody2D implements IRigidBody2D {
         tempVec2_1.y = (this._animatedPos.y - b2Pos.y) * timeStep;
         b2body.SetLinearVelocity(tempVec2_1);
 
-        const b2Rotation = b2body.GetAngle();
-        b2body.SetAngularVelocity((this._animatedAngle - b2Rotation) * timeStep);
+        //convert b2Rotation to [-PI~PI], which is the same as this._animatedAngle
+        let b2Rotation = b2body.GetAngle() % (TWO_PI);
+        if (b2Rotation > Math.PI) {
+            b2Rotation -= TWO_PI;
+        }
+
+        //calculate angular velocity
+        let angularVelocity = (this._animatedAngle - b2Rotation) * timeStep;
+        if (this._animatedAngle < -HALF_PI && b2Rotation > HALF_PI) { //ccw, crossing PI
+            angularVelocity = (this._animatedAngle + TWO_PI - b2Rotation) * timeStep;
+        } if (this._animatedAngle > HALF_PI && b2Rotation < -HALF_PI) { //cw, crossing PI
+            angularVelocity = (this._animatedAngle - TWO_PI - b2Rotation) * timeStep;
+        }
+
+        b2body.SetAngularVelocity(angularVelocity);
     }
 
     syncSceneToPhysics () {
@@ -171,7 +184,7 @@ export class b2RigidBody2D implements IRigidBody2D {
 
         const rot = this._rigidBody.node.worldRotation;
         const euler = tempVec3;
-        Quat.toEuler(euler, rot);
+        Quat.toEulerInYXZOrder(euler, rot);
         const rotation = toRadian(euler.z);
 
         const bodyType = this._rigidBody.type;