Physics Constraints debug viewer (#15960)

* axis limits * More understandable gizmos * refresh WIP * Linear constraint cage * spring line added to update list * removed distance infos * ragdoll update * min max values * build * ragdoll limits * fix imports
BabylonJS · Feb 3, 2025 · 9133a15 · 9133a15
1 parent 138baab
commit 9133a15
Show file tree

Hide file tree

Showing 2 changed files with 161 additions and 20 deletions.
diff --git a/packages/dev/core/src/Debug/physicsViewer.ts b/packages/dev/core/src/Debug/physicsViewer.ts
@@ -10,7 +10,7 @@ import type { Material } from "../Materials/material";
 import { EngineStore } from "../Engines/engineStore";
 import { StandardMaterial } from "../Materials/standardMaterial";
 import type { IPhysicsEnginePlugin as IPhysicsEnginePluginV1 } from "../Physics/v1/IPhysicsEnginePlugin";
-import type { IPhysicsEnginePluginV2, PhysicsMassProperties } from "../Physics/v2/IPhysicsEnginePlugin";
+import { PhysicsConstraintAxis, PhysicsConstraintAxisLimitMode, type IPhysicsEnginePluginV2, type PhysicsMassProperties } from "../Physics/v2/IPhysicsEnginePlugin";
 import { PhysicsImpostor } from "../Physics/v1/physicsImpostor";
 import { UtilityLayerRenderer } from "../Rendering/utilityLayerRenderer";
 import { CreateCylinder } from "../Meshes/Builders/cylinderBuilder";
@@ -44,7 +44,7 @@ export class PhysicsViewer {
     /** @internal */
     protected _inertiaMeshes: Array<Nullable<AbstractMesh>> = [];
     /** @internal */
-    protected _constraintMeshes: Array<Nullable<AbstractMesh>> = [];
+    protected _constraintMeshes: Array<Nullable<Array<AbstractMesh>>> = [];
     /** @internal */
     protected _scene: Nullable<Scene>;
     /** @internal */
@@ -71,12 +71,14 @@ export class PhysicsViewer {
     private _debugMeshMeshes = new Array<Mesh>();
 
     private _constraintAxesSize = 0.4;
+    private _constraintAngularSize = 0.4;
 
     /**
      * Creates a new PhysicsViewer
      * @param scene defines the hosting scene
+     * @param size Physics V2 size scalar
      */
-    constructor(scene?: Scene) {
+    constructor(scene?: Scene, size?: number) {
         this._scene = scene || EngineStore.LastCreatedScene;
         if (!this._scene) {
             return;
@@ -90,6 +92,10 @@ export class PhysicsViewer {
         this._utilityLayer = new UtilityLayerRenderer(this._scene, false);
         this._utilityLayer.pickUtilitySceneFirst = false;
         this._utilityLayer.utilityLayerScene.autoClearDepthAndStencil = true;
+        if (size) {
+            this._constraintAxesSize = 0.4 * size;
+            this._constraintAngularSize = 0.4 * size;
+        }
     }
 
     /**
@@ -211,7 +217,7 @@ export class PhysicsViewer {
             const constraint = this._constraints[i];
             const mesh = this._constraintMeshes[i];
             if (constraint && mesh) {
-                this._updateDebugConstraint(constraint, mesh);
+                this._updateDebugConstraint(constraint, mesh[0]);
             }
         }
     }
@@ -413,7 +419,7 @@ export class PhysicsViewer {
             this._numConstraints++;
         }
 
-        return debugMesh;
+        return debugMesh ? debugMesh[0] : null;
     }
 
     /**
@@ -571,14 +577,16 @@ export class PhysicsViewer {
 
         for (let i = 0; i < this._numConstraints; i++) {
             if (this._constraints[i] === constraint) {
-                const mesh = this._constraintMeshes[i];
+                const meshes = this._constraintMeshes[i];
 
-                if (!mesh) {
+                if (!meshes) {
                     continue;
                 }
 
-                utilityLayerScene.removeMesh(mesh);
-                mesh.dispose();
+                meshes.forEach((mesh) => {
+                    utilityLayerScene.removeMesh(mesh);
+                    mesh.dispose();
+                });
 
                 this._constraints.splice(i, 1);
                 this._constraintMeshes.splice(i, 1);
@@ -626,6 +634,13 @@ export class PhysicsViewer {
         return this._debugInertiaMaterial;
     }
 
+    private _getDebugAxisColoredMaterial(axisNumber: number, scene: Scene): Material {
+        const material = new StandardMaterial("", scene);
+        material.emissiveColor = axisNumber == 0 ? Color3.Red() : axisNumber == 1 ? Color3.Green() : Color3.Blue();
+        material.disableLighting = true;
+        return material;
+    }
+
     private _getDebugBoxMesh(scene: Scene): AbstractMesh {
         if (!this._debugBoxMesh) {
             this._debugBoxMesh = CreateBox("physicsBodyBoxViewMesh", { size: 1 }, scene);
@@ -873,7 +888,54 @@ export class PhysicsViewer {
         }
     }
 
-    private _getDebugConstraintMesh(constraint: PhysicsConstraint): Nullable<AbstractMesh> {
+    private _createAngularConstraintMesh(minLimit: number, maxLimit: number, axisNumber: number, parent: TransformNode, scene: Scene): AbstractMesh {
+        const arcAngle = (maxLimit - minLimit) / (Math.PI * 2);
+        const mesh = MeshBuilder.CreateCylinder("ConstraintCylinder", { height: 0.0001, diameter: 3 * this._constraintAngularSize, arc: arcAngle }, scene);
+        mesh.material = this._getDebugAxisColoredMaterial(axisNumber, scene);
+        mesh.parent = parent;
+        const parentScaling = parent.absoluteScaling;
+        switch (axisNumber) {
+            case 0:
+                mesh.rotation.z = Math.PI * 0.5;
+                mesh.rotation.x = -minLimit + Math.PI * 0.5;
+                // scaling on y,z
+                mesh.scaling.x = 1 / parentScaling.x;
+                mesh.scaling.y = 1 / parentScaling.z;
+                mesh.scaling.z = 1 / parentScaling.y;
+                break;
+            case 1:
+                mesh.rotation.y = Math.PI * 1.5 + minLimit;
+                // flip x,z
+                mesh.scaling.x = 1 / parentScaling.z;
+                mesh.scaling.y = 1 / parentScaling.y;
+                mesh.scaling.z = 1 / parentScaling.x;
+                break;
+            case 2:
+                mesh.rotation.x = Math.PI * 0.5;
+                // flip z,y
+                mesh.scaling.x = 1 / parentScaling.x;
+                mesh.scaling.y = 1 / parentScaling.z;
+                mesh.scaling.z = 1 / parentScaling.y;
+                break;
+        }
+        return mesh;
+    }
+
+    private _createCage(parent: TransformNode, scene: Scene): AbstractMesh {
+        const cage = MeshBuilder.CreateBox("cage", { size: 1 }, scene);
+        cage.setPivotPoint(new Vector3(-0.5, -0.5, -0.5));
+        const transparentMaterial = new StandardMaterial("cage_material", scene);
+        transparentMaterial.alpha = 0; // Fully transparent
+        cage.material = transparentMaterial;
+
+        cage.enableEdgesRendering();
+        cage.edgesWidth = 4.0;
+        cage.edgesColor = new Color4(1, 1, 1, 1);
+        cage.parent = parent;
+        return cage;
+    }
+
+    private _getDebugConstraintMesh(constraint: PhysicsConstraint): Nullable<Array<AbstractMesh>> {
         if (!this._utilityLayer) {
             return null;
         }
@@ -896,6 +958,9 @@ export class PhysicsViewer {
         // First, get a reference to all physic bodies that are using this constraint
         const bodiesUsingConstraint = constraint.getBodiesUsingConstraint();
 
+        const parentedConstraintMeshes = [];
+        parentedConstraintMeshes.push(parentingMesh);
+
         for (const bodyPairInfo of bodiesUsingConstraint) {
             // Create a mesh to keep the pair of constraint axes
             const parentOfPair = new TransformNode("parentOfPair", utilityLayerScene);
@@ -949,9 +1014,76 @@ export class PhysicsViewer {
             childAxes.xAxis.parent = childTransformNode;
             childAxes.yAxis.parent = childTransformNode;
             childAxes.zAxis.parent = childTransformNode;
+
+            // constrain vis
+            const engine = this._physicsEnginePlugin! as IPhysicsEnginePluginV2;
+
+            const constraintAxisAngular = [PhysicsConstraintAxis.ANGULAR_X, PhysicsConstraintAxis.ANGULAR_Y, PhysicsConstraintAxis.ANGULAR_Z];
+            const constraintAxisLinear = [PhysicsConstraintAxis.LINEAR_X, PhysicsConstraintAxis.LINEAR_Y, PhysicsConstraintAxis.LINEAR_Z];
+            const constraintAxis = [constraintAxisAngular, constraintAxisLinear];
+
+            // count axis. Angular and Linear
+            const lockCount = [0, 0];
+            for (let angularLinear = 0; angularLinear < 2; angularLinear++) {
+                for (let axis = 0; axis < 3; axis++) {
+                    const constraintAxisValue = constraintAxis[angularLinear][axis];
+                    const axisMode = engine.getAxisMode(constraint, constraintAxisValue);
+                    if (axisMode == PhysicsConstraintAxisLimitMode.LOCKED) {
+                        lockCount[angularLinear]++;
+                    }
+                }
+            }
+
+            // Any free/limited Linear axis
+            if (lockCount[1] != 3) {
+                const cage = this._createCage(parentTransformNode, utilityLayerScene);
+
+                const min = TmpVectors.Vector3[0];
+                const max = TmpVectors.Vector3[1];
+                const limited = [false, false, false];
+
+                limited[0] = engine.getAxisMode(constraint, PhysicsConstraintAxis.LINEAR_X) == PhysicsConstraintAxisLimitMode.LIMITED;
+                limited[1] = engine.getAxisMode(constraint, PhysicsConstraintAxis.LINEAR_Y) == PhysicsConstraintAxisLimitMode.LIMITED;
+                limited[2] = engine.getAxisMode(constraint, PhysicsConstraintAxis.LINEAR_Z) == PhysicsConstraintAxisLimitMode.LIMITED;
+
+                min.x = limited[0] ? engine.getAxisMinLimit(constraint, PhysicsConstraintAxis.LINEAR_X)! : 0;
+                max.x = limited[0] ? engine.getAxisMaxLimit(constraint, PhysicsConstraintAxis.LINEAR_X)! : 0;
+                min.y = limited[1] ? engine.getAxisMinLimit(constraint, PhysicsConstraintAxis.LINEAR_Y)! : 0;
+                max.y = limited[1] ? engine.getAxisMaxLimit(constraint, PhysicsConstraintAxis.LINEAR_Y)! : 0;
+                min.z = limited[2] ? engine.getAxisMinLimit(constraint, PhysicsConstraintAxis.LINEAR_Z)! : 0;
+                max.z = limited[2] ? engine.getAxisMaxLimit(constraint, PhysicsConstraintAxis.LINEAR_Z)! : 0;
+
+                cage.position.x = min.x + 0.5;
+                cage.position.y = min.y + 0.5;
+                cage.position.z = min.z + 0.5;
+
+                cage.scaling.x = max.x - min.x + Epsilon;
+                cage.scaling.y = max.y - min.y + Epsilon;
+                cage.scaling.z = max.z - min.z + Epsilon;
+                parentedConstraintMeshes.push(cage);
+            }
+
+            // Angular
+            if (lockCount[0] != 3) {
+                for (let axisIndex = 0; axisIndex < 3; axisIndex++) {
+                    const axis = constraintAxisAngular[axisIndex];
+                    const axisMode = engine.getAxisMode(constraint, axis);
+                    let minLimit = 0;
+                    let maxLimit = Math.PI * 2;
+                    if (axisMode == PhysicsConstraintAxisLimitMode.LIMITED) {
+                        minLimit = engine.getAxisMinLimit(constraint, axis)!;
+                        maxLimit = engine.getAxisMaxLimit(constraint, axis)!;
+                    }
+                    if (axisMode != PhysicsConstraintAxisLimitMode.LOCKED && constraint.options.pivotB) {
+                        const mesh = this._createAngularConstraintMesh(minLimit, maxLimit, axisIndex, childBody.transformNode, utilityLayerScene);
+                        mesh.position.copyFrom(constraint.options.pivotB);
+                        parentedConstraintMeshes.push(mesh);
+                    }
+                }
+            }
         }
 
-        return parentingMesh;
+        return parentedConstraintMeshes;
     }
 
     /**

diff --git a/packages/dev/core/src/Physics/v2/ragdoll.ts b/packages/dev/core/src/Physics/v2/ragdoll.ts
@@ -35,7 +35,7 @@ export class RagdollBoneProperties {
     /**
      * Type of Physics Constraint used between bones
      */
-    joint?: number | undefined;
+    joint?: number | undefined | PhysicsConstraintType;
     /**
      * Main rotation axis used by the constraint, in local space
      */
@@ -68,7 +68,7 @@ export class Ragdoll {
     private _rootTransformNode: Mesh | TransformNode;
     private _config: any;
     private _boxConfigs: Array<RagdollBoneProperties> = new Array<RagdollBoneProperties>();
-    private _joints: Array<PhysicsConstraint> = new Array<PhysicsConstraint>();
+    private _constraints: Array<PhysicsConstraint> = new Array<PhysicsConstraint>();
     private _bones: Array<Bone> = new Array<Bone>();
     private _initialRotation: Array<Quaternion> = new Array<Quaternion>();
     // without mesh transform, to figure out later
@@ -110,6 +110,14 @@ export class Ragdoll {
         this._init();
     }
 
+    /**
+     * returns an array of created constraints
+     * @returns array of created constraints
+     */
+    public getConstraints(): Array<PhysicsConstraint> {
+        return this._constraints;
+    }
+
     /**
      * Returns the aggregate corresponding to the ragdoll bone index
      * @param index ragdoll bone aggregate index
@@ -227,8 +235,9 @@ export class Ragdoll {
             const boxAbsPos = this._transforms[i].position.clone();
             const myConnectedPivot = boneAbsPos.subtract(boxAbsPos);
 
-            const joint = new PhysicsConstraint(
-                PhysicsConstraintType.BALL_AND_SOCKET,
+            const constraintType = this._boxConfigs[i].joint ?? this._defaultJoint;
+            const constraint = new PhysicsConstraint(
+                constraintType,
                 {
                     pivotA: distanceFromParentBoxToBone,
                     pivotB: myConnectedPivot,
@@ -239,9 +248,9 @@ export class Ragdoll {
                 this._scene
             );
 
-            this._aggregates[boneParentIndex].body.addConstraint(this._aggregates[i].body, joint);
-            joint.isEnabled = false;
-            this._joints.push(joint);
+            this._aggregates[boneParentIndex].body.addConstraint(this._aggregates[i].body, constraint);
+            constraint.isEnabled = false;
+            this._constraints.push(constraint);
         }
     }
 
@@ -363,8 +372,8 @@ export class Ragdoll {
         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._constraints.length; i++) {
+            this._constraints[i].isEnabled = true;
         }
         for (let i = 0; i < this._aggregates.length; i++) {
             this._aggregates[i].body.setMotionType(PhysicsMotionType.DYNAMIC);