Physics V2 plugin iteration

packages/dev/core/src/Debug/physicsViewer.ts

@@ -17,6 +17,8 @@ import { CreateCylinder } from "../Meshes/Builders/cylinderBuilder";
 import type { ICreateCapsuleOptions } from "../Meshes/Builders/capsuleBuilder";
 import { CreateCapsule } from "../Meshes/Builders/capsuleBuilder";
 import { Logger } from "../Misc/logger";
+import type { PhysicsBody } from "../Physics/v2/physicsBody";
+import { VertexData } from "../Meshes/mesh.vertexData";
 
 /**
  * Used to show the physics impostor around the specific mesh
@@ -27,10 +29,16 @@ export class PhysicsViewer {
     /** @internal */
     protected _meshes: Array<Nullable<AbstractMesh>> = [];
     /** @internal */
+    protected _bodies: Array<Nullable<PhysicsBody>> = [];
+    /** @internal */
+    protected _bodyMeshes: Array<Nullable<AbstractMesh>> = [];
+    /** @internal */
     protected _scene: Nullable<Scene>;
     /** @internal */
     protected _numMeshes = 0;
     /** @internal */
+    protected _numBodies = 0;
+    /** @internal */
     protected _physicsEnginePlugin: IPhysicsEnginePluginV1 | IPhysicsEnginePluginV2 | null;
     private _renderFunction: () => void;
     private _utilityLayer: Nullable<UtilityLayerRenderer>;
@@ -122,6 +130,36 @@ export class PhysicsViewer {
         return debugMesh;
     }
 
+    /**
+     *
+     */
+    public showBody(body: PhysicsBody): Nullable<AbstractMesh> {
+        if (!this._scene) {
+            return null;
+        }
+
+        for (let i = 0; i < this._numBodies; i++) {
+            if (this._bodies[i] == body) {
+                return null;
+            }
+        }
+
+        const debugMesh = this._getDebugBodyMesh(body);
+
+        if (debugMesh) {
+            this._bodies[this._numBodies] = body;
+            this._bodyMeshes[this._numBodies] = debugMesh;
+
+            if (this._numBodies === 0) {
+                this._renderFunction = this._updateDebugMeshes.bind(this);
+                this._scene.registerBeforeRender(this._renderFunction);
+            }
+
+            this._numBodies++;
+        }
+
+        return debugMesh;
+    }
     /**
      * Hides a specified physic impostor
      * @param impostor defines the impostor to hide
@@ -331,6 +369,23 @@ export class PhysicsViewer {
         return mesh;
     }
 
+    private _getDebugBodyMesh(body: PhysicsBody): Nullable<AbstractMesh> {
+        if (!this._utilityLayer) {
+            return null;
+        }
+
+        const utilityLayerScene = this._utilityLayer.utilityLayerScene;
+
+        const mesh = new Mesh("custom", utilityLayerScene);
+        const vertexData = new VertexData();
+        const geometry = body.getGeometry() as any;
+        vertexData.positions = geometry.positions;
+        vertexData.indices = geometry.indices;
+        vertexData.applyToMesh(mesh);
+        mesh.material = this._getDebugMaterial(utilityLayerScene);
+        return mesh;
+    }
+
     /** Releases all resources */
     public dispose() {
         const count = this._numMeshes;

packages/dev/core/src/Physics/v2/IPhysicsEnginePlugin.ts

@@ -7,6 +7,7 @@ import type { PhysicsConstraint } from "./physicsConstraint";
 import type { BoundingBox } from "../../Culling/boundingBox";
 import type { TransformNode } from "../../Meshes/transformNode";
 import type { PhysicsMaterial } from "./physicsMaterial";
+import type { Mesh } from "../../Meshes/mesh";
 
 /** @internal */
 export enum ConstraintAxisLimitMode {
@@ -45,6 +46,7 @@ export enum ShapeType {
     CONVEX_HULL,
     CONTAINER,
     MESH,
+    HEIGHTFIELD,
 }
 
 /** @internal */
@@ -109,11 +111,12 @@ export interface IPhysicsEnginePluginV2 {
     setGravity(gravity: Vector3): void;
     setTimeStep(timeStep: number): void;
     getTimeStep(): number;
-    executeStep(delta: number): void; //not forgetting pre and post events
+    executeStep(delta: number, bodies: Array<PhysicsBody>): void; //not forgetting pre and post events
     getPluginVersion(): number;
 
     // body
-    initBody(body: PhysicsBody): void;
+    initBody(body: PhysicsBody, position: Vector3, orientation: Quaternion): void;
+    initBodyInstances(body: PhysicsBody, mesh: Mesh): void;
     setShape(body: PhysicsBody, shape: PhysicsShape): void;
     getShape(body: PhysicsBody): PhysicsShape;
     setFilterGroup(body: PhysicsBody, group: number): void;
@@ -131,6 +134,7 @@ export interface IPhysicsEnginePluginV2 {
     applyImpulse(body: PhysicsBody, location: Vector3, impulse: Vector3): void;
     setAngularVelocity(body: PhysicsBody, angVel: Vector3): void;
     getAngularVelocityToRef(body: PhysicsBody, angVel: Vector3): void;
+    getBodyGeometry(body: PhysicsBody): {};
     disposeBody(body: PhysicsBody): void;
 
     // shape

packages/dev/core/src/Physics/v2/physicsBody.ts

@@ -1,7 +1,10 @@
 import type { IPhysicsEnginePluginV2, MassProperties } from "./IPhysicsEnginePlugin";
 import type { PhysicsShape } from "./physicsShape";
 import type { Vector3 } from "../../Maths/math.vector";
+import { Quaternion } from "../../Maths/math.vector";
 import type { Scene } from "../../scene";
+import type { PhysicsEngine } from "./physicsEngine";
+import type { Mesh, TransformNode } from "../../Meshes";
 
 /**
  *
@@ -10,19 +13,25 @@ import type { Scene } from "../../scene";
 export class PhysicsBody {
     /** @internal */
     public _pluginData: any = undefined;
-
+    /**
+     *
+     */
+    public _pluginDataInstances: Array<any> = [];
     private _physicsPlugin: IPhysicsEnginePluginV2;
-
+    /**
+     *
+     */
+    node: TransformNode;
     /**
      *
      * @param scene
      * @returns
      */
-    constructor(scene: Scene) {
+    constructor(node: TransformNode, scene: Scene) {
         if (!scene) {
             return;
         }
-        const physicsEngine = scene.getPhysicsEngine();
+        const physicsEngine = scene.getPhysicsEngine() as PhysicsEngine;
         if (!physicsEngine) {
             throw new Error("No Physics Engine available.");
         }
@@ -35,7 +44,19 @@ export class PhysicsBody {
         }
 
         this._physicsPlugin = physicsPlugin as IPhysicsEnginePluginV2;
-        this._physicsPlugin.initBody(this);
+        if (!node.rotationQuaternion) {
+            node.rotationQuaternion = Quaternion.FromEulerAngles(node.rotation.x, node.rotation.y, node.rotation.z);
+        }
+        // instances?
+        const m = node as Mesh;
+        if (m.hasThinInstances) {
+            this._physicsPlugin.initBodyInstances(this, m);
+        } else {
+            // single instance
+            this._physicsPlugin.initBody(this, node.position, node.rotationQuaternion);
+        }
+        this.node = node;
+        physicsEngine.addBody(this);
     }
     /**
      *
@@ -174,6 +195,10 @@ export class PhysicsBody {
         this._physicsPlugin.applyImpulse(this, location, impulse);
     }
 
+    public getGeometry(): {} {
+        return this._physicsPlugin.getBodyGeometry(this);
+    }
+
     /**
      *
      */

packages/dev/core/src/Physics/v2/physicsEngine.ts

@@ -4,6 +4,7 @@ import type { IPhysicsEngine } from "../IPhysicsEngine";
 import type { IPhysicsEnginePluginV2 } from "./IPhysicsEnginePlugin";
 import { PhysicsRaycastResult } from "../physicsRaycastResult";
 import { _WarnImport } from "../../Misc/devTools";
+import type { PhysicsBody } from "./physicsBody";
 
 /**
  * Class used to control physics engine
@@ -12,13 +13,7 @@ import { _WarnImport } from "../../Misc/devTools";
 /** @internal */
 export class PhysicsEngine implements IPhysicsEngine {
     /** @internal */
-    /**
-     * Global value used to control the smallest number supported by the simulation
-     */
-    public static Epsilon = 0.001;
-
-    //private _impostors: Array<PhysicsImpostor> = [];
-    //private _joints: Array<PhysicsImpostorJoint> = [];
+    private _physicsBodies: Array<PhysicsBody> = [];
     private _subTimeStep: number = 0;
     //private _uniqueIdCounter = 0;
 
@@ -27,6 +22,10 @@ export class PhysicsEngine implements IPhysicsEngine {
      */
     public gravity: Vector3;
 
+    /**
+     *
+     * @returns physics plugin version
+     */
     public getPluginVersion(): number {
         return this._physicsPlugin.getPluginVersion();
     }
@@ -134,7 +133,24 @@ export class PhysicsEngine implements IPhysicsEngine {
             delta = 1.0 / 60.0;
         }
 
-        this._physicsPlugin.executeStep(delta);
+        this._physicsPlugin.executeStep(delta, this._physicsBodies);
+    }
+
+    /**
+     *
+     * @param body
+     */
+    public addBody(physicsBody: PhysicsBody): void {
+        this._physicsBodies.push(physicsBody);
+    }
+    /**
+     *
+     */
+    public removeBody(physicsBody: PhysicsBody): void {
+        const index = this._physicsBodies.indexOf(physicsBody);
+        if (index > -1) {
+            /*const removed =*/ this._physicsBodies.splice(index, 1);
+        }
     }
 
     /**

packages/dev/core/src/Physics/v2/physicsShape.ts

@@ -76,8 +76,8 @@ export class PhysicsShape {
      *
      * @param materialId
      */
-    public setMaterial(materialId: PhysicsMaterial): void {
-        this._physicsPlugin.setMaterial(this, materialId);
+    public setMaterial(material: PhysicsMaterial): void {
+        this._physicsPlugin.setMaterial(this, material);
     }
 
     /**
@@ -156,7 +156,7 @@ export class PhysicsShapeSphere extends PhysicsShape {
      * @param scene
      */
     constructor(center: Vector3, radius: number, scene: Scene) {
-        super(ShapeType.BOX, { center: center, radius: radius }, scene);
+        super(ShapeType.SPHERE, { center: center, radius: radius }, scene);
     }
 }