Fix rotation error when add joint

packages/core/src/physics/DynamicCollider.ts

@@ -9,6 +9,9 @@ import { PhysicsScene } from "./PhysicsScene";
  * A dynamic collider can act with self-defined movement or physical force.
  */
 export class DynamicCollider extends Collider {
+  private static _tempVector3 = new Vector3();
+  private static _tempQuat = new Quaternion();
+
   private _linearDamping = 0;
   private _angularDamping = 0.05;
   @ignoreClone
@@ -390,7 +393,13 @@ export class DynamicCollider extends Collider {
   override _onLateUpdate(): void {
     const { transform } = this.entity;
     const { worldPosition, worldRotationQuaternion } = transform;
-    (<IDynamicCollider>this._nativeCollider).getWorldTransform(worldPosition, worldRotationQuaternion);
+    const outPosition = DynamicCollider._tempVector3;
+    const outRotation = DynamicCollider._tempQuat;
+    (<IDynamicCollider>this._nativeCollider).getWorldTransform(outPosition, outRotation);
+    if (!Vector3.equals(outPosition, worldPosition) || !Quaternion.equals(outRotation, worldRotationQuaternion)) {
+      worldPosition.copyFrom(outPosition);
+      worldRotationQuaternion.copyFrom(outRotation);
+    }
     this._updateFlag.flag = false;
   }
 

packages/core/src/physics/joint/Joint.ts

@@ -1,5 +1,5 @@
 import { IJoint } from "@galacean/engine-design";
-import { Vector3 } from "@galacean/engine-math";
+import { Matrix, Quaternion, Vector3 } from "@galacean/engine-math";
 import { Component } from "../../Component";
 import { DependentMode, dependentComponents } from "../../ComponentsDependencies";
 import { Entity } from "../../Entity";
@@ -15,6 +15,7 @@ import { DynamicCollider } from "../DynamicCollider";
 @dependentComponents(DynamicCollider, DependentMode.AutoAdd)
 export abstract class Joint extends Component {
   private static _tempVector3 = new Vector3();
+  private static _tempMatrix = new Matrix();
 
   @deepClone
   protected _colliderInfo = new JointColliderInfo();
@@ -40,6 +41,7 @@ export abstract class Joint extends Component {
       value?.entity._updateFlagManager.addListener(this._onConnectedTransformChanged);
       this._connectedColliderInfo.collider = value;
       this._nativeJoint?.setConnectedCollider(value?._nativeCollider);
+      this._updateRotation();
       if (this._automaticConnectedAnchor) {
         this._calculateConnectedAnchor();
       } else {
@@ -196,6 +198,7 @@ export abstract class Joint extends Component {
   override _onEnableInScene(): void {
     this._createJoint();
     this._syncNative();
+    this._updateRotation();
   }
 
   /**
@@ -229,21 +232,20 @@ export abstract class Joint extends Component {
   private _calculateConnectedAnchor(): void {
     const colliderInfo = this._colliderInfo;
     const connectedColliderInfo = this._connectedColliderInfo;
-    const { worldPosition: selfPos } = this.entity.transform;
-    const selfActualAnchor = colliderInfo.actualAnchor;
     const connectedAnchor = connectedColliderInfo.anchor;
     const connectedActualAnchor = connectedColliderInfo.actualAnchor;
     const connectedCollider = connectedColliderInfo.collider;
 
     // @ts-ignore
     connectedAnchor._onValueChanged = null;
     if (connectedCollider) {
-      const { worldPosition: connectedPos, lossyWorldScale: connectedWorldScale } = connectedCollider.entity.transform;
-      Vector3.subtract(selfPos, connectedPos, Joint._tempVector3);
-      Vector3.add(Joint._tempVector3, selfActualAnchor, connectedActualAnchor);
-      Vector3.divide(connectedActualAnchor, connectedWorldScale, connectedAnchor);
+      const tempVector3 = Joint._tempVector3;
+      const tempMatrix = Joint._tempMatrix;
+      Vector3.transformCoordinate(colliderInfo.anchor, this.entity.transform.worldMatrix, tempVector3);
+      tempMatrix.copyFrom(connectedCollider.entity.transform.worldMatrix).invert();
+      Vector3.transformCoordinate(tempVector3, tempMatrix, connectedAnchor);
     } else {
-      Vector3.add(selfPos, selfActualAnchor, connectedActualAnchor);
+      Vector3.transformCoordinate(colliderInfo.anchor, this.entity.transform.worldMatrix, connectedActualAnchor);
       connectedAnchor.copyFrom(connectedActualAnchor);
     }
     // @ts-ignore
@@ -274,6 +276,17 @@ export abstract class Joint extends Component {
     }
   }
 
+  private _updateRotation(): void {
+    const quat = new Quaternion();
+    const connectedColliderInfo = this._connectedColliderInfo;
+    const connectedCollider = connectedColliderInfo.collider;
+    if (connectedCollider) {
+      Quaternion.invert(connectedCollider.entity.transform.worldRotationQuaternion, quat);
+    }
+    Quaternion.multiply(quat, this.entity.transform.worldRotationQuaternion, quat);
+    this._nativeJoint?.setRotation(quat);
+  }
+
   private _updateActualAnchor(flag: AnchorOwner): void {
     if (flag & AnchorOwner.Self) {
       const worldScale = this.entity.transform.lossyWorldScale;

packages/design/src/physics/joints/IJoint.ts

@@ -1,4 +1,4 @@
-import { Vector3 } from "@galacean/engine-math";
+import { Quaternion, Vector3 } from "@galacean/engine-math";
 import { ICollider } from "../ICollider";
 
 /**
@@ -26,6 +26,12 @@ export interface IJoint {
    */
   setConnectedMassScale(value: number): void;
 
+  /**
+   * The rotation of the joint.
+   * @param value The rotation of the joint.
+   */
+  setRotation(value: Quaternion): void;
+
   /**
    * The scale to apply to the inverse mass of collider0 for resolving this constraint.
    */

packages/loader/src/resource-deserialize/resources/parser/HierarchyParser.ts

@@ -35,6 +35,7 @@
     this._organizeEntities = this._organizeEntities.bind(this);
     this._parseComponents = this._parseComponents.bind(this);
     this._parsePrefabModification = this._parsePrefabModification.bind(this);
+    this._parseAddedComponents = this._parseAddedComponents.bind(this);
     this._parsePrefabRemovedEntities = this._parsePrefabRemovedEntities.bind(this);
     this._parsePrefabRemovedComponents = this._parsePrefabRemovedComponents.bind(this);
     this._clearAndResolve = this._clearAndResolve.bind(this);
@@ -51,6 +52,7 @@
       .then(this._organizeEntities)
       .then(this._parseComponents)
       .then(this._parsePrefabModification)
+      .then(this._parseAddedComponents)
       .then(this._parsePrefabRemovedEntities)
       .then(this._parsePrefabRemovedComponents)
       .then(this._clearAndResolve)
@@ -88,19 +90,11 @@
     const promises = [];
     for (let i = 0, l = entitiesConfig.length; i < l; i++) {
       const entityConfig = entitiesConfig[i];
-      const entity = entityMap.get(entityConfig.id);
-      for (let i = 0; i < entityConfig.components.length; i++) {
-        const componentConfig = entityConfig.components[i];
-        const key = !componentConfig.refId ? componentConfig.class : componentConfig.refId;
-        const component = entity.addComponent(Loader.getClass(key));
-        this.context.addComponent(componentConfig.id, component);
-        const promise = this._reflectionParser.parsePropsAndMethods(component, componentConfig);
-        promises.push(promise);
+      if ((entityConfig as IStrippedEntity).strippedId) {
+        continue;
       }
-    }
-
-    for (const waitingList of this.context.componentWaitingMap.values()) {
-      waitingList.forEach((resolve) => resolve(null));
+      const entity = entityMap.get(entityConfig.id);
+      this._addComponents(entity, entityConfig.components, promises);
     }
 
     return Promise.all(promises);
@@ -140,6 +134,25 @@
     return Promise.all(promises);
   }
 
+  private _parseAddedComponents() {
+    const entityMap = this.context.entityMap;
+    const entityConfigMap = this.context.entityConfigMap;
+    const strippedIds = this.context.strippedIds;
+    const promises = [];
+
+    for (let i = 0, n = strippedIds.length; i < n; i++) {
+      const entityConfig = entityConfigMap.get(strippedIds[i]) as IStrippedEntity;
+      const prefabContext = this._prefabContextMap.get(entityMap.get(entityConfig.prefabInstanceId));
+      const entity = prefabContext.entityMap.get(entityConfig.prefabSource.entityId);
+      this._addComponents(entity, entityConfig.components, promises);
+    }
+    for (const waitingList of this.context.componentWaitingMap.values()) {
+      waitingList.forEach((resolve) => resolve(null));
+    }
+
+    return Promise.all(promises);
+  }
+
   private _parsePrefabRemovedEntities() {
     const entitiesConfig = this.data.entities;
     const entityMap = this.context.entityMap;
@@ -286,6 +299,22 @@
     }
   }
 
+  private _addComponents(
+    entity: Entity,
+    components: IEntity["components"],
+    promises: Promise<void>[]
+  ): Promise<void>[] {
+    for (let i = 0, n = components.length; i < n; i++) {
+      const componentConfig = components[i];
+      const key = !componentConfig.refId ? componentConfig.class : componentConfig.refId;
+      const component = entity.addComponent(Loader.getClass(key));
+      this.context.addComponent(componentConfig.id, component);
+      const promise = this._reflectionParser.parsePropsAndMethods(component, componentConfig);
+      promises.push(promise);
+    }
+    return promises;
+  }
+
   private _applyEntityData(entity: Entity, entityConfig: IEntity = {}): Entity {
     entity.isActive = entityConfig.isActive ?? entity.isActive;
     entity.name = entityConfig.name ?? entity.name;

packages/physics-physx/src/joint/PhysXHingeJoint.ts

@@ -10,9 +10,9 @@ import { PhysXJoint } from "./PhysXJoint";
 export class PhysXHingeJoint extends PhysXJoint implements IHingeJoint {
   protected static _xAxis = new Vector3(1, 0, 0);
 
-  private _anchor: Vector3;
-  private _connectedAnchor: Vector3;
+  private _axis: Vector3;
   private _axisRotationQuaternion = new Quaternion();
+  private _connectedAxisRotationQuaternion = new Quaternion();
 
   constructor(physXPhysics: PhysXPhysics, collider: PhysXCollider) {
     super(physXPhysics);
@@ -27,27 +27,38 @@ export class PhysXHingeJoint extends PhysXJoint implements IHingeJoint {
     );
   }
 
+  override setRotation(value: Quaternion): void {
+    this._rotation.copyFrom(value);
+    this._axis && this.setAxis(this._axis);
+  }
+
   /**
    * {@inheritDoc IHingeJoint.setAxis }
    */
   setAxis(value: Vector3): void {
+    this._axis = value;
     const xAxis = PhysXHingeJoint._xAxis;
     const axisRotationQuaternion = this._axisRotationQuaternion;
     xAxis.set(1, 0, 0);
     const angle = Math.acos(Vector3.dot(xAxis, value));
     Vector3.cross(xAxis, value, xAxis);
     Quaternion.rotationAxisAngle(xAxis, angle, axisRotationQuaternion);
     this._setLocalPose(0, this._anchor, axisRotationQuaternion);
-    this._setLocalPose(1, this._connectedAnchor, axisRotationQuaternion);
+    const connectedAxisRotationQuaternion = this._connectedAxisRotationQuaternion;
+    Quaternion.multiply(this._rotation, axisRotationQuaternion, connectedAxisRotationQuaternion);
+    this._setLocalPose(1, this._connectedAnchor, connectedAxisRotationQuaternion);
   }
 
   override setAnchor(value: Vector3): void {
     this._setLocalPose(0, value, this._axisRotationQuaternion);
     this._anchor = value;
   }
 
+  /**
+   * {@inheritDoc IJoint.setConnectedAnchor }
+   */
   override setConnectedAnchor(value: Vector3): void {
-    this._setLocalPose(1, value, this._axisRotationQuaternion);
+    this._setLocalPose(1, value, this._connectedAxisRotationQuaternion);
     this._connectedAnchor = value;
   }
 

packages/physics-physx/src/joint/PhysXJoint.ts

@@ -11,6 +11,9 @@ export class PhysXJoint implements IJoint {
   protected static _defaultQuat = new Quaternion();
 
   protected _pxJoint: any;
+  protected _anchor: Vector3;
+  protected _connectedAnchor: Vector3;
+  protected _rotation: Quaternion = new Quaternion();
   protected _collider: PhysXCollider;
   private _breakForce: number = Number.MAX_VALUE;
   private _breakTorque: number = Number.MAX_VALUE;
@@ -33,13 +36,20 @@ export class PhysXJoint implements IJoint {
    */
   setAnchor(value: Vector3): void {
     this._setLocalPose(0, value, PhysXJoint._defaultQuat);
+    this._anchor = value;
   }
 
   /**
    * {@inheritDoc IJoint.setConnectedAnchor }
    */
   setConnectedAnchor(value: Vector3): void {
-    this._setLocalPose(1, value, PhysXJoint._defaultQuat);
+    this._setLocalPose(1, value, this._rotation);
+    this._connectedAnchor = value;
+  }
+
+  setRotation(value: Quaternion): void {
+    this._rotation.copyFrom(value);
+    this._setLocalPose(1, this._connectedAnchor, value);
   }
 
   /**

tests/src/core/physics/Joint.test.ts

@@ -134,6 +134,37 @@ describe("Joint", function () {
     expect(box.transform.position).deep.include({ x: 4, y: 4, z: 4 });
   });
 
+  it("rotated", function () {
+    const box = addBox(new Vector3(1, 1, 1), DynamicCollider, new Vector3(4, 4, 4));
+    box.transform.rotation = new Vector3(30, 45, 0);
+    box.addComponent(FixedJoint);
+    // @ts-ignore
+    engine.sceneManager.activeScene.physics._update(1);
+    expect(formatValue(box.transform.rotation.x)).eq(30);
+    expect(formatValue(box.transform.rotation.y)).eq(45);
+    expect(formatValue(box.transform.rotation.z)).eq(0);
+  });
+
+  it("rotated with connectedCollider", function () {
+    const box = addBox(new Vector3(1, 1, 1), DynamicCollider, new Vector3(4, 4, 4));
+    const box2 = addBox(new Vector3(1, 1, 1), DynamicCollider, new Vector3(4, 4, 4));
+    const connectedCollider = box2.getComponent(DynamicCollider);
+    box.getComponent(DynamicCollider).useGravity = false;
+    connectedCollider.useGravity = false;
+    box.transform.rotation = new Vector3(30, 45, 0);
+    box2.transform.rotation = new Vector3(20, 45, 50);
+    const joint = box.addComponent(FixedJoint);
+    joint.connectedCollider = connectedCollider;
+    // @ts-ignore
+    engine.sceneManager.activeScene.physics._update(1);
+    expect(formatValue(box.transform.rotation.x)).eq(30);
+    expect(formatValue(box.transform.rotation.y)).eq(45);
+    expect(formatValue(box.transform.rotation.z)).eq(0);
+    expect(formatValue(box2.transform.rotation.x)).eq(20);
+    expect(formatValue(box2.transform.rotation.y)).eq(45);
+    expect(formatValue(box2.transform.rotation.z)).eq(50);
+  });
+
   it("massScale", function () {
     engine.sceneManager.activeScene.physics.gravity = new Vector3(0, -1, 0);
     const box1 = addBox(new Vector3(1, 1, 1), DynamicCollider, new Vector3(0, 5, 0));