From 1219613c980a3f7f43744ea1c2083308680e75a0 Mon Sep 17 00:00:00 2001 From: Cedric Guillemet <1312968+CedricGuillemet@users.noreply.github.com> Date: Wed, 13 Mar 2024 17:07:45 +0100 Subject: [PATCH] ragdoll improvements with animated skeletons (#14817) * ragdoll improvements with animated skeletons * fix animated character explosion * get aggregate * fix offset axis * wip * almost there * removed commented import * do not change orientation twice for root * PR feedback --- packages/dev/core/src/Physics/v2/ragdoll.ts | 153 +++++++++++++------- 1 file changed, 101 insertions(+), 52 deletions(-) diff --git a/packages/dev/core/src/Physics/v2/ragdoll.ts b/packages/dev/core/src/Physics/v2/ragdoll.ts index 18917ad69ea..c4d2ff00b1d 100644 --- a/packages/dev/core/src/Physics/v2/ragdoll.ts +++ b/packages/dev/core/src/Physics/v2/ragdoll.ts @@ -1,12 +1,11 @@ import type { Skeleton } from "../../Bones/skeleton"; -import { Vector3, Matrix, TmpVectors } from "../../Maths/math.vector"; -import { Quaternion } from "../../Maths/math.vector"; +import { Vector3, Matrix, TmpVectors, Quaternion } from "../../Maths/math.vector"; import type { Scene } from "../../scene"; import { PhysicsAggregate } from "./physicsAggregate"; -import { BallAndSocketConstraint } from "./physicsConstraint"; +import { PhysicsConstraint } from "./physicsConstraint"; import type { Mesh } from "../../Meshes/mesh"; import { Axis, Space } from "core/Maths/math.axis"; -import { PhysicsShapeType, PhysicsConstraintType } from "./IPhysicsEnginePlugin"; +import { PhysicsShapeType, PhysicsConstraintType, PhysicsMotionType } from "./IPhysicsEnginePlugin"; import type { Nullable } from "../../types"; import type { Bone } from "../../Bones/bone"; import { Logger } from "../../Misc/logger"; @@ -66,12 +65,14 @@ export class RagdollBoneProperties { export class Ragdoll { private _skeleton: Skeleton; private _scene: Scene; - private _mesh: Mesh; + private _rootTransformNode: Mesh | TransformNode; private _config: any; private _boxConfigs: Array = new Array(); - + private _joints: Array = new Array(); private _bones: Array = new Array(); private _initialRotation: Array = new Array(); + // without mesh transform, to figure out later + private _initialRotation2: Array = new Array(); private _boneNames: string[] = []; private _transforms: Array = new Array(); private _aggregates: Array = new Array(); @@ -91,27 +92,40 @@ export class Ragdoll { private _defaultJointMin: number = -90; private _defaultJointMax: number = 90; - private _boneOffsetAxis: Vector3; - /** * Construct a new Ragdoll object. Once ready, it can be made dynamic by calling `Ragdoll` method * @param skeleton The skeleton containing bones to be physicalized - * @param mesh The mesh used by the skeleton + * @param rootTransformNode The mesh or its transform used by the skeleton * @param config an array of `RagdollBoneProperties` corresponding to bones and their properties used to instanciate physics bodies */ - constructor(skeleton: Skeleton, mesh: Mesh, config: RagdollBoneProperties[]) { + constructor(skeleton: Skeleton, rootTransformNode: Mesh | TransformNode, config: RagdollBoneProperties[]) { this._skeleton = skeleton; this._scene = skeleton.getScene(); - this._mesh = mesh; + this._rootTransformNode = rootTransformNode; this._config = config; // initial, user defined box configs. May have several box configs jammed into 1 index. this._boxConfigs = []; // final box configs. Every element is a separate box config (this.config may have several configs jammed into 1 index). this._putBoxesInBoneCenter = false; this._defaultJoint = PhysicsConstraintType.HINGE; - this._boneOffsetAxis = Axis.Y; + + this._init(); + } + + /** + * Returns the aggregate corresponding to the ragdoll bone index + * @param index ragdoll bone aggregate index + * @returns the aggregate for the bone index for the root aggregate if index is invalid + */ + public getAggregate(index: number): PhysicsAggregate { + if (index < 0 || index >= this._aggregates.length) { + return this._aggregates[this._rootBoneIndex]; + } + return this._aggregates[index]; } private _createColliders(): void { - this._mesh.computeWorldMatrix(); + this._rootTransformNode.computeWorldMatrix(); + this._skeleton.computeAbsoluteMatrices(true); + this._skeleton.prepare(true); const config = this._config; for (let i = 0; i < config.length; i++) { @@ -155,11 +169,11 @@ export class Ragdoll { currentRagdollBoneProperties.boxOffset = boxOffset; // Offset axis. - const boneOffsetAxis = config[i].boneOffsetAxis !== undefined ? config[i].boneOffsetAxis : this._boneOffsetAxis; - const boneDir = currentBone.getDirection(boneOffsetAxis, this._mesh); + const boneOffsetAxis = config[i].boneOffsetAxis !== undefined ? config[i].boneOffsetAxis : Axis.Y; + const boneDir = currentBone.getDirection(boneOffsetAxis, this._rootTransformNode); currentRagdollBoneProperties.boneOffsetAxis = boneOffsetAxis; - transform.position = currentBone.getAbsolutePosition(this._mesh).add(boneDir.scale(boxOffset)); + transform.position = currentBone.getAbsolutePosition(this._rootTransformNode).add(boneDir.scale(boxOffset)); const mass = config[i].mass !== undefined ? config[i].mass : this._mass; const restitution = config[i].restitution !== undefined ? config[i].restitution : this._restitution; @@ -176,18 +190,20 @@ export class Ragdoll { ); aggregate.body.setCollisionCallbackEnabled(true); aggregate.body.disablePreStep = false; + aggregate.body.setMotionType(PhysicsMotionType.ANIMATED); this._aggregates.push(aggregate); this._bones.push(currentBone); this._boneNames.push(currentBone.name); this._transforms.push(transform); this._boxConfigs.push(currentRagdollBoneProperties); - this._initialRotation.push(currentBone.getRotationQuaternion(Space.WORLD)); + this._initialRotation.push(currentBone.getRotationQuaternion(Space.WORLD, this._rootTransformNode)); + this._initialRotation2.push(currentBone.getRotationQuaternion(Space.WORLD)); } } } private _initJoints(): void { - this._mesh.computeWorldMatrix(); + this._rootTransformNode.computeWorldMatrix(); for (let i = 0; i < this._bones.length; i++) { // The root bone has no joints. if (i == this._rootBoneIndex) continue; @@ -201,65 +217,95 @@ export class Ragdoll { const boneParentIndex = this._boneNames.indexOf(nearestParent.name); - let distanceFromParentBoxToBone = this._bones[i].getAbsolutePosition(this._mesh).subtract(this._transforms[boneParentIndex].position); + let distanceFromParentBoxToBone = this._bones[i].getAbsolutePosition(this._rootTransformNode).subtract(this._transforms[boneParentIndex].position); const wmat = this._transforms[boneParentIndex].computeWorldMatrix(); const invertedWorldMat = Matrix.Invert(wmat); - distanceFromParentBoxToBone = Vector3.TransformCoordinates(this._bones[i].getAbsolutePosition(this._mesh), invertedWorldMat); + distanceFromParentBoxToBone = Vector3.TransformCoordinates(this._bones[i].getAbsolutePosition(this._rootTransformNode), invertedWorldMat); - const boneAbsPos = this._bones[i].getAbsolutePosition(this._mesh); + const boneAbsPos = this._bones[i].getAbsolutePosition(this._rootTransformNode); const boxAbsPos = this._transforms[i].position.clone(); const myConnectedPivot = boneAbsPos.subtract(boxAbsPos); - const joint = new BallAndSocketConstraint( - distanceFromParentBoxToBone, - myConnectedPivot, - this._boxConfigs[i].rotationAxis!, - this._boxConfigs[i].rotationAxis!, + const joint = new PhysicsConstraint( + PhysicsConstraintType.BALL_AND_SOCKET, + { + pivotA: distanceFromParentBoxToBone, + pivotB: myConnectedPivot, + axisA: this._boxConfigs[i].rotationAxis!, + axisB: this._boxConfigs[i].rotationAxis!, + collision: false, + }, this._scene ); + this._aggregates[boneParentIndex].body.addConstraint(this._aggregates[i].body, joint); + joint.isEnabled = false; + this._joints.push(joint); + } + } + + // set physics body orientation/position from bones + private _syncBonesToPhysics(): void { + const rootMatrix = this._rootTransformNode.getWorldMatrix(); + for (let i = 0; i < this._bones.length; i++) { + // position + const transform = this._aggregates[i].transformNode; + const rootPos = this._bones[i].getAbsolutePosition(); + Vector3.TransformCoordinatesToRef(rootPos, rootMatrix, transform.position); + + // added offset + this._bones[i].getDirectionToRef(this._boxConfigs[i].boneOffsetAxis!, this._rootTransformNode, TmpVectors.Vector3[0]); + TmpVectors.Vector3[0].scaleInPlace(this._boxConfigs[i].boxOffset ?? 0); + transform.position.addInPlace(TmpVectors.Vector3[0]); + + this._setBoneOrientationToBody(i); } } + private _setBoneOrientationToBody(boneIndex: number): void { + const transform = this._aggregates[boneIndex].transformNode; + const bone = this._bones[boneIndex]; + this._initialRotation[boneIndex].conjugateToRef(TmpVectors.Quaternion[0]); + bone.getRotationQuaternionToRef(Space.WORLD, this._rootTransformNode, TmpVectors.Quaternion[1]); + TmpVectors.Quaternion[1].multiplyToRef(TmpVectors.Quaternion[0], transform.rotationQuaternion!); + transform.rotationQuaternion!.normalize(); + } + private _syncBonesAndBoxes(): void { if (this.pauseSync) { return; } if (this._ragdollMode) { - this._bones[this._rootBoneIndex].getDirectionToRef(this._boxConfigs[this._rootBoneIndex].boneOffsetAxis!, this._mesh, TmpVectors.Vector3[0]); - TmpVectors.Vector3[0].scaleInPlace(this._boxConfigs[this._rootBoneIndex].boxOffset!); - this._bones[this._rootBoneIndex].getAbsolutePositionToRef(this._mesh, TmpVectors.Vector3[1]); - TmpVectors.Vector3[1].addInPlace(TmpVectors.Vector3[0]); - - this._bones[this._rootBoneIndex].setAbsolutePosition(this._transforms[this._rootBoneIndex].position, this._mesh); - this._addImpostorRotationToBone(this._rootBoneIndex); + this._setBodyOrientationToBone(this._rootBoneIndex); const rootPos = this._aggregates[this._rootBoneIndex].body.transformNode.position; + this._rootTransformNode.getWorldMatrix().invertToRef(TmpVectors.Matrix[0]); - // Move the mesh, to guarantee alignment between root bone and impostor box position - TmpVectors.Vector3[1].subtractToRef(rootPos, TmpVectors.Vector3[0]); - this._mesh.position.subtractToRef(TmpVectors.Vector3[0], this._mesh.position); + Vector3.TransformCoordinatesToRef(rootPos, TmpVectors.Matrix[0], TmpVectors.Vector3[0]); + this._bones[this._rootBoneIndex].setAbsolutePosition(TmpVectors.Vector3[0]); for (let i = 0; i < this._bones.length; i++) { if (i == this._rootBoneIndex) continue; - this._addImpostorRotationToBone(i); + this._setBodyOrientationToBone(i); } + } else { + this._syncBonesToPhysics(); } } - private _addImpostorRotationToBone(boneIndex: number): void { - const qmesh = this._mesh.rotationQuaternion ?? Quaternion.FromEulerAngles(this._mesh.rotation.x, this._mesh.rotation.y, this._mesh.rotation.z); - const qbind = this._initialRotation[boneIndex]; + private _setBodyOrientationToBone(boneIndex: number): void { + const qmesh = + this._rootTransformNode.rotationQuaternion ?? + Quaternion.FromEulerAngles(this._rootTransformNode.rotation.x, this._rootTransformNode.rotation.y, this._rootTransformNode.rotation.z); + const qbind = this._initialRotation2[boneIndex]; const qphys = this._aggregates[boneIndex].body?.transformNode?.rotationQuaternion!; - // TmpVectors.Quaternion[1] = mesh.rotation * this._initialRotation[boneIndex] - // TmpVectors.Quaternion[0] = body.rotation * TmpVectors.Quaternion[1] qmesh.multiplyToRef(qbind, TmpVectors.Quaternion[1]); qphys.multiplyToRef(TmpVectors.Quaternion[1], TmpVectors.Quaternion[0]); - this._bones[boneIndex].setRotationQuaternion(TmpVectors.Quaternion[0], Space.WORLD, this._mesh); + this._bones[boneIndex].setRotationQuaternion(TmpVectors.Quaternion[0], Space.WORLD, this._rootTransformNode); } // Return true if root bone is valid/exists in this.bonesNames. false otherwise. @@ -294,14 +340,9 @@ export class Ragdoll { } private _init() { - // detach bones with link transform to let physics have control - this._skeleton.bones.forEach((bone) => { - bone.linkTransformNode(null); - }); - this._createColliders(); - // If this.defineRootBone() returns false... there is not root bone. + // If this.defineRootBone() returns ... there is not root bone. if (!this._defineRootBone()) { return; } @@ -310,15 +351,23 @@ export class Ragdoll { this._scene.registerBeforeRender(() => { this._syncBonesAndBoxes(); }); + this._syncBonesToPhysics(); } /** * Enable ragdoll mode. Create physics objects and make them dynamic. */ public ragdoll(): void { - if (!this._ragdollMode) { - this._ragdollMode = true; - this._init(); + this._ragdollMode = true; + // detach bones with link transform to let physics have control + this._skeleton.bones.forEach((bone) => { + bone.linkTransformNode(null); + }); + for (let i = 0; i < this._joints.length; i++) { + this._joints[i].isEnabled = true; + } + for (let i = 0; i < this._aggregates.length; i++) { + this._aggregates[i].body.setMotionType(PhysicsMotionType.DYNAMIC); } }