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

Another small fix, taking parents' rotation but NOT scaling

src/Physics/Plugins/babylon.cannonJSPlugin.ts

@@ -273,7 +273,10 @@
                     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);
+                    object.rotationQuaternion && object.rotationQuaternion.copyFrom(impostor.getParentsRotation());
+
+                    object.rotationQuaternion && object.parent && object.rotationQuaternion.conjugateInPlace();
+
                     let transform = object.computeWorldMatrix(true);
                     // convert rawVerts to object space
                     var temp = new Array<number>();
@@ -290,7 +293,20 @@
                     oldQuaternion && object.rotationQuaternion && object.rotationQuaternion.copyFrom(oldQuaternion);
                     break;
                 case PhysicsImpostor.HeightmapImpostor:
+                    let oldPosition2 = object.position.clone();
+                    let oldRotation2 = object.rotation && object.rotation.clone();
+                    let oldQuaternion2 = object.rotationQuaternion && object.rotationQuaternion.clone();
+                    object.position.copyFromFloats(0, 0, 0);
+                    object.rotation && object.rotation.copyFromFloats(0, 0, 0);
+                    object.rotationQuaternion && object.rotationQuaternion.copyFrom(impostor.getParentsRotation());
+                    object.rotationQuaternion && object.parent && object.rotationQuaternion.conjugateInPlace();
+                    object.rotationQuaternion && object.rotationQuaternion.multiplyInPlace(this._minus90X);
+
                     returnValue = this._createHeightmap(object);
+                    object.position.copyFrom(oldPosition2);
+                    oldRotation2 && object.rotation && object.rotation.copyFrom(oldRotation2);
+                    oldQuaternion2 && object.rotationQuaternion && object.rotationQuaternion.copyFrom(oldQuaternion2);
+                    object.computeWorldMatrix(true);
                     break;
                 case PhysicsImpostor.ParticleImpostor:
                     returnValue = new this.BJSCANNON.Particle();
@@ -302,21 +318,29 @@
 
         private _createHeightmap(object: IPhysicsEnabledObject, pointDepth?: number) {
             var pos = <FloatArray>(object.getVerticesData(VertexBuffer.PositionKind));
+            let transform = object.computeWorldMatrix(true);
+            // convert rawVerts to object space
+            var temp = new Array<number>();
+            var index: number;
+            for (index = 0; index < pos.length; index += 3) {
+                Vector3.TransformCoordinates(Vector3.FromArray(pos, index), transform).toArray(temp, index);
+            }
+            pos = temp;
             var matrix = new Array<Array<any>>();
 
             //For now pointDepth will not be used and will be automatically calculated.
             //Future reference - try and find the best place to add a reference to the pointDepth variable.
             var arraySize = pointDepth || ~~(Math.sqrt(pos.length / 3) - 1);
             let boundingInfo = <BoundingInfo>(object.getBoundingInfo());
-            var dim = Math.min(boundingInfo.boundingBox.extendSizeWorld.x, boundingInfo.boundingBox.extendSizeWorld.z);
-            var minY = boundingInfo.boundingBox.extendSizeWorld.y;
+            var dim = Math.min(boundingInfo.boundingBox.extendSizeWorld.x, boundingInfo.boundingBox.extendSizeWorld.y);
+            var minY = boundingInfo.boundingBox.extendSizeWorld.z;
 
             var elementSize = dim * 2 / arraySize;
 
             for (var i = 0; i < pos.length; i = i + 3) {
                 var x = Math.round((pos[i + 0]) / elementSize + arraySize / 2);
-                var z = Math.round(((pos[i + 2]) / elementSize - arraySize / 2) * -1);
-                var y = pos[i + 1] + minY;
+                var z = Math.round(((pos[i + 1]) / elementSize - arraySize / 2) * -1);
+                var y = -pos[i + 2] + minY;
                 if (!matrix[x]) {
                     matrix[x] = [];
                 }

src/Physics/babylon.physicsImpostor.ts

@@ -375,8 +375,24 @@ module BABYLON {
         }
 
         //temp variables for parent rotation calculations
-        private _mats: Array<Matrix> = [new Matrix(), new Matrix()];
+        //private _mats: Array<Matrix> = [new Matrix(), new Matrix()];
         private _tmpQuat: Quaternion = new Quaternion();
+        private _tmpQuat2: Quaternion = new Quaternion();
+
+        public getParentsRotation() {
+            let parent = this.object.parent;
+            this._tmpQuat.copyFromFloats(0, 0, 0, 1);
+            while (parent) {
+                if (parent.rotationQuaternion) {
+                    this._tmpQuat2.copyFrom(parent.rotationQuaternion);
+                } else {
+                    Quaternion.RotationYawPitchRollToRef(parent.rotation.y, parent.rotation.x, parent.rotation.z, this._tmpQuat2)
+                }
+                this._tmpQuat.multiplyToRef(this._tmpQuat2, this._tmpQuat);
+                parent = parent.parent;
+            }
+            return this._tmpQuat;
+        }
 
         /**
          * this function is executed by the physics engine.
@@ -388,10 +404,9 @@ module BABYLON {
 
             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();
+            if (this.object.parent && this.object.rotationQuaternion) {
+                this.getParentsRotation();
+                this._tmpQuat.multiplyToRef(this.object.rotationQuaternion, this._tmpQuat);
             } else {
                 this._tmpQuat.copyFrom(this.object.rotationQuaternion || new Quaternion());
             }
@@ -415,10 +430,8 @@ module BABYLON {
             this._physicsEngine.getPhysicsPlugin().setTransformationFromPhysicsBody(this);
             // 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.getParentsRotation();
                 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.