Merge pull request #3085 from RaananW/physics-pivot-fix

Physics pivot fix

src/Physics/Plugins/babylon.cannonJSPlugin.ts

@@ -116,7 +116,7 @@
             if (meshChildren.length) {
                 var processMesh = (localPosition: Vector3, mesh: AbstractMesh) => {
 
-                    if (!currentRotation || ! mesh.rotationQuaternion) {
+                    if (!currentRotation || !mesh.rotationQuaternion) {
                         return;
                     }
 
@@ -263,10 +263,31 @@
                     returnValue = new this.BJSCANNON.Plane();
                     break;
                 case PhysicsImpostor.MeshImpostor:
+                    // should transform the vertex data to world coordinates!!
                     var rawVerts = object.getVerticesData ? object.getVerticesData(VertexBuffer.PositionKind) : [];
                     var rawFaces = object.getIndices ? object.getIndices() : [];
+                    if (!rawVerts) return;
+                    // get only scale! so the object could transform correctly.
+                    let oldPosition = object.position.clone();
+                    let oldRotation = object.rotation && object.rotation.clone();
+                    let oldQuaternion = object.rotationQuaternion && object.rotationQuaternion.clone();
+                    object.position.copyFromFloats(0, 0, 0);
+                    object.rotation && object.rotation.copyFromFloats(0, 0, 0);
+                    object.rotationQuaternion && object.rotationQuaternion.copyFromFloats(0, 0, 0, 1);
+                    let transform = object.computeWorldMatrix(true);
+                    // convert rawVerts to object space
+                    var temp = new Array<number>();
+                    var index: number;
+                    for (index = 0; index < rawVerts.length; index += 3) {
+                        Vector3.TransformCoordinates(Vector3.FromArray(rawVerts, index), transform).toArray(temp, index);
+                    }
+
                     Tools.Warn("MeshImpostor only collides against spheres.");
-                    returnValue = new this.BJSCANNON.Trimesh(<number[]>rawVerts, <number[]>rawFaces);
+                    returnValue = new this.BJSCANNON.Trimesh(temp, <number[]>rawFaces);
+                    //now set back the transformation!
+                    object.position.copyFrom(oldPosition);
+                    oldRotation && object.rotation && object.rotation.copyFrom(oldRotation);
+                    oldQuaternion && object.rotationQuaternion && object.rotationQuaternion.copyFrom(oldQuaternion);
                     break;
                 case PhysicsImpostor.HeightmapImpostor:
                     returnValue = this._createHeightmap(object);
@@ -289,7 +310,7 @@
             let boundingInfo = <BoundingInfo>(object.getBoundingInfo());
             var dim = Math.min(boundingInfo.boundingBox.extendSizeWorld.x, boundingInfo.boundingBox.extendSizeWorld.z);
             var minY = boundingInfo.boundingBox.extendSizeWorld.y;
-            
+
             var elementSize = dim * 2 / arraySize;
 
             for (var i = 0; i < pos.length; i = i + 3) {
@@ -349,8 +370,11 @@
             //make sure it is updated...
             object.computeWorldMatrix && object.computeWorldMatrix(true);
             // The delta between the mesh position and the mesh bounding box center
+            let bInfo = object.getBoundingInfo();
+            if (!bInfo) return;
             var center = impostor.getObjectCenter();
-            this._tmpDeltaPosition.copyFrom(object.position.subtract(center));
+            //m.getAbsolutePosition().subtract(m.getBoundingInfo().boundingBox.centerWorld)
+            this._tmpDeltaPosition.copyFrom(object.getAbsolutePivotPoint().subtract(center));
             this._tmpPosition.copyFrom(center);
             var quaternion = object.rotationQuaternion;
 
@@ -401,7 +425,7 @@
                 mesh.computeWorldMatrix(true);
             } else if (impostor.type === PhysicsImpostor.MeshImpostor) {
                 this._tmpDeltaPosition.copyFromFloats(0, 0, 0);
-                this._tmpPosition.copyFrom(object.position);
+                //this._tmpPosition.copyFrom(object.position);
             }
 
             impostor.setDeltaPosition(this._tmpDeltaPosition);
@@ -414,11 +438,15 @@
             impostor.object.position.copyFrom(impostor.physicsBody.position);
             if (impostor.object.rotationQuaternion) {
                 impostor.object.rotationQuaternion.copyFrom(impostor.physicsBody.quaternion);
+                //impostor.object.rotationQuaternion.y *= -1;
+                //impostor.object.rotationQuaternion.z *= -1;
             }
         }
 
         public setPhysicsBodyTransformation(impostor: PhysicsImpostor, newPosition: Vector3, newRotation: Quaternion) {
             impostor.physicsBody.position.copy(newPosition);
+            //newRotation.y *= -1;
+            //newRotation.z *= -1;
             impostor.physicsBody.quaternion.copy(newRotation);
         }
 

src/Physics/babylon.physicsImpostor.ts

@@ -16,13 +16,17 @@ module BABYLON {
         rotation?: Vector3;
         parent?: any;
         getBoundingInfo(): Nullable<BoundingInfo>;
-        computeWorldMatrix?(force: boolean): void;
+        computeWorldMatrix(force: boolean): Matrix;
         getWorldMatrix?(): Matrix;
         getChildMeshes?(directDescendantsOnly?: boolean): Array<AbstractMesh>;
         getVerticesData(kind: string): Nullable<Array<number> | Float32Array>;
         getIndices?(): Nullable<IndicesArray>;
         getScene?(): Scene;
         getAbsolutePosition(): Vector3;
+        getAbsolutePivotPoint(): Vector3;
+        rotate(axis: Vector3, amount: number, space?: Space): IPhysicsEnabledObject;
+        translate(axis: Vector3, distance: number, space?: Space): IPhysicsEnabledObject;
+        setAbsolutePosition(absolutePosition: Vector3): IPhysicsEnabledObject;
     }
 
     export class PhysicsImpostor {
@@ -370,8 +374,9 @@ module BABYLON {
             }
         }
 
-        private _tmpPositionWithDelta: Vector3 = Vector3.Zero();
-        private _tmpRotationWithDelta: Quaternion = new Quaternion();
+        //temp variables for parent rotation calculations
+        private _mats: Array<Matrix> = [new Matrix(), new Matrix()];
+        private _tmpQuat: Quaternion = new Quaternion();
 
         /**
          * this function is executed by the physics engine.
@@ -381,23 +386,17 @@ module BABYLON {
                 return;
             }
 
-            if (this._options.ignoreParent && this.object.parent) {
-                this._tmpPositionWithDelta.copyFrom(this.object.getAbsolutePosition());
-                //this.object.getAbsolutePosition().subtractToRef(this._deltaPosition, this._tmpPositionWithDelta);
+            this.object.translate(this._deltaPosition, -1);
+            this._deltaRotationConjugated && this.object.rotationQuaternion && this.object.rotationQuaternion.multiplyToRef(this._deltaRotationConjugated, this.object.rotationQuaternion);
+            if (this.object.parent) {
+                this.object.computeWorldMatrix(false).getRotationMatrixToRef(this._mats[0]);
+                Quaternion.FromRotationMatrixToRef(this._mats[0], this._tmpQuat);
+                this._tmpQuat.normalize();
             } else {
-                this.object.position.subtractToRef(this._deltaPosition, this._tmpPositionWithDelta);
+                this._tmpQuat.copyFrom(this.object.rotationQuaternion || new Quaternion());
             }
-            //conjugate deltaRotation
-            if (this.object.rotationQuaternion) {
-                if (this._deltaRotationConjugated) {
-                    this.object.rotationQuaternion.multiplyToRef(this._deltaRotationConjugated, this._tmpRotationWithDelta);
-                } else {
-                    this._tmpRotationWithDelta.copyFrom(this.object.rotationQuaternion);
-                }
-            }
-            // Only if not disabled
             if (!this._options.disableBidirectionalTransformation) {
-                this._physicsEngine.getPhysicsPlugin().setPhysicsBodyTransformation(this, this._tmpPositionWithDelta, this._tmpRotationWithDelta);
+                this.object.rotationQuaternion && this._physicsEngine.getPhysicsPlugin().setPhysicsBodyTransformation(this, /*bInfo.boundingBox.centerWorld*/ this.object.getAbsolutePivotPoint(), this._tmpQuat);
             }
 
             this._onBeforePhysicsStepCallbacks.forEach((func) => {
@@ -413,21 +412,19 @@ module BABYLON {
                 return;
             }
 
-            this._onAfterPhysicsStepCallbacks.forEach((func) => {
-                func(this);
-            });
-
             this._physicsEngine.getPhysicsPlugin().setTransformationFromPhysicsBody(this);
-
-            if (this._options.ignoreParent && this.object.parent) {
-                this.object.position.subtractInPlace(this.object.parent.getAbsolutePosition());
-            } else {
-                this.object.position.addInPlace(this._deltaPosition)
-            }
-
-            if (this._deltaRotation && this.object.rotationQuaternion) {
-                this.object.rotationQuaternion.multiplyInPlace(this._deltaRotation);
-            }
+            // object has now its world rotation. needs to be converted to local.
+            if (this.object.parent && this.object.rotationQuaternion) {
+                this.object.parent.computeWorldMatrix(false).getRotationMatrixToRef(this._mats[0]);
+                Quaternion.FromRotationMatrixToRef(this._mats[0], this._tmpQuat);
+                this._tmpQuat.conjugateInPlace();
+                this._tmpQuat.normalize();
+                this._tmpQuat.multiplyToRef(this.object.rotationQuaternion, this.object.rotationQuaternion);
+            }
+            // take the position set and make it the absolute position of this object.
+            this.object.setAbsolutePosition(this.object.position);
+            this._deltaRotation && this.object.rotationQuaternion && this.object.rotationQuaternion.multiplyToRef(this._deltaRotation, this.object.rotationQuaternion);
+            this.object.translate(this._deltaPosition, 1);
         }
 
         /**