From 0070d14d2868e470d611e941b3015329bd97cd58 Mon Sep 17 00:00:00 2001 From: luzhuang <364439895@qq.com> Date: Fri, 24 Jan 2025 11:06:26 +0800 Subject: [PATCH] Fix rotation error when add joint (#2526) * fix: error when physicsMaterial used in prefab * fix: rotation error when add joint --- packages/core/src/physics/DynamicCollider.ts | 14 ++++- packages/core/src/physics/joint/Joint.ts | 29 +++++++--- packages/design/src/physics/joints/IJoint.ts | 8 ++- .../resources/parser/HierarchyParser.ts | 53 ++++++++++++++----- .../src/joint/PhysXHingeJoint.ts | 19 +++++-- .../physics-physx/src/joint/PhysXJoint.ts | 12 ++++- tests/src/core/physics/Joint.test.ts | 31 +++++++++++ 7 files changed, 139 insertions(+), 27 deletions(-) diff --git a/packages/core/src/physics/DynamicCollider.ts b/packages/core/src/physics/DynamicCollider.ts index 83f6db4b46..d323b25209 100644 --- a/packages/core/src/physics/DynamicCollider.ts +++ b/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,16 @@ export class DynamicCollider extends Collider { override _onLateUpdate(): void { const { transform } = this.entity; const { worldPosition, worldRotationQuaternion } = transform; - (this._nativeCollider).getWorldTransform(worldPosition, worldRotationQuaternion); + const outPosition = DynamicCollider._tempVector3; + const outRotation = DynamicCollider._tempQuat; + (this._nativeCollider).getWorldTransform(outPosition, outRotation); + // To resolve the issue where onValueChanged is triggered even though the values are equal. + if (!Vector3.equals(outPosition, worldPosition)) { + worldPosition.copyFrom(outPosition); + } + if (!Quaternion.equals(outRotation, worldRotationQuaternion)) { + worldRotationQuaternion.copyFrom(outRotation); + } this._updateFlag.flag = false; } diff --git a/packages/core/src/physics/joint/Joint.ts b/packages/core/src/physics/joint/Joint.ts index ae7d90b866..0adde0095e 100644 --- a/packages/core/src/physics/joint/Joint.ts +++ b/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,8 +232,6 @@ 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; @@ -238,12 +239,13 @@ export abstract class Joint extends Component { // @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; diff --git a/packages/design/src/physics/joints/IJoint.ts b/packages/design/src/physics/joints/IJoint.ts index 45fac72c1b..094249e06b 100644 --- a/packages/design/src/physics/joints/IJoint.ts +++ b/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. */ diff --git a/packages/loader/src/resource-deserialize/resources/parser/HierarchyParser.ts b/packages/loader/src/resource-deserialize/resources/parser/HierarchyParser.ts index ba6263bc01..08f2001d11 100644 --- a/packages/loader/src/resource-deserialize/resources/parser/HierarchyParser.ts +++ b/packages/loader/src/resource-deserialize/resources/parser/HierarchyParser.ts @@ -35,6 +35,7 @@ export abstract class HierarchyParser resolve(null)); + const entity = entityMap.get(entityConfig.id); + this._addComponents(entity, entityConfig.components, promises); } return Promise.all(promises); @@ -140,6 +134,25 @@ export abstract class HierarchyParser resolve(null)); + } + + return Promise.all(promises); + } + private _parsePrefabRemovedEntities() { const entitiesConfig = this.data.entities; const entityMap = this.context.entityMap; @@ -286,6 +299,22 @@ export abstract class HierarchyParser[] + ): Promise[] { + 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; diff --git a/packages/physics-physx/src/joint/PhysXHingeJoint.ts b/packages/physics-physx/src/joint/PhysXHingeJoint.ts index 77f71edc8a..3efe36473b 100644 --- a/packages/physics-physx/src/joint/PhysXHingeJoint.ts +++ b/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,10 +27,16 @@ 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); @@ -38,7 +44,9 @@ export class PhysXHingeJoint extends PhysXJoint implements IHingeJoint { 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 { @@ -46,8 +54,11 @@ export class PhysXHingeJoint extends PhysXJoint implements IHingeJoint { 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; } diff --git a/packages/physics-physx/src/joint/PhysXJoint.ts b/packages/physics-physx/src/joint/PhysXJoint.ts index 59783dc7d8..1faf8f4e1b 100644 --- a/packages/physics-physx/src/joint/PhysXJoint.ts +++ b/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); } /** diff --git a/tests/src/core/physics/Joint.test.ts b/tests/src/core/physics/Joint.test.ts index e6746fec91..bfde78a127 100644 --- a/tests/src/core/physics/Joint.test.ts +++ b/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));