Merge pull request #761 from RaananW/physics-api-changes

Physics api changes and Oimo fixes

src/Physics/Plugins/babylon.cannonJSPlugin.js

@@ -38,10 +38,12 @@ var BABYLON;
                 }
                 // Body position
                 var bodyX = registeredMesh.body.position.x, bodyY = registeredMesh.body.position.y, bodyZ = registeredMesh.body.position.z;
-                var deltaPos = registeredMesh.delta || BABYLON.Vector3.Zero();
-                registeredMesh.mesh.position.x = bodyX + deltaPos.x;
-                registeredMesh.mesh.position.y = bodyY + deltaPos.y;
-                registeredMesh.mesh.position.z = bodyZ + deltaPos.z;
+                if (!registeredMesh.delta) {
+                    registeredMesh.delta = BABYLON.Vector3.Zero();
+                }
+                registeredMesh.mesh.position.x = bodyX + registeredMesh.delta.x;
+                registeredMesh.mesh.position.y = bodyY + registeredMesh.delta.y;
+                registeredMesh.mesh.position.z = bodyZ + registeredMesh.delta.z;
                 registeredMesh.mesh.rotationQuaternion.x = registeredMesh.body.quaternion.x;
                 registeredMesh.mesh.rotationQuaternion.y = registeredMesh.body.quaternion.y;
                 registeredMesh.mesh.rotationQuaternion.z = registeredMesh.body.quaternion.z;
@@ -53,18 +55,27 @@ var BABYLON;
         };
         CannonJSPlugin.prototype.registerMesh = function (mesh, impostor, options) {
             this.unregisterMesh(mesh);
+            if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
+            }
             mesh.computeWorldMatrix(true);
             var shape = this._createShape(mesh, impostor, options);
             return this._createRigidBodyFromShape(shape, mesh, options.mass, options.friction, options.restitution);
         };
         CannonJSPlugin.prototype._createShape = function (mesh, impostor, options) {
+            //get the correct bounding box
+            var oldQuaternion = mesh.rotationQuaternion;
+            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
+            var returnValue;
             switch (impostor) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
                     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;
-                    return new CANNON.Sphere(Math.max(this._checkWithEpsilon(radiusX), this._checkWithEpsilon(radiusY), this._checkWithEpsilon(radiusZ)) / 2);
+                    returnValue = new CANNON.Sphere(Math.max(this._checkWithEpsilon(radiusX), this._checkWithEpsilon(radiusY), this._checkWithEpsilon(radiusZ)) / 2);
+                    break;
                 //TMP also for cylinder - TODO Cannon supports cylinder natively.
                 case BABYLON.PhysicsEngine.CylinderImpostor:
                     BABYLON.Tools.Warn("CylinderImposter not yet implemented, using BoxImposter instead");
@@ -73,15 +84,20 @@ var BABYLON;
                     var min = bbox.minimumWorld;
                     var max = bbox.maximumWorld;
                     var box = max.subtract(min).scale(0.5);
-                    return new CANNON.Box(new CANNON.Vec3(this._checkWithEpsilon(box.x), this._checkWithEpsilon(box.y), this._checkWithEpsilon(box.z)));
+                    returnValue = new CANNON.Box(new CANNON.Vec3(this._checkWithEpsilon(box.x), this._checkWithEpsilon(box.y), this._checkWithEpsilon(box.z)));
+                    break;
                 case BABYLON.PhysicsEngine.PlaneImpostor:
                     BABYLON.Tools.Warn("Attention, Cannon.js PlaneImposter might not behave as you wish. Consider using BoxImposter instead");
-                    return new CANNON.Plane();
+                    returnValue = new CANNON.Plane();
+                    break;
                 case BABYLON.PhysicsEngine.MeshImpostor:
                     var rawVerts = mesh.getVerticesData(BABYLON.VertexBuffer.PositionKind);
                     var rawFaces = mesh.getIndices();
-                    return this._createConvexPolyhedron(rawVerts, rawFaces, mesh, options);
+                    returnValue = this._createConvexPolyhedron(rawVerts, rawFaces, mesh, options);
+                    break;
             }
+            mesh.rotationQuaternion = oldQuaternion;
+            return returnValue;
         };
         CannonJSPlugin.prototype._createConvexPolyhedron = function (rawVerts, rawFaces, mesh, options) {
             var verts = [], faces = [];
@@ -118,7 +134,6 @@ var BABYLON;
             return currentMat;
         };
         CannonJSPlugin.prototype._createRigidBodyFromShape = function (shape, mesh, mass, friction, restitution) {
-            var initialRotation = null;
             if (!mesh.rotationQuaternion) {
                 mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
             }
@@ -219,6 +234,18 @@ var BABYLON;
         CannonJSPlugin.prototype.isSupported = function () {
             return window.CANNON !== undefined;
         };
+        CannonJSPlugin.prototype.getWorldObject = function () {
+            return this._world;
+        };
+        CannonJSPlugin.prototype.getPhysicsBodyOfMesh = function (mesh) {
+            for (var index = 0; index < this._registeredMeshes.length; index++) {
+                var registeredMesh = this._registeredMeshes[index];
+                if (registeredMesh.mesh === mesh) {
+                    return registeredMesh.body;
+                }
+            }
+            return null;
+        };
         return CannonJSPlugin;
     })();
     BABYLON.CannonJSPlugin = CannonJSPlugin;

src/Physics/Plugins/babylon.cannonJSPlugin.ts

@@ -32,11 +32,13 @@
                     bodyY = registeredMesh.body.position.y,
                     bodyZ = registeredMesh.body.position.z;
 
-                var deltaPos = registeredMesh.delta || Vector3.Zero();
+                if(!registeredMesh.delta) {
+                    registeredMesh.delta = Vector3.Zero();
+                }
 
-                registeredMesh.mesh.position.x = bodyX + deltaPos.x;
-                registeredMesh.mesh.position.y = bodyY + deltaPos.y;
-                registeredMesh.mesh.position.z = bodyZ + deltaPos.z;
+                registeredMesh.mesh.position.x = bodyX + registeredMesh.delta.x;
+                registeredMesh.mesh.position.y = bodyY + registeredMesh.delta.y;
+                registeredMesh.mesh.position.z = bodyZ + registeredMesh.delta.z;
 
                 registeredMesh.mesh.rotationQuaternion.x = registeredMesh.body.quaternion.x;
                 registeredMesh.mesh.rotationQuaternion.y = registeredMesh.body.quaternion.y;
@@ -52,6 +54,10 @@
         public registerMesh(mesh: AbstractMesh, impostor: number, options?: PhysicsBodyCreationOptions): any {
             this.unregisterMesh(mesh);
 
+			if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
+            }
+			
             mesh.computeWorldMatrix(true);
 
             var shape = this._createShape(mesh, impostor, options);
@@ -60,6 +66,14 @@
         }
 
         private _createShape(mesh: AbstractMesh, impostor: number, options?: PhysicsBodyCreationOptions) {
+		
+			//get the correct bounding box
+			var oldQuaternion = mesh.rotationQuaternion;
+			mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
+			
+			var returnValue;
+		
             switch (impostor) {
                 case PhysicsEngine.SphereImpostor:
                     var bbox = mesh.getBoundingInfo().boundingBox;
@@ -67,7 +81,9 @@
                     var radiusY = bbox.maximumWorld.y - bbox.minimumWorld.y;
                     var radiusZ = bbox.maximumWorld.z - bbox.minimumWorld.z;
 
-                    return new CANNON.Sphere(Math.max(this._checkWithEpsilon(radiusX), this._checkWithEpsilon(radiusY), this._checkWithEpsilon(radiusZ)) / 2);
+                    returnValue = new CANNON.Sphere(Math.max(this._checkWithEpsilon(radiusX), this._checkWithEpsilon(radiusY), this._checkWithEpsilon(radiusZ)) / 2);
+					
+					break;
                 //TMP also for cylinder - TODO Cannon supports cylinder natively.
                 case PhysicsEngine.CylinderImpostor:
                     Tools.Warn("CylinderImposter not yet implemented, using BoxImposter instead");
@@ -76,16 +92,23 @@
                     var min = bbox.minimumWorld;
                     var max = bbox.maximumWorld;
                     var box = max.subtract(min).scale(0.5);
-                    return new CANNON.Box(new CANNON.Vec3(this._checkWithEpsilon(box.x), this._checkWithEpsilon(box.y), this._checkWithEpsilon(box.z)));
+                    returnValue =  new CANNON.Box(new CANNON.Vec3(this._checkWithEpsilon(box.x), this._checkWithEpsilon(box.y), this._checkWithEpsilon(box.z)));
+					break;
                 case PhysicsEngine.PlaneImpostor:
                     Tools.Warn("Attention, Cannon.js PlaneImposter might not behave as you wish. Consider using BoxImposter instead");
-                    return new CANNON.Plane();
+                    returnValue = new CANNON.Plane();
+					break;
                 case PhysicsEngine.MeshImpostor:
                     var rawVerts = mesh.getVerticesData(VertexBuffer.PositionKind);
                     var rawFaces = mesh.getIndices();
 
-                    return this._createConvexPolyhedron(rawVerts, rawFaces, mesh, options);
+                    returnValue = this._createConvexPolyhedron(rawVerts, rawFaces, mesh, options);
+					break;
             }
+			
+			mesh.rotationQuaternion = oldQuaternion;
+			
+			return returnValue;
         }
 
         private _createConvexPolyhedron(rawVerts: number[] | Float32Array, rawFaces: number[], mesh: AbstractMesh, options?: PhysicsBodyCreationOptions): any {
@@ -138,8 +161,6 @@
         }
 
         private _createRigidBodyFromShape(shape: any, mesh: AbstractMesh, mass: number, friction: number, restitution: number): any {
-            var initialRotation: Quaternion = null;
-
             if (!mesh.rotationQuaternion) {
                 mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
             }
@@ -287,6 +308,20 @@
         public isSupported(): boolean {
             return window.CANNON !== undefined;
         }
+        
+        public getWorldObject() : any {
+            return this._world;
+        }
+        
+        public getPhysicsBodyOfMesh(mesh: AbstractMesh) {
+            for (var index = 0; index < this._registeredMeshes.length; index++) {
+                var registeredMesh = this._registeredMeshes[index];
+                if (registeredMesh.mesh === mesh) {
+                    return registeredMesh.body;
+                }
+            }
+            return null;
+        }
     }
 }
 

src/Physics/Plugins/babylon.oimoJSPlugin.js

@@ -15,7 +15,7 @@ var BABYLON;
                         mesh.computeWorldMatrix(true);
                         var center = mesh.getBoundingInfo().boundingBox.center;
                         body.setPosition(new OIMO.Vec3(center.x, center.y, center.z));
-                        body.setRotation(new OIMO.Vec3(mesh.rotation.x, mesh.rotation.y, mesh.rotation.z));
+                        body.setQuaternion(mesh.rotationQuaternion);
                         body.sleeping = false;
                         return;
                     }
@@ -24,10 +24,9 @@ var BABYLON;
                         mesh.computeWorldMatrix(true);
                         registeredMesh.mesh.computeWorldMatrix(true);
                         var absolutePosition = registeredMesh.mesh.getAbsolutePosition();
-                        var absoluteRotation = mesh.rotation;
                         body = registeredMesh.body.body;
                         body.setPosition(new OIMO.Vec3(absolutePosition.x, absolutePosition.y, absolutePosition.z));
-                        body.setRotation(new OIMO.Vec3(absoluteRotation.x, absoluteRotation.y, absoluteRotation.z));
+                        body.setQuaternion(mesh.rotationQuaternion);
                         body.sleeping = false;
                         return;
                     }
@@ -38,31 +37,34 @@ var BABYLON;
             return value < BABYLON.PhysicsEngine.Epsilon ? BABYLON.PhysicsEngine.Epsilon : value;
         };
         OimoJSPlugin.prototype.initialize = function (iterations) {
-            this._world = new OIMO.World();
+            this._world = new OIMO.World(null, null, iterations);
             this._world.clear();
         };
         OimoJSPlugin.prototype.setGravity = function (gravity) {
             this._world.gravity = gravity;
         };
         OimoJSPlugin.prototype.registerMesh = function (mesh, impostor, options) {
-            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);
+            if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
             }
+            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);
-            }
+            //calculate rotation to fit Oimo's needs (Euler...)
+            var rot = new OIMO.Euler().setFromQuaternion({ x: mesh.rotationQuaternion.x, y: mesh.rotationQuaternion.y, z: mesh.rotationQuaternion.z, s: mesh.rotationQuaternion.w });
+            //get the correct bounding box
+            var oldQuaternion = mesh.rotationQuaternion;
+            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
+            var bodyConfig = {
+                pos: [bbox.center.x, bbox.center.y, bbox.center.z],
+                rot: [rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD],
+                move: options.mass != 0,
+                config: [options.mass, options.friction, options.restitution],
+                world: this._world
+            };
             // register mesh
             switch (impostor) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
@@ -70,15 +72,8 @@ var BABYLON;
                     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;
-                    body = new OIMO.Body({
-                        type: 'sphere',
-                        size: [size],
-                        pos: [bbox.center.x, bbox.center.y, bbox.center.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD],
-                        move: options.mass != 0,
-                        config: [options.mass, options.friction, options.restitution],
-                        world: this._world
-                    });
+                    bodyConfig.type = 'sphere';
+                    bodyConfig.size = [size];
                     break;
                 case BABYLON.PhysicsEngine.PlaneImpostor:
                 //Oimo "fakes" a cylinder as a box, so why don't we!
@@ -90,30 +85,25 @@ var BABYLON;
                     var sizeX = this._checkWithEpsilon(box.x);
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
-                    body = new OIMO.Body({
-                        type: 'box',
-                        size: [sizeX, sizeY, sizeZ],
-                        pos: [bbox.center.x, bbox.center.y, bbox.center.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD],
-                        move: options.mass != 0,
-                        config: [options.mass, options.friction, options.restitution],
-                        world: this._world
-                    });
+                    bodyConfig.type = 'box';
+                    bodyConfig.size = [sizeX, sizeY, sizeZ];
                     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();
-            }
+            var body = new OIMO.Body(bodyConfig);
+            //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(mesh.rotationQuaternion.w, mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z);
+            //TEST
+            //body.body.resetQuaternion(new OIMO.Quat(mesh.rotationQuaternion.w, mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z));
+            //update the internal rotation matrix
+            //body.body.syncShapes();
             this._registeredMeshes.push({
                 mesh: mesh,
                 body: body,
                 delta: deltaPosition
             });
+            //for the sake of consistency.
+            mesh.rotationQuaternion = oldQuaternion;
             return body;
         };
         OimoJSPlugin.prototype.registerMeshesAsCompound = function (parts, options) {
@@ -136,6 +126,9 @@ var BABYLON;
                 config: [options.mass, options.friction, options.restitution],
                 world: this._world
             });
+            //Reset the body's rotation to be of the initial mesh's.
+            var rot = new OIMO.Euler().setFromQuaternion({ x: initialMesh.rotationQuaternion.x, y: initialMesh.rotationQuaternion.y, z: initialMesh.rotationQuaternion.z, s: initialMesh.rotationQuaternion.w });
+            body.resetRotation(rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD);
             this._registeredMeshes.push({
                 mesh: initialMesh,
                 body: body
@@ -143,10 +136,23 @@ var BABYLON;
             return body;
         };
         OimoJSPlugin.prototype._createBodyAsCompound = function (part, options, initialMesh) {
-            var bodyParameters = null;
             var mesh = part.mesh;
+            if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
+            }
             // We need the bounding box/sphere info to compute the physics body
-            mesh.computeWorldMatrix();
+            mesh.computeWorldMatrix(true);
+            var rot = new OIMO.Euler().setFromQuaternion({ x: mesh.rotationQuaternion.x, y: mesh.rotationQuaternion.y, z: mesh.rotationQuaternion.z, s: mesh.rotationQuaternion.w });
+            var bodyParameters = {
+                pos: [mesh.position.x, mesh.position.y, mesh.position.z],
+                //A bug in Oimo (Body class) prevents us from using rot directly.
+                rot: [0, 0, 0],
+                //For future reference, if the bug will ever be fixed.
+                realRot: [rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD]
+            };
+            var oldQuaternion = mesh.rotationQuaternion;
+            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
             switch (part.impostor) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
                     var bbox = mesh.getBoundingInfo().boundingBox;
@@ -154,15 +160,11 @@ var BABYLON;
                     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;
-                    bodyParameters = {
-                        type: 'sphere',
-                        /* bug with oimo : sphere needs 3 sizes in this case */
-                        size: [size, -1, -1],
-                        pos: [mesh.position.x, mesh.position.y, mesh.position.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD]
-                    };
+                    bodyParameters.type = 'sphere';
+                    bodyParameters.size = [size, size, size];
                     break;
                 case BABYLON.PhysicsEngine.PlaneImpostor:
+                case BABYLON.PhysicsEngine.CylinderImpostor:
                 case BABYLON.PhysicsEngine.BoxImpostor:
                     bbox = mesh.getBoundingInfo().boundingBox;
                     var min = bbox.minimumWorld;
@@ -171,15 +173,11 @@ var BABYLON;
                     var sizeX = this._checkWithEpsilon(box.x);
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
-                    var relativePosition = mesh.position;
-                    bodyParameters = {
-                        type: 'box',
-                        size: [sizeX, sizeY, sizeZ],
-                        pos: [relativePosition.x, relativePosition.y, relativePosition.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD]
-                    };
+                    bodyParameters.type = 'box';
+                    bodyParameters.size = [sizeX, sizeY, sizeZ];
                     break;
             }
+            mesh.rotationQuaternion = oldQuaternion;
             return bodyParameters;
         };
         OimoJSPlugin.prototype.unregisterMesh = function (mesh) {
@@ -257,6 +255,18 @@ var BABYLON;
         OimoJSPlugin.prototype.isSupported = function () {
             return OIMO !== undefined;
         };
+        OimoJSPlugin.prototype.getWorldObject = function () {
+            return this._world;
+        };
+        OimoJSPlugin.prototype.getPhysicsBodyOfMesh = function (mesh) {
+            for (var index = 0; index < this._registeredMeshes.length; index++) {
+                var registeredMesh = this._registeredMeshes[index];
+                if (registeredMesh.mesh === mesh) {
+                    return registeredMesh.body;
+                }
+            }
+            return null;
+        };
         OimoJSPlugin.prototype._getLastShape = function (body) {
             var lastShape = body.shapes;
             while (lastShape.next) {
@@ -272,41 +282,22 @@ var BABYLON;
             while (i--) {
                 var body = this._registeredMeshes[i].body.body;
                 var mesh = this._registeredMeshes[i].mesh;
-                var delta = this._registeredMeshes[i].delta;
+                if (!this._registeredMeshes[i].delta) {
+                    this._registeredMeshes[i].delta = BABYLON.Vector3.Zero();
+                }
                 if (!body.sleeping) {
+                    //TODO check that
                     if (body.shapes.next) {
                         var parentShape = this._getLastShape(body);
                         mesh.position.x = parentShape.position.x * OIMO.WORLD_SCALE;
                         mesh.position.y = parentShape.position.y * OIMO.WORLD_SCALE;
                         mesh.position.z = parentShape.position.z * OIMO.WORLD_SCALE;
-                        var mtx = BABYLON.Matrix.FromArray(body.getMatrix());
-                        if (!mesh.rotationQuaternion) {
-                            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        }
-                        mesh.rotationQuaternion.fromRotationMatrix(mtx);
-                        mesh.computeWorldMatrix();
                     }
                     else {
-                        m = body.getMatrix();
-                        mtx = BABYLON.Matrix.FromArray(m);
-                        // Body position
-                        var bodyX = mtx.m[12], bodyY = mtx.m[13], bodyZ = mtx.m[14];
-                        if (!delta) {
-                            mesh.position.x = bodyX;
-                            mesh.position.y = bodyY;
-                            mesh.position.z = bodyZ;
-                        }
-                        else {
-                            mesh.position.x = bodyX + delta.x;
-                            mesh.position.y = bodyY + delta.y;
-                            mesh.position.z = bodyZ + delta.z;
-                        }
-                        if (!mesh.rotationQuaternion) {
-                            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-                        }
-                        BABYLON.Quaternion.FromRotationMatrixToRef(mtx, mesh.rotationQuaternion);
-                        mesh.computeWorldMatrix();
+                        mesh.position.copyFrom(body.getPosition()).addInPlace(this._registeredMeshes[i].delta);
                     }
+                    mesh.rotationQuaternion.copyFrom(body.getQuaternion());
+                    mesh.computeWorldMatrix();
                 }
             }
         };

src/Physics/Plugins/babylon.oimoJSPlugin.ts

@@ -10,7 +10,7 @@ module BABYLON {
         }
 
         public initialize(iterations?: number): void {
-            this._world = new OIMO.World();
+            this._world = new OIMO.World(null, null, iterations);
             this._world.clear();
         }
 
@@ -19,35 +19,38 @@ module BABYLON {
         }
 
         public registerMesh(mesh: AbstractMesh, impostor: number, options: PhysicsBodyCreationOptions): any {
-            var body = null;
             this.unregisterMesh(mesh);
-            mesh.computeWorldMatrix(true);
-
-
-            var initialRotation = null;
-            if (mesh.rotationQuaternion) {
-                initialRotation = mesh.rotationQuaternion.clone();
-                mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
-                mesh.computeWorldMatrix(true);
+            
+            if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
             }
+            
+            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);
+            
+            //calculate rotation to fit Oimo's needs (Euler...)
+            var rot = new OIMO.Euler().setFromQuaternion({ x: mesh.rotationQuaternion.x, y: mesh.rotationQuaternion.y, z: mesh.rotationQuaternion.z, s: mesh.rotationQuaternion.w });
 
-            // Transform delta position with the rotation
-            if (initialRotation) {
-                var m = new Matrix();
-                initialRotation.toRotationMatrix(m);
-                deltaPosition = Vector3.TransformCoordinates(deltaPosition, m);
-            }
+			//get the correct bounding box
+			var oldQuaternion = mesh.rotationQuaternion;
+			mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
+			
+            var bodyConfig : any = {
+                pos: [bbox.center.x, bbox.center.y, bbox.center.z],
+                rot: [rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD],
+                move: options.mass != 0,
+                config: [options.mass, options.friction, options.restitution],
+                world: this._world
+            };
 
             // register mesh
             switch (impostor) {
                 case PhysicsEngine.SphereImpostor:
-
-
                     var radiusX = bbox.maximumWorld.x - bbox.minimumWorld.x;
                     var radiusY = bbox.maximumWorld.y - bbox.minimumWorld.y;
                     var radiusZ = bbox.maximumWorld.z - bbox.minimumWorld.z;
@@ -57,16 +60,8 @@ module BABYLON {
                         this._checkWithEpsilon(radiusY),
                         this._checkWithEpsilon(radiusZ)) / 2;
 
-                    body = new OIMO.Body({
-                        type: 'sphere',
-                        size: [size],
-                        pos: [bbox.center.x, bbox.center.y, bbox.center.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD],
-                        move: options.mass != 0,
-                        config: [options.mass, options.friction, options.restitution],
-                        world: this._world
-                    });
-
+                    bodyConfig.type = 'sphere';
+                    bodyConfig.size = [size];
                     break;
 
                 case PhysicsEngine.PlaneImpostor:
@@ -81,33 +76,29 @@ module BABYLON {
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
 
-                    body = new OIMO.Body({
-                        type: 'box',
-                        size: [sizeX, sizeY, sizeZ],
-                        pos: [bbox.center.x, bbox.center.y, bbox.center.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD],
-                        move: options.mass != 0,
-                        config: [options.mass, options.friction, options.restitution],
-                        world: this._world
-                    });
-
+                    bodyConfig.type = 'box';
+                    bodyConfig.size = [sizeX, sizeY, sizeZ];
                     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();
-            }
-
+            
+            var body = new OIMO.Body(bodyConfig);
+
+            //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(mesh.rotationQuaternion.w, mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z);
+            //TEST
+            //body.body.resetQuaternion(new OIMO.Quat(mesh.rotationQuaternion.w, mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z));
+            //update the internal rotation matrix
+            //body.body.syncShapes();
+            
             this._registeredMeshes.push({
                 mesh: mesh,
                 body: body,
                 delta: deltaPosition
             });
+			
+			//for the sake of consistency.
+			mesh.rotationQuaternion = oldQuaternion;
 
             return body;
         }
@@ -119,7 +110,7 @@ module BABYLON {
                 rotations = [];
 
             var initialMesh = parts[0].mesh;
-
+            
             for (var index = 0; index < parts.length; index++) {
                 var part = parts[index];
                 var bodyParameters = this._createBodyAsCompound(part, options, initialMesh);
@@ -138,7 +129,12 @@ module BABYLON {
                 config: [options.mass, options.friction, options.restitution],
                 world: this._world
             });
-
+            
+            //Reset the body's rotation to be of the initial mesh's.
+            var rot = new OIMO.Euler().setFromQuaternion({ x: initialMesh.rotationQuaternion.x, y: initialMesh.rotationQuaternion.y, z: initialMesh.rotationQuaternion.z, s: initialMesh.rotationQuaternion.w });
+			
+			body.resetRotation(rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD);
+            
             this._registeredMeshes.push({
                 mesh: initialMesh,
                 body: body
@@ -148,11 +144,29 @@ module BABYLON {
         }
 
         private _createBodyAsCompound(part: PhysicsCompoundBodyPart, options: PhysicsBodyCreationOptions, initialMesh: AbstractMesh): any {
-            var bodyParameters = null;
             var mesh = part.mesh;
+			
+			if (!mesh.rotationQuaternion) {
+                mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
+            }
+			
             // We need the bounding box/sphere info to compute the physics body
-            mesh.computeWorldMatrix();
+            mesh.computeWorldMatrix(true);
 
+            var rot = new OIMO.Euler().setFromQuaternion({ x: mesh.rotationQuaternion.x, y: mesh.rotationQuaternion.y, z: mesh.rotationQuaternion.z, s: mesh.rotationQuaternion.w });
+            
+            var bodyParameters : any = {
+                pos: [mesh.position.x, mesh.position.y, mesh.position.z],
+                //A bug in Oimo (Body class) prevents us from using rot directly.
+                rot: [0,0,0],
+				//For future reference, if the bug will ever be fixed.
+                realRot: [rot.x / OIMO.TO_RAD, rot.y / OIMO.TO_RAD, rot.z / OIMO.TO_RAD]
+            };
+			
+			var oldQuaternion = mesh.rotationQuaternion;
+			mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
+            mesh.computeWorldMatrix(true);
+            
             switch (part.impostor) {
                 case PhysicsEngine.SphereImpostor:
                     var bbox = mesh.getBoundingInfo().boundingBox;
@@ -164,16 +178,14 @@ module BABYLON {
                         this._checkWithEpsilon(radiusX),
                         this._checkWithEpsilon(radiusY),
                         this._checkWithEpsilon(radiusZ)) / 2;
-                    bodyParameters = {
-                        type: 'sphere',
-                        /* bug with oimo : sphere needs 3 sizes in this case */
-                        size: [size, -1, -1],
-                        pos: [mesh.position.x, mesh.position.y, mesh.position.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD]
-                    };
+                    
+                    bodyParameters.type = 'sphere';
+                    bodyParameters.size = [size, size, size];
+                    
                     break;
 
                 case PhysicsEngine.PlaneImpostor:
+                case PhysicsEngine.CylinderImpostor:
                 case PhysicsEngine.BoxImpostor:
                     bbox = mesh.getBoundingInfo().boundingBox;
                     var min = bbox.minimumWorld;
@@ -182,15 +194,14 @@ module BABYLON {
                     var sizeX = this._checkWithEpsilon(box.x);
                     var sizeY = this._checkWithEpsilon(box.y);
                     var sizeZ = this._checkWithEpsilon(box.z);
-                    var relativePosition = mesh.position;
-                    bodyParameters = {
-                        type: 'box',
-                        size: [sizeX, sizeY, sizeZ],
-                        pos: [relativePosition.x, relativePosition.y, relativePosition.z],
-                        rot: [mesh.rotation.x / OIMO.TO_RAD, mesh.rotation.y / OIMO.TO_RAD, mesh.rotation.z / OIMO.TO_RAD]
-                    };
+                    
+                    bodyParameters.type = 'box';
+                    bodyParameters.size = [sizeX, sizeY, sizeZ];
+                    
                     break;
             }
+			
+			mesh.rotationQuaternion = oldQuaternion;
 
             return bodyParameters;
         }
@@ -232,7 +243,7 @@ module BABYLON {
 
                     var center = mesh.getBoundingInfo().boundingBox.center;
                     body.setPosition(new OIMO.Vec3(center.x, center.y, center.z));
-                    body.setRotation(new OIMO.Vec3(mesh.rotation.x, mesh.rotation.y, mesh.rotation.z));
+                    body.setQuaternion(mesh.rotationQuaternion);
                     body.sleeping = false;
                     return;
                 }
@@ -242,11 +253,10 @@ module BABYLON {
                     registeredMesh.mesh.computeWorldMatrix(true);
 
                     var absolutePosition = registeredMesh.mesh.getAbsolutePosition();
-                    var absoluteRotation = mesh.rotation;
 
                     body = registeredMesh.body.body;
                     body.setPosition(new OIMO.Vec3(absolutePosition.x, absolutePosition.y, absolutePosition.z));
-                    body.setRotation(new OIMO.Vec3(absoluteRotation.x, absoluteRotation.y, absoluteRotation.z));
+                    body.setQuaternion(mesh.rotationQuaternion);
                     body.sleeping = false;
                     return;
                 }
@@ -313,6 +323,20 @@ module BABYLON {
         public isSupported(): boolean {
             return OIMO !== undefined;
         }
+        
+        public getWorldObject() : any {
+            return this._world;
+        }
+        
+        public getPhysicsBodyOfMesh(mesh: AbstractMesh) {
+            for (var index = 0; index < this._registeredMeshes.length; index++) {
+                var registeredMesh = this._registeredMeshes[index];
+                if (registeredMesh.mesh === mesh) {
+                    return registeredMesh.body;
+                }
+            }
+            return null;
+        }
 
         private _getLastShape(body: any): any {
             var lastShape = body.shapes;
@@ -332,48 +356,24 @@ module BABYLON {
 
                 var body = this._registeredMeshes[i].body.body;
                 var mesh = this._registeredMeshes[i].mesh;
-                var delta = this._registeredMeshes[i].delta;
+                
+                if(!this._registeredMeshes[i].delta) {
+                    this._registeredMeshes[i].delta = Vector3.Zero();
+                }
 
                 if (!body.sleeping) {
-
+                    //TODO check that
                     if (body.shapes.next) {
                         var parentShape = this._getLastShape(body);
                         mesh.position.x = parentShape.position.x * OIMO.WORLD_SCALE;
                         mesh.position.y = parentShape.position.y * OIMO.WORLD_SCALE;
                         mesh.position.z = parentShape.position.z * OIMO.WORLD_SCALE;
-                        var mtx = Matrix.FromArray(body.getMatrix());
-
-                        if (!mesh.rotationQuaternion) {
-                            mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
-                        }
-                        mesh.rotationQuaternion.fromRotationMatrix(mtx);
-                        mesh.computeWorldMatrix();
-
                     } else {
-                        m = body.getMatrix();
-                        mtx = Matrix.FromArray(m);
-
-                        // Body position
-                        var bodyX = mtx.m[12],
-                            bodyY = mtx.m[13],
-                            bodyZ = mtx.m[14];
-
-                        if (!delta) {
-                            mesh.position.x = bodyX;
-                            mesh.position.y = bodyY;
-                            mesh.position.z = bodyZ;
-                        } else {
-                            mesh.position.x = bodyX + delta.x;
-                            mesh.position.y = bodyY + delta.y;
-                            mesh.position.z = bodyZ + delta.z;
-                        }
-
-                        if (!mesh.rotationQuaternion) {
-                            mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
-                        }
-                        Quaternion.FromRotationMatrixToRef(mtx, mesh.rotationQuaternion);
-                        mesh.computeWorldMatrix();
+                        mesh.position.copyFrom(body.getPosition()).addInPlace(this._registeredMeshes[i].delta);
+                        
                     }
+                    mesh.rotationQuaternion.copyFrom(body.getQuaternion());
+                    mesh.computeWorldMatrix();
                 }
             }
         }

src/Physics/babylon.physicsEngine.js

@@ -45,6 +45,9 @@ var BABYLON;
         PhysicsEngine.prototype.isSupported = function () {
             return this._currentPlugin.isSupported();
         };
+        PhysicsEngine.prototype.getPhysicsBodyOfMesh = function (mesh) {
+            return this._currentPlugin.getPhysicsBodyOfMesh(mesh);
+        };
         // Statics
         PhysicsEngine.NoImpostor = 0;
         PhysicsEngine.SphereImpostor = 1;

src/Physics/babylon.physicsEngine.ts

@@ -11,6 +11,8 @@
         dispose(): void;
         isSupported(): boolean;
         updateBodyPosition(mesh: AbstractMesh): void;
+        getWorldObject() : any; //Will return the physics world object of the engine used.
+        getPhysicsBodyOfMesh(mesh: AbstractMesh) : any;
     }
 
     export interface PhysicsBodyCreationOptions {
@@ -84,6 +86,10 @@
         public isSupported(): boolean {
             return this._currentPlugin.isSupported();
         }
+        
+        public getPhysicsBodyOfMesh(mesh: AbstractMesh) {
+            return this._currentPlugin.getPhysicsBodyOfMesh(mesh);
+        }
 
         // Statics
         public static NoImpostor = 0;

src/babylon.scene.js

@@ -1810,6 +1810,12 @@ var BABYLON;
         Scene.prototype.getPhysicsEngine = function () {
             return this._physicsEngine;
         };
+        /**
+         * Enables physics to the current scene
+         * @param {BABYLON.Vector3} [gravity] - the scene's gravity for the physics engine
+         * @param {BABYLON.IPhysicsEnginePlugin} [plugin] - The physics engine to be used. defaults to OimoJS.
+         * @return {boolean} was the physics engine initialized
+         */
         Scene.prototype.enablePhysics = function (gravity, plugin) {
             if (this._physicsEngine) {
                 return true;
@@ -1832,6 +1838,10 @@ var BABYLON;
         Scene.prototype.isPhysicsEnabled = function () {
             return this._physicsEngine !== undefined;
         };
+        /**
+         * Sets the gravity of the physics engine (and NOT of the scene)
+         * @param {BABYLON.Vector3} [gravity] - the new gravity to be used
+         */
         Scene.prototype.setGravity = function (gravity) {
             if (!this._physicsEngine) {
                 return;

src/babylon.scene.ts

@@ -2272,7 +2272,13 @@
             return this._physicsEngine;
         }
 
-        public enablePhysics(gravity: Vector3, plugin?: IPhysicsEnginePlugin): boolean {
+        /**
+         * Enables physics to the current scene
+         * @param {BABYLON.Vector3} [gravity] - the scene's gravity for the physics engine
+         * @param {BABYLON.IPhysicsEnginePlugin} [plugin] - The physics engine to be used. defaults to OimoJS.
+         * @return {boolean} was the physics engine initialized
+         */
+        public enablePhysics(gravity?: Vector3, plugin?: IPhysicsEnginePlugin): boolean {
             if (this._physicsEngine) {
                 return true;
             }
@@ -2302,6 +2308,10 @@
             return this._physicsEngine !== undefined;
         }
 
+        /**
+         * Sets the gravity of the physics engine (and NOT of the scene)
+         * @param {BABYLON.Vector3} [gravity] - the new gravity to be used
+         */
         public setGravity(gravity: Vector3): void {
             if (!this._physicsEngine) {
                 return;