Initial quaternion fix and Cleanup

If an object had a quaternion as its rotation, the rotation is lost
after initializing the physics.
The problem is Oimo's setQuaternion function, which actually doesn't do
that. It sets the newQuaternion variable, that is only set after a
collision.
This way, the rotaionQuaternion is set correctly and is applied to the
body right after initialization.

Babylon/Physics/Plugins/babylon.oimoJSPlugin.js

@@ -46,27 +46,27 @@ var BABYLON;
             var body = null;
             this.unregisterMesh(mesh);
             mesh.computeWorldMatrix(true);
+            var initialRotation = null;
+            if (mesh.rotationQuaternion) {
+                initialRotation = mesh.rotationQuaternion.clone();
+                mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+                mesh.computeWorldMatrix(true);
+            }
+            var bbox = mesh.getBoundingInfo().boundingBox;
+            // The delta between the mesh position and the mesh bounding box center
+            var deltaPosition = mesh.position.subtract(bbox.center);
+            // Transform delta position with the rotation
+            if (initialRotation) {
+                var m = new BABYLON.Matrix();
+                initialRotation.toRotationMatrix(m);
+                deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
+            }
             switch (impostor) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
-                    var initialRotation = null;
-                    if (mesh.rotationQuaternion) {
-                        initialRotation = mesh.rotationQuaternion.clone();
-                        mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        mesh.computeWorldMatrix(true);
-                    }
-                    var bbox = mesh.getBoundingInfo().boundingBox;
                     var radiusX = bbox.maximumWorld.x - bbox.minimumWorld.x;
                     var radiusY = bbox.maximumWorld.y - bbox.minimumWorld.y;
                     var radiusZ = bbox.maximumWorld.z - bbox.minimumWorld.z;
                     var size = Math.max(this._checkWithEpsilon(radiusX), this._checkWithEpsilon(radiusY), this._checkWithEpsilon(radiusZ)) / 2;
-                    // The delta between the mesh position and the mesh bounding box center
-                    var deltaPosition = mesh.position.subtract(bbox.center);
-                    // Transform delta position with the rotation
-                    if (initialRotation) {
-                        var m = new BABYLON.Matrix();
-                        initialRotation.toRotationMatrix(m);
-                        deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
-                    }
                     body = new OIMO.Body({
                         type: 'sphere',
                         size: [size],
@@ -76,39 +76,16 @@ var BABYLON;
                         config: [options.mass, options.friction, options.restitution],
                         world: this._world
                     });
-                    // Restore rotation
-                    if (initialRotation) {
-                        body.setQuaternion(initialRotation);
-                    }
-                    this._registeredMeshes.push({
-                        mesh: mesh,
-                        body: body,
-                        delta: deltaPosition
-                    });
                     break;
                 case BABYLON.PhysicsEngine.PlaneImpostor:
+                case BABYLON.PhysicsEngine.CylinderImpostor:
                 case BABYLON.PhysicsEngine.BoxImpostor:
-                    initialRotation = null;
-                    if (mesh.rotationQuaternion) {
-                        initialRotation = mesh.rotationQuaternion.clone();
-                        mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        mesh.computeWorldMatrix(true);
-                    }
-                    bbox = mesh.getBoundingInfo().boundingBox;
                     var min = bbox.minimumWorld;
                     var max = bbox.maximumWorld;
                     var box = max.subtract(min);
                     var sizeX = this._checkWithEpsilon(box.x);
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
-                    // The delta between the mesh position and the mesh boudning box center
-                    deltaPosition = mesh.position.subtract(bbox.center);
-                    // Transform delta position with the rotation
-                    if (initialRotation) {
-                        m = new BABYLON.Matrix();
-                        initialRotation.toRotationMatrix(m);
-                        deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
-                    }
                     body = new OIMO.Body({
                         type: 'box',
                         size: [sizeX, sizeY, sizeZ],
@@ -118,16 +95,21 @@ var BABYLON;
                         config: [options.mass, options.friction, options.restitution],
                         world: this._world
                     });
-                    if (initialRotation) {
-                        body.setQuaternion(initialRotation);
-                    }
-                    this._registeredMeshes.push({
-                        mesh: mesh,
-                        body: body,
-                        delta: deltaPosition
-                    });
                     break;
             }
+            //If quaternion was set as the rotation of the object
+            if (initialRotation) {
+                //We have to access the rigid body's properties to set the quaternion. 
+                //The setQuaternion function of Oimo only sets the newOrientation that is only set after an impulse is given or a collision.
+                body.body.orientation = new OIMO.Quat(initialRotation.w, initialRotation.x, initialRotation.y, initialRotation.z);
+                //update the internal rotation matrix
+                body.body.syncShapes();
+            }
+            this._registeredMeshes.push({
+                mesh: mesh,
+                body: body,
+                delta: deltaPosition
+            });
             return body;
         };
         OimoJSPlugin.prototype.registerMeshesAsCompound = function (parts, options) {
@@ -318,7 +300,7 @@ var BABYLON;
                         if (!mesh.rotationQuaternion) {
                             mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
                         }
-                        mesh.rotationQuaternion.fromRotationMatrix(mtx);
+                        BABYLON.Quaternion.FromRotationMatrixToRef(mtx, mesh.rotationQuaternion);
                         mesh.computeWorldMatrix();
                     }
                 }

Babylon/Physics/Plugins/babylon.oimoJSPlugin.ts

@@ -23,18 +23,31 @@ module BABYLON {
             this.unregisterMesh(mesh);
             mesh.computeWorldMatrix(true);
 
+
+            var initialRotation = null;
+            if (mesh.rotationQuaternion) {
+                initialRotation = mesh.rotationQuaternion.clone();
+                mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+                mesh.computeWorldMatrix(true);
+            }
+
+            var bbox = mesh.getBoundingInfo().boundingBox;
+
+            // The delta between the mesh position and the mesh bounding box center
+            var deltaPosition = mesh.position.subtract(bbox.center);
+
+            // Transform delta position with the rotation
+            if (initialRotation) {
+                var m = new BABYLON.Matrix();
+                initialRotation.toRotationMatrix(m);
+                deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
+            }
+
             // register mesh
             switch (impostor) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
 
-                    var initialRotation = null;
-                    if (mesh.rotationQuaternion) {
-                        initialRotation = mesh.rotationQuaternion.clone();
-                        mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        mesh.computeWorldMatrix(true);
-                    }
-
-                    var bbox = mesh.getBoundingInfo().boundingBox;
+                    
                     var radiusX = bbox.maximumWorld.x - bbox.minimumWorld.x;
                     var radiusY = bbox.maximumWorld.y - bbox.minimumWorld.y;
                     var radiusZ = bbox.maximumWorld.z - bbox.minimumWorld.z;
@@ -44,16 +57,6 @@ module BABYLON {
                         this._checkWithEpsilon(radiusY),
                         this._checkWithEpsilon(radiusZ)) / 2;
 
-                    // The delta between the mesh position and the mesh bounding box center
-                    var deltaPosition = mesh.position.subtract(bbox.center);
-
-                    // Transform delta position with the rotation
-                    if (initialRotation) {
-                        var m = new BABYLON.Matrix();
-                        initialRotation.toRotationMatrix(m);
-                        deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
-                    }
-
                     body = new OIMO.Body({
                         type: 'sphere',
                         size: [size],
@@ -64,29 +67,13 @@ module BABYLON {
                         world: this._world
                     });
 
-                    // Restore rotation
-                    if (initialRotation) {
-                        body.setQuaternion(initialRotation);
-                    }
-
-                    this._registeredMeshes.push({
-                        mesh: mesh,
-                        body: body,
-                        delta: deltaPosition
-                    });
                     break;
 
-                case BABYLON.PhysicsEngine.PlaneImpostor:
-                case BABYLON.PhysicsEngine.BoxImpostor:
-
-                    initialRotation = null;
-                    if (mesh.rotationQuaternion) {
-                        initialRotation = mesh.rotationQuaternion.clone();
-                        mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        mesh.computeWorldMatrix(true);
-                    }
+                case PhysicsEngine.PlaneImpostor:
+                //Oimo "fakes" a cylinder as a box, so why don't we!
+                case PhysicsEngine.CylinderImpostor:
+                case PhysicsEngine.BoxImpostor:
 
-                    bbox = mesh.getBoundingInfo().boundingBox;
                     var min = bbox.minimumWorld;
                     var max = bbox.maximumWorld;
                     var box = max.subtract(min);
@@ -94,16 +81,6 @@ module BABYLON {
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
 
-                    // The delta between the mesh position and the mesh boudning box center
-                    deltaPosition = mesh.position.subtract(bbox.center);
-
-                    // Transform delta position with the rotation
-                    if (initialRotation) {
-                        m = new BABYLON.Matrix();
-                        initialRotation.toRotationMatrix(m);
-                        deltaPosition = BABYLON.Vector3.TransformCoordinates(deltaPosition, m);
-                    }
-
                     body = new OIMO.Body({
                         type: 'box',
                         size: [sizeX, sizeY, sizeZ],
@@ -114,18 +91,24 @@ module BABYLON {
                         world: this._world
                     });
 
-                    if (initialRotation) {
-                        body.setQuaternion(initialRotation);
-                    }
-
-                    this._registeredMeshes.push({
-                        mesh: mesh,
-                        body: body,
-                        delta: deltaPosition
-                    });
                     break;
+            }
 
+            //If quaternion was set as the rotation of the object
+            if (initialRotation) {
+                //We have to access the rigid body's properties to set the quaternion. 
+                //The setQuaternion function of Oimo only sets the newOrientation that is only set after an impulse is given or a collision.
+                body.body.orientation = new OIMO.Quat(initialRotation.w, initialRotation.x, initialRotation.y, initialRotation.z);
+                //update the internal rotation matrix
+                body.body.syncShapes();
             }
+
+            this._registeredMeshes.push({
+                mesh: mesh,
+                body: body,
+                delta: deltaPosition
+            });
+
             return body;
         }
 
@@ -386,7 +369,7 @@ module BABYLON {
                         if (!mesh.rotationQuaternion) {
                             mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
                         }
-                        mesh.rotationQuaternion.fromRotationMatrix(mtx);
+                        Quaternion.FromRotationMatrixToRef(mtx, mesh.rotationQuaternion);
                         mesh.computeWorldMatrix();
                     }
                 }