Merge pull request #983 from RaananW/PhysicsEngine

Physicsengine

Tools/Gulp/config.json

@@ -85,6 +85,8 @@
       "../../src/PostProcess/babylon.postProcess.js",
       "../../src/PostProcess/babylon.postProcessManager.js",
       "../../src/PostProcess/babylon.passPostProcess.js",
+      "../../src/Physics/babylon.physicsJoint.js",
+      "../../src/Physics/babylon.physicsImpostor.js",
       "../../src/Physics/babylon.physicsEngine.js",
       "../../src/Mesh/babylon.mesh.vertexData.js",
       "../../src/Tools/babylon.tags.js",

Tools/Gulp/gulpfile.js

@@ -128,8 +128,8 @@ gulp.task("build", ["workers", "shaders"], function () {
         gulp.src(config.core.files),
         gulp.src(config.extras.files),
         shadersStream,
-        workersStream, 
-        includeShadersStream
+        includeShadersStream,
+        workersStream
         )
         .pipe(concat(config.build.filename))
         .pipe(cleants())

src/Math/babylon.math.js

@@ -155,7 +155,7 @@ var BABYLON;
         Color3.Yellow = function () { return new Color3(1, 1, 0); };
         Color3.Gray = function () { return new Color3(0.5, 0.5, 0.5); };
         return Color3;
-    })();
+    }());
     BABYLON.Color3 = Color3;
     var Color4 = (function () {
         function Color4(r, g, b, a) {
@@ -276,7 +276,7 @@ var BABYLON;
             return colors;
         };
         return Color4;
-    })();
+    }());
     BABYLON.Color4 = Color4;
     var Vector2 = (function () {
         function Vector2(x, y) {
@@ -464,7 +464,7 @@ var BABYLON;
             return (x * x) + (y * y);
         };
         return Vector2;
-    })();
+    }());
     BABYLON.Vector2 = Vector2;
     var Vector3 = (function () {
         function Vector3(x, y, z) {
@@ -963,7 +963,7 @@ var BABYLON;
             ref.z = roll;
         };
         return Vector3;
-    })();
+    }());
     BABYLON.Vector3 = Vector3;
     //Vector4 class created for EulerAngle class conversion to Quaternion
     var Vector4 = (function () {
@@ -1220,7 +1220,7 @@ var BABYLON;
             return center;
         };
         return Vector4;
-    })();
+    }());
     BABYLON.Vector4 = Vector4;
     var Quaternion = (function () {
         function Quaternion(x, y, z, w) {
@@ -1492,7 +1492,7 @@ var BABYLON;
             return new Quaternion((num3 * left.x) + (num2 * right.x), (num3 * left.y) + (num2 * right.y), (num3 * left.z) + (num2 * right.z), (num3 * left.w) + (num2 * right.w));
         };
         return Quaternion;
-    })();
+    }());
     BABYLON.Quaternion = Quaternion;
     var Matrix = (function () {
         function Matrix() {
@@ -2127,7 +2127,7 @@ var BABYLON;
         Matrix._yAxis = Vector3.Zero();
         Matrix._zAxis = Vector3.Zero();
         return Matrix;
-    })();
+    }());
     BABYLON.Matrix = Matrix;
     var Plane = (function () {
         function Plane(a, b, c, d) {
@@ -2220,7 +2220,7 @@ var BABYLON;
             return Vector3.Dot(point, normal) + d;
         };
         return Plane;
-    })();
+    }());
     BABYLON.Plane = Plane;
     var Viewport = (function () {
         function Viewport(x, y, width, height) {
@@ -2240,7 +2240,7 @@ var BABYLON;
             return new Viewport(this.x * width, this.y * height, this.width * width, this.height * height);
         };
         return Viewport;
-    })();
+    }());
     BABYLON.Viewport = Viewport;
     var Frustum = (function () {
         function Frustum() {
@@ -2292,7 +2292,7 @@ var BABYLON;
             frustumPlanes[5].normalize();
         };
         return Frustum;
-    })();
+    }());
     BABYLON.Frustum = Frustum;
     var Ray = (function () {
         function Ray(origin, direction, length) {
@@ -2460,7 +2460,7 @@ var BABYLON;
             return new Ray(newOrigin, newDirection, ray.length);
         };
         return Ray;
-    })();
+    }());
     BABYLON.Ray = Ray;
     (function (Space) {
         Space[Space["LOCAL"] = 0] = "LOCAL";
@@ -2474,7 +2474,7 @@ var BABYLON;
         Axis.Y = new Vector3(0, 1, 0);
         Axis.Z = new Vector3(0, 0, 1);
         return Axis;
-    })();
+    }());
     BABYLON.Axis = Axis;
     ;
     var BezierCurve = (function () {
@@ -2500,7 +2500,7 @@ var BABYLON;
                 Math.pow(refinedT, 3);
         };
         return BezierCurve;
-    })();
+    }());
     BABYLON.BezierCurve = BezierCurve;
     (function (Orientation) {
         Orientation[Orientation["CW"] = 0] = "CW";
@@ -2528,7 +2528,7 @@ var BABYLON;
             return new Angle(degrees * Math.PI / 180);
         };
         return Angle;
-    })();
+    }());
     BABYLON.Angle = Angle;
     var Arc2 = (function () {
         function Arc2(startPoint, midPoint, endPoint) {
@@ -2558,7 +2558,7 @@ var BABYLON;
             this.angle = Angle.FromDegrees(this.orientation === Orientation.CW ? a1 - a3 : a3 - a1);
         }
         return Arc2;
-    })();
+    }());
     BABYLON.Arc2 = Arc2;
     var PathCursor = (function () {
         function PathCursor(path) {
@@ -2615,7 +2615,7 @@ var BABYLON;
             return this;
         };
         return PathCursor;
-    })();
+    }());
     BABYLON.PathCursor = PathCursor;
     var Path2 = (function () {
         function Path2(x, y) {
@@ -2700,7 +2700,7 @@ var BABYLON;
             return new Path2(x, y);
         };
         return Path2;
-    })();
+    }());
     BABYLON.Path2 = Path2;
     var Path3D = (function () {
         /**
@@ -2847,7 +2847,7 @@ var BABYLON;
             return normal0;
         };
         return Path3D;
-    })();
+    }());
     BABYLON.Path3D = Path3D;
     var Curve3 = (function () {
         function Curve3(points) {
@@ -2914,7 +2914,7 @@ var BABYLON;
             return l;
         };
         return Curve3;
-    })();
+    }());
     BABYLON.Curve3 = Curve3;
     // SphericalHarmonics
     var SphericalHarmonics = (function () {
@@ -2954,7 +2954,7 @@ var BABYLON;
             this.L22 = this.L22.scale(scale);
         };
         return SphericalHarmonics;
-    })();
+    }());
     BABYLON.SphericalHarmonics = SphericalHarmonics;
     // SphericalPolynomial
     var SphericalPolynomial = (function () {
@@ -2989,7 +2989,7 @@ var BABYLON;
             return result;
         };
         return SphericalPolynomial;
-    })();
+    }());
     BABYLON.SphericalPolynomial = SphericalPolynomial;
     // Vertex formats
     var PositionNormalVertex = (function () {
@@ -3003,7 +3003,7 @@ var BABYLON;
             return new PositionNormalVertex(this.position.clone(), this.normal.clone());
         };
         return PositionNormalVertex;
-    })();
+    }());
     BABYLON.PositionNormalVertex = PositionNormalVertex;
     var PositionNormalTextureVertex = (function () {
         function PositionNormalTextureVertex(position, normal, uv) {
@@ -3018,7 +3018,7 @@ var BABYLON;
             return new PositionNormalTextureVertex(this.position.clone(), this.normal.clone(), this.uv.clone());
         };
         return PositionNormalTextureVertex;
-    })();
+    }());
     BABYLON.PositionNormalTextureVertex = PositionNormalTextureVertex;
     // Temporary pre-allocated objects for engine internal use
     // usage in any internal function :
@@ -3038,6 +3038,6 @@ var BABYLON;
             Matrix.Zero(), Matrix.Zero(),
             Matrix.Zero(), Matrix.Zero()]; // 6 temp Matrices at once should be enough
         return Tmp;
-    })();
+    }());
     BABYLON.Tmp = Tmp;
 })(BABYLON || (BABYLON = {}));

src/Math/babylon.math.ts

@@ -591,6 +591,7 @@
     export class Vector3 {
 
         constructor(public x: number, public y: number, public z: number) {
+            
         }
 
         public toString(): string {
@@ -1520,7 +1521,6 @@
 
     export class Quaternion {
         constructor(public x: number = 0, public y: number = 0, public z: number = 0, public w: number = 1) {
-
         }
 
         public toString(): string {

src/Mesh/babylon.abstractMesh.js

@@ -14,8 +14,8 @@ var BABYLON;
             // Properties
             this.definedFacingForward = true; // orientation for POV movement & rotation
             this.position = new BABYLON.Vector3(0, 0, 0);
-            this.rotation = new BABYLON.Vector3(0, 0, 0);
-            this.scaling = new BABYLON.Vector3(1, 1, 1);
+            this._rotation = new BABYLON.Vector3(0, 0, 0);
+            this._scaling = new BABYLON.Vector3(1, 1, 1);
             this.billboardMode = AbstractMesh.BILLBOARDMODE_NONE;
             this.visibility = 1.0;
             this.alphaIndex = Number.MAX_VALUE;
@@ -45,8 +45,6 @@ var BABYLON;
             this.useOctreeForCollisions = true;
             this.layerMask = 0x0FFFFFFF;
             this.alwaysSelectAsActiveMesh = false;
-            // Physics
-            this._physicImpostor = BABYLON.PhysicsEngine.NoImpostor;
             // Collisions
             this._checkCollisions = false;
             this.ellipsoid = new BABYLON.Vector3(0.5, 1, 0.5);
@@ -145,6 +143,47 @@ var BABYLON;
             enumerable: true,
             configurable: true
         });
+        Object.defineProperty(AbstractMesh.prototype, "rotation", {
+            /**
+             * Getting the rotation object.
+             * If rotation quaternion is set, this vector will (almost always) be the Zero vector!
+             */
+            get: function () {
+                return this._rotation;
+            },
+            set: function (newRotation) {
+                this._rotation = newRotation;
+            },
+            enumerable: true,
+            configurable: true
+        });
+        Object.defineProperty(AbstractMesh.prototype, "scaling", {
+            get: function () {
+                return this._scaling;
+            },
+            set: function (newScaling) {
+                this._scaling = newScaling;
+                if (this.physicsImpostor) {
+                    this.physicsImpostor.forceUpdate();
+                }
+            },
+            enumerable: true,
+            configurable: true
+        });
+        Object.defineProperty(AbstractMesh.prototype, "rotationQuaternion", {
+            get: function () {
+                return this._rotationQuaternion;
+            },
+            set: function (quaternion) {
+                this._rotationQuaternion = quaternion;
+                //reset the rotation vector. 
+                if (this.rotation.length()) {
+                    this.rotation.copyFromFloats(0, 0, 0);
+                }
+            },
+            enumerable: true,
+            configurable: true
+        });
         // Methods
         AbstractMesh.prototype.updatePoseMatrix = function (matrix) {
             this._poseMatrix.copyFrom(matrix);
@@ -446,6 +485,14 @@ var BABYLON;
             // Scaling
             BABYLON.Matrix.ScalingToRef(this.scaling.x * this.scalingDeterminant, this.scaling.y * this.scalingDeterminant, this.scaling.z * this.scalingDeterminant, BABYLON.Tmp.Matrix[1]);
             // Rotation
+            //rotate, if quaternion is set and rotation was used
+            if (this.rotationQuaternion) {
+                var len = this.rotation.length();
+                if (len) {
+                    this.rotationQuaternion.multiplyInPlace(BABYLON.Quaternion.RotationYawPitchRoll(this.rotation.y, this.rotation.x, this.rotation.z));
+                    this.rotation.copyFromFloats(0, 0, 0);
+                }
+            }
             if (this.rotationQuaternion) {
                 this.rotationQuaternion.toRotationMatrix(BABYLON.Tmp.Matrix[0]);
                 this._cache.rotationQuaternion.copyFrom(this.rotationQuaternion);
@@ -602,62 +649,67 @@ var BABYLON;
             }
             return this._boundingInfo.intersectsPoint(point);
         };
+        AbstractMesh.prototype.getChildMeshes = function () {
+            var _this = this;
+            return this.getScene().meshes.filter(function (m) {
+                return m.parent === _this;
+            });
+        };
+        AbstractMesh.prototype.getChildren = function () {
+            var results = [];
+            for (var index = 0; index < this.getScene().meshes.length; index++) {
+                var mesh = this.getScene().meshes[index];
+                if (mesh.parent === this) {
+                    results.push(mesh);
+                }
+            }
+            for (var index = 0; index < this.getScene().lights.length; index++) {
+                var light = this.getScene().lights[index];
+                if (light.parent === this) {
+                    results.push(mesh);
+                }
+            }
+            for (var index = 0; index < this.getScene().cameras.length; index++) {
+                var camera = this.getScene().cameras[index];
+                if (camera.parent === this) {
+                    results.push(mesh);
+                }
+            }
+            return results;
+        };
         // Physics
+        /**
+         *  @Deprecated. Use new PhysicsImpostor instead.
+         * */
         AbstractMesh.prototype.setPhysicsState = function (impostor, options) {
-            var physicsEngine = this.getScene().getPhysicsEngine();
-            if (!physicsEngine) {
-                return null;
-            }
-            impostor = impostor || BABYLON.PhysicsEngine.NoImpostor;
+            //legacy support
             if (impostor.impostor) {
-                // Old API
                 options = impostor;
                 impostor = impostor.impostor;
             }
-            if (impostor === BABYLON.PhysicsEngine.NoImpostor) {
-                physicsEngine._unregisterMesh(this);
-                return null;
-            }
-            if (!options) {
-                options = { mass: 0, friction: 0.2, restitution: 0.2 };
-            }
-            else {
-                if (!options.mass && options.mass !== 0)
-                    options.mass = 0;
-                if (!options.friction && options.friction !== 0)
-                    options.friction = 0.2;
-                if (!options.restitution && options.restitution !== 0)
-                    options.restitution = 0.2;
-            }
-            this._physicImpostor = impostor;
-            this._physicsMass = options.mass;
-            this._physicsFriction = options.friction;
-            this._physicRestitution = options.restitution;
-            return physicsEngine._registerMesh(this, impostor, options);
+            this.physicsImpostor = new BABYLON.PhysicsImpostor(this, impostor, options);
+            return this.physicsImpostor.physicsBody;
         };
         AbstractMesh.prototype.getPhysicsImpostor = function () {
-            if (!this._physicImpostor) {
-                return BABYLON.PhysicsEngine.NoImpostor;
-            }
-            return this._physicImpostor;
+            return this.physicsImpostor;
         };
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("mass");
+         */
         AbstractMesh.prototype.getPhysicsMass = function () {
-            if (!this._physicsMass) {
-                return 0;
-            }
-            return this._physicsMass;
+            return this.physicsImpostor.getParam("mass");
         };
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("friction");
+         */
         AbstractMesh.prototype.getPhysicsFriction = function () {
-            if (!this._physicsFriction) {
-                return 0;
-            }
-            return this._physicsFriction;
+            return this.physicsImpostor.getParam("friction");
         };
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("restitution");
+         */
         AbstractMesh.prototype.getPhysicsRestitution = function () {
-            if (!this._physicRestitution) {
-                return 0;
-            }
-            return this._physicRestitution;
+            return this.physicsImpostor.getParam("resitution");
         };
         AbstractMesh.prototype.getPositionInCameraSpace = function (camera) {
             if (!camera) {
@@ -672,26 +724,35 @@ var BABYLON;
             return this.absolutePosition.subtract(camera.position).length();
         };
         AbstractMesh.prototype.applyImpulse = function (force, contactPoint) {
-            if (!this._physicImpostor) {
+            if (!this.physicsImpostor) {
                 return;
             }
-            this.getScene().getPhysicsEngine()._applyImpulse(this, force, contactPoint);
+            this.physicsImpostor.applyImpulse(force, contactPoint);
         };
         AbstractMesh.prototype.setPhysicsLinkWith = function (otherMesh, pivot1, pivot2, options) {
-            if (!this._physicImpostor) {
+            if (!this.physicsImpostor || !otherMesh.physicsImpostor) {
                 return;
             }
-            this.getScene().getPhysicsEngine()._createLink(this, otherMesh, pivot1, pivot2, options);
+            this.physicsImpostor.createJoint(otherMesh.physicsImpostor, BABYLON.PhysicsJoint.HingeJoint, {
+                mainPivot: pivot1,
+                connectedPivot: pivot2,
+                nativeParams: options
+            });
         };
+        /**
+         * @Deprecated
+         */
         AbstractMesh.prototype.updatePhysicsBodyPosition = function () {
             BABYLON.Tools.Warn("updatePhysicsBodyPosition() is deprecated, please use updatePhysicsBody()");
             this.updatePhysicsBody();
         };
+        /**
+         * @Deprecated
+         * Calling this function is not needed anymore.
+         * The physics engine takes care of transofmration automatically.
+         */
         AbstractMesh.prototype.updatePhysicsBody = function () {
-            if (!this._physicImpostor) {
-                return;
-            }
-            this.getScene().getPhysicsEngine()._updateBodyPosition(this);
+            //Unneeded
         };
         Object.defineProperty(AbstractMesh.prototype, "checkCollisions", {
             // Collisions
@@ -870,8 +931,8 @@ var BABYLON;
             // Animations
             this.getScene().stopAnimation(this);
             // Physics
-            if (this.getPhysicsImpostor() !== BABYLON.PhysicsEngine.NoImpostor) {
-                this.setPhysicsState(BABYLON.PhysicsEngine.NoImpostor);
+            if (this.physicsImpostor) {
+                this.physicsImpostor.dispose(!doNotRecurse);
             }
             // Intersections in progress
             for (index = 0; index < this._intersectionsInProgress.length; index++) {
@@ -928,6 +989,6 @@ var BABYLON;
         AbstractMesh._BILLBOARDMODE_Z = 4;
         AbstractMesh._BILLBOARDMODE_ALL = 7;
         return AbstractMesh;
-    })(BABYLON.Node);
+    }(BABYLON.Node));
     BABYLON.AbstractMesh = AbstractMesh;
 })(BABYLON || (BABYLON = {}));

src/Mesh/babylon.abstractMesh.ts

@@ -30,9 +30,9 @@
         // Properties
         public definedFacingForward = true; // orientation for POV movement & rotation
         public position = new Vector3(0, 0, 0);
-        public rotation = new Vector3(0, 0, 0);
-        public rotationQuaternion: Quaternion;
-        public scaling = new Vector3(1, 1, 1);
+        private _rotation = new Vector3(0, 0, 0);
+        public _rotationQuaternion: Quaternion;
+        private _scaling = new Vector3(1, 1, 1);
         public billboardMode = AbstractMesh.BILLBOARDMODE_NONE;
         public visibility = 1.0;
         public alphaIndex = Number.MAX_VALUE;
@@ -69,10 +69,8 @@
         public alwaysSelectAsActiveMesh = false;
 
         // Physics
-        public _physicImpostor = PhysicsEngine.NoImpostor;
-        public _physicsMass: number;
-        public _physicsFriction: number;
-        public _physicRestitution: number;
+        public physicsImpostor: BABYLON.PhysicsImpostor;
+        //Deprecated, Legacy support
         public onPhysicsCollide: (collidedMesh: AbstractMesh, contact: any) => void; 
 
         // Collisions
@@ -158,6 +156,41 @@
             scene.addMesh(this);
         }
 
+        /**
+         * Getting the rotation object. 
+         * If rotation quaternion is set, this vector will (almost always) be the Zero vector!
+         */
+        public get rotation(): Vector3 {
+            return this._rotation;
+        }
+
+        public set rotation(newRotation: Vector3) {
+            this._rotation = newRotation;
+        }
+
+        public get scaling(): Vector3 {
+            return this._scaling;
+        }
+
+        public set scaling(newScaling: Vector3) {
+            this._scaling = newScaling;
+            if (this.physicsImpostor) {
+                this.physicsImpostor.forceUpdate();
+            }
+        }
+
+        public get rotationQuaternion() {
+            return this._rotationQuaternion;
+        }
+
+        public set rotationQuaternion(quaternion: Quaternion) {
+            this._rotationQuaternion = quaternion;
+            //reset the rotation vector. 
+            if (this.rotation.length()) {
+                this.rotation.copyFromFloats(0, 0, 0);
+            }
+        }
+
         // Methods
         public updatePoseMatrix(matrix: Matrix) {
             this._poseMatrix.copyFrom(matrix);
@@ -495,6 +528,16 @@
             Matrix.ScalingToRef(this.scaling.x * this.scalingDeterminant, this.scaling.y * this.scalingDeterminant, this.scaling.z * this.scalingDeterminant, Tmp.Matrix[1]);
 
             // Rotation
+            
+            //rotate, if quaternion is set and rotation was used
+            if (this.rotationQuaternion) {
+                var len = this.rotation.length();
+                if (len) {
+                    this.rotationQuaternion.multiplyInPlace(BABYLON.Quaternion.RotationYawPitchRoll(this.rotation.y, this.rotation.x, this.rotation.z))
+                    this.rotation.copyFromFloats(0, 0, 0);
+                }
+            }
+
             if (this.rotationQuaternion) {
                 this.rotationQuaternion.toRotationMatrix(Tmp.Matrix[0]);
                 this._cache.rotationQuaternion.copyFrom(this.rotationQuaternion);
@@ -692,74 +735,75 @@
             return this._boundingInfo.intersectsPoint(point);
         }
 
-        // Physics
-        public setPhysicsState(impostor?: any, options?: PhysicsBodyCreationOptions): any {
-            var physicsEngine = this.getScene().getPhysicsEngine();
-
-            if (!physicsEngine) {
-                return null;
-            }
-
-            impostor = impostor || PhysicsEngine.NoImpostor;
+        public getChildMeshes(): AbstractMesh[] {
+            return this.getScene().meshes.filter((m) => {
+                return m.parent === this;
+            });
+        }
 
-            if (impostor.impostor) {
-                // Old API
-                options = impostor;
-                impostor = impostor.impostor;
+        public getChildren(): Node[] {
+            var results = [];
+            for (var index = 0; index < this.getScene().meshes.length; index++) {
+                var mesh = this.getScene().meshes[index];
+                if (mesh.parent === this) {
+                    results.push(mesh);
+                }
             }
 
-            if (impostor === PhysicsEngine.NoImpostor) {
-                physicsEngine._unregisterMesh(this);
-                return null;
+            for (var index = 0; index < this.getScene().lights.length; index++) {
+                var light = this.getScene().lights[index];
+                if (light.parent === this) {
+                    results.push(mesh);
+                }
             }
 
-            if (!options) {
-                options = { mass: 0, friction: 0.2, restitution: 0.2 };
-            } else {
-                if (!options.mass && options.mass !== 0) options.mass = 0;
-                if (!options.friction && options.friction !== 0) options.friction = 0.2;
-                if (!options.restitution && options.restitution !== 0) options.restitution = 0.2;
+            for (var index = 0; index < this.getScene().cameras.length; index++) {
+                var camera = this.getScene().cameras[index];
+                if (camera.parent === this) {
+                    results.push(mesh);
+                }
             }
 
-            this._physicImpostor = impostor;
-            this._physicsMass = options.mass;
-            this._physicsFriction = options.friction;
-            this._physicRestitution = options.restitution;
-
-
-            return physicsEngine._registerMesh(this, impostor, options);
+            return results;
         }
 
-        public getPhysicsImpostor(): number {
-            if (!this._physicImpostor) {
-                return PhysicsEngine.NoImpostor;
+        // Physics
+        /**
+         *  @Deprecated. Use new PhysicsImpostor instead.
+         * */
+        public setPhysicsState(impostor?: any, options?: PhysicsImpostorParameters): any {
+            //legacy support
+            if (impostor.impostor) {
+                options = impostor;
+                impostor = impostor.impostor;
             }
+            this.physicsImpostor = new PhysicsImpostor(this, impostor, options);
+            return this.physicsImpostor.physicsBody;
+        }
 
-            return this._physicImpostor;
+        public getPhysicsImpostor(): PhysicsImpostor {
+            return this.physicsImpostor;
         }
 
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("mass");
+         */
         public getPhysicsMass(): number {
-            if (!this._physicsMass) {
-                return 0;
-            }
-
-            return this._physicsMass;
+            return this.physicsImpostor.getParam("mass")
         }
 
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("friction");
+         */
         public getPhysicsFriction(): number {
-            if (!this._physicsFriction) {
-                return 0;
-            }
-
-            return this._physicsFriction;
+            return this.physicsImpostor.getParam("friction")
         }
 
+        /**
+         * @Deprecated. Use getPhysicsImpostor().getParam("restitution");
+         */
         public getPhysicsRestitution(): number {
-            if (!this._physicRestitution) {
-                return 0;
-            }
-
-            return this._physicRestitution;
+            return this.physicsImpostor.getParam("resitution")
         }
 
         public getPositionInCameraSpace(camera?: Camera): Vector3 {
@@ -779,31 +823,40 @@
         }
 
         public applyImpulse(force: Vector3, contactPoint: Vector3): void {
-            if (!this._physicImpostor) {
+            if (!this.physicsImpostor) {
                 return;
             }
 
-            this.getScene().getPhysicsEngine()._applyImpulse(this, force, contactPoint);
+            this.physicsImpostor.applyImpulse(force, contactPoint);
         }
 
         public setPhysicsLinkWith(otherMesh: Mesh, pivot1: Vector3, pivot2: Vector3, options?: any): void {
-            if (!this._physicImpostor) {
+            if (!this.physicsImpostor || !otherMesh.physicsImpostor) {
                 return;
             }
 
-            this.getScene().getPhysicsEngine()._createLink(this, otherMesh, pivot1, pivot2, options);
+            this.physicsImpostor.createJoint(otherMesh.physicsImpostor, PhysicsJoint.HingeJoint, {
+                mainPivot: pivot1,
+                connectedPivot: pivot2,
+                nativeParams: options
+            })
         }
 
+        /**
+         * @Deprecated
+         */
         public updatePhysicsBodyPosition(): void {
             Tools.Warn("updatePhysicsBodyPosition() is deprecated, please use updatePhysicsBody()")
             this.updatePhysicsBody();
         }
 
+        /**
+         * @Deprecated
+         * Calling this function is not needed anymore. 
+         * The physics engine takes care of transofmration automatically.
+         */
         public updatePhysicsBody(): void {
-            if (!this._physicImpostor) {
-                return;
-            }
-            this.getScene().getPhysicsEngine()._updateBodyPosition(this);
+            //Unneeded
         }
 
 
@@ -1039,8 +1092,8 @@
             this.getScene().stopAnimation(this);
 
             // Physics
-            if (this.getPhysicsImpostor() !== PhysicsEngine.NoImpostor) {
-                this.setPhysicsState(PhysicsEngine.NoImpostor);
+            if (this.physicsImpostor) {
+                this.physicsImpostor.dispose(!doNotRecurse);
             }
 
             // Intersections in progress

src/Mesh/babylon.mesh.js

@@ -12,7 +12,7 @@ var BABYLON;
             this.renderSelf = new Array();
         }
         return _InstancesBatch;
-    })();
+    }());
     BABYLON._InstancesBatch = _InstancesBatch;
     var Mesh = (function (_super) {
         __extends(Mesh, _super);
@@ -715,16 +715,6 @@ var BABYLON;
             }
             return results;
         };
-        Mesh.prototype.getChildren = function () {
-            var results = [];
-            for (var index = 0; index < this.getScene().meshes.length; index++) {
-                var mesh = this.getScene().meshes[index];
-                if (mesh.parent === this) {
-                    results.push(mesh);
-                }
-            }
-            return results;
-        };
         Mesh.prototype._checkDelayState = function () {
             var _this = this;
             var that = this;
@@ -1246,13 +1236,6 @@ var BABYLON;
                     mesh.numBoneInfluencers = parsedMesh.numBoneInfluencers;
                 }
             }
-            // Physics
-            if (parsedMesh.physicsImpostor) {
-                if (!scene.isPhysicsEnabled()) {
-                    scene.enablePhysics();
-                }
-                mesh.setPhysicsState({ impostor: parsedMesh.physicsImpostor, mass: parsedMesh.physicsMass, friction: parsedMesh.physicsFriction, restitution: parsedMesh.physicsRestitution });
-            }
             // Animations
             if (parsedMesh.animations) {
                 for (var animationIndex = 0; animationIndex < parsedMesh.animations.length; animationIndex++) {
@@ -1719,6 +1702,6 @@ var BABYLON;
         Mesh._CAP_END = 2;
         Mesh._CAP_ALL = 3;
         return Mesh;
-    })(BABYLON.AbstractMesh);
+    }(BABYLON.AbstractMesh));
     BABYLON.Mesh = Mesh;
 })(BABYLON || (BABYLON = {}));

src/Mesh/babylon.mesh.ts

@@ -724,6 +724,7 @@
             if (!this._geometry || !this._geometry.getVertexBuffers() || !this._geometry.getIndexBuffer()) {
                 return;
             }
+
             var callbackIndex: number;
             for (callbackIndex = 0; callbackIndex < this._onBeforeRenderCallbacks.length; callbackIndex++) {
                 this._onBeforeRenderCallbacks[callbackIndex](this);
@@ -822,18 +823,6 @@
             return results;
         }
 
-        public getChildren(): Node[] {
-            var results = [];
-            for (var index = 0; index < this.getScene().meshes.length; index++) {
-                var mesh = this.getScene().meshes[index];
-                if (mesh.parent === this) {
-                    results.push(mesh);
-                }
-            }
-
-            return results;
-        }
-
         public _checkDelayState(): void {
             var that = this;
             var scene = this.getScene();
@@ -1472,15 +1461,6 @@
                 }
             }
 
-            // Physics
-            if (parsedMesh.physicsImpostor) {
-                if (!scene.isPhysicsEnabled()) {
-                    scene.enablePhysics();
-                }
-
-                mesh.setPhysicsState({ impostor: parsedMesh.physicsImpostor, mass: parsedMesh.physicsMass, friction: parsedMesh.physicsFriction, restitution: parsedMesh.physicsRestitution });
-            }
-
             // Animations
             if (parsedMesh.animations) {
                 for (var animationIndex = 0; animationIndex < parsedMesh.animations.length; animationIndex++) {

src/Physics/Plugins/babylon.cannonJSPlugin.js

@@ -1,124 +1,206 @@
 var BABYLON;
 (function (BABYLON) {
     var CannonJSPlugin = (function () {
-        function CannonJSPlugin(_useDeltaForWorldStep) {
+        function CannonJSPlugin(_useDeltaForWorldStep, iterations) {
             if (_useDeltaForWorldStep === void 0) { _useDeltaForWorldStep = true; }
+            if (iterations === void 0) { iterations = 10; }
             this._useDeltaForWorldStep = _useDeltaForWorldStep;
-            this._registeredMeshes = [];
+            this.name = "CannonJSPlugin";
             this._physicsMaterials = [];
             this._fixedTimeStep = 1 / 60;
-            //private _maxSubSteps : number = 15;
-            this.name = "CannonJS";
-            this.updateBodyPosition = function (mesh) {
-                for (var index = 0; index < this._registeredMeshes.length; index++) {
-                    var registeredMesh = this._registeredMeshes[index];
-                    if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                        var body = registeredMesh.body;
-                        var center = mesh.getBoundingInfo().boundingBox.center.clone();
-                        body.quaternion.copy(mesh.rotationQuaternion);
-                        if (registeredMesh.deltaRotation) {
-                            var tmpQ = new CANNON.Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
-                            body.quaternion = body.quaternion.mult(tmpQ);
-                        }
-                        if (registeredMesh.type === CANNON.Shape.types.HEIGHTFIELD) {
-                            //calculate the correct body position:
-                            var rotationQuaternion = mesh.rotationQuaternion;
-                            mesh.rotationQuaternion = new BABYLON.Quaternion();
-                            mesh.computeWorldMatrix(true);
-                            //get original center with no rotation
-                            var center = mesh.getBoundingInfo().boundingBox.center.clone();
-                            var oldPivot = mesh.getPivotMatrix() || BABYLON.Matrix.Translation(0, 0, 0);
-                            //rotation is back
-                            mesh.rotationQuaternion = rotationQuaternion;
-                            //calculate the new center using a pivot (since Cannon.js doesn't center height maps)
-                            var p = BABYLON.Matrix.Translation(mesh.getBoundingInfo().boundingBox.extendSize.x, 0, -mesh.getBoundingInfo().boundingBox.extendSize.z);
-                            mesh.setPivotMatrix(p);
-                            mesh.computeWorldMatrix(true);
-                            //calculate the translation
-                            var translation = mesh.getBoundingInfo().boundingBox.center.subtract(center).subtract(mesh.position).negate();
-                            center.copyFromFloats(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
-                            //add it inverted to the delta 
-                            registeredMesh.delta = mesh.getBoundingInfo().boundingBox.center.subtract(center);
-                            registeredMesh.delta.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
-                            mesh.setPivotMatrix(oldPivot);
-                            mesh.computeWorldMatrix(true);
-                        }
-                        else if (registeredMesh.type === CANNON.Shape.types.TRIMESH) {
-                            center.copyFromFloats(mesh.position.x, mesh.position.y, mesh.position.z);
-                        }
-                        body.position.set(center.x, center.y, center.z);
-                        return;
-                    }
-                }
-            };
+            //See https://github.com/schteppe/cannon.js/blob/gh-pages/demos/collisionFilter.html
+            this._currentCollisionGroup = 2;
+            this._minus90X = new BABYLON.Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
+            this._plus90X = new BABYLON.Quaternion(0.7071067811865475, 0, 0, 0.7071067811865475);
+            this._tmpPosition = BABYLON.Vector3.Zero();
+            this._tmpQuaternion = new BABYLON.Quaternion();
+            this._tmpDeltaPosition = BABYLON.Vector3.Zero();
+            this._tmpDeltaRotation = new BABYLON.Quaternion();
+            this._tmpUnityRotation = new BABYLON.Quaternion();
+            if (!this.isSupported()) {
+                BABYLON.Tools.Error("CannonJS is not available. Please make sure you included the js file.");
+                return;
+            }
+            this.world = new CANNON.World();
+            this.world.broadphase = new CANNON.NaiveBroadphase();
+            this.world.solver.iterations = iterations;
         }
-        CannonJSPlugin.prototype.initialize = function (iterations) {
-            if (iterations === void 0) { iterations = 10; }
-            this._world = new CANNON.World();
-            this._world.broadphase = new CANNON.NaiveBroadphase();
-            this._world.solver.iterations = iterations;
+        CannonJSPlugin.prototype.setGravity = function (gravity) {
+            this.world.gravity.copy(gravity);
         };
-        CannonJSPlugin.prototype._checkWithEpsilon = function (value) {
-            return value < BABYLON.PhysicsEngine.Epsilon ? BABYLON.PhysicsEngine.Epsilon : value;
+        CannonJSPlugin.prototype.executeStep = function (delta, impostors) {
+            this.world.step(this._fixedTimeStep, this._useDeltaForWorldStep ? delta * 1000 : 0);
         };
-        CannonJSPlugin.prototype.runOneStep = function (delta) {
-            var _this = this;
-            this._world.step(this._fixedTimeStep, this._useDeltaForWorldStep ? delta * 1000 : 0);
-            this._registeredMeshes.forEach(function (registeredMesh) {
-                // Body position
-                var bodyX = registeredMesh.body.position.x, bodyY = registeredMesh.body.position.y, bodyZ = registeredMesh.body.position.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.copyFrom(registeredMesh.body.quaternion);
-                if (registeredMesh.deltaRotation) {
-                    registeredMesh.mesh.rotationQuaternion.multiplyInPlace(registeredMesh.deltaRotation);
+        CannonJSPlugin.prototype.applyImpulse = function (impostor, force, contactPoint) {
+            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
+            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
+            impostor.physicsBody.applyImpulse(impulse, worldPoint);
+        };
+        CannonJSPlugin.prototype.applyForce = function (impostor, force, contactPoint) {
+            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
+            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
+            impostor.physicsBody.applyImpulse(impulse, worldPoint);
+        };
+        CannonJSPlugin.prototype.generatePhysicsBody = function (impostor) {
+            //parent-child relationship. Does this impostor has a parent impostor?
+            if (impostor.parent) {
+                if (impostor.physicsBody) {
+                    this.removePhysicsBody(impostor);
+                    //TODO is that needed?
+                    impostor.forceUpdate();
                 }
-                //is the physics collision callback is set?
-                if (registeredMesh.mesh.onPhysicsCollide) {
-                    if (!registeredMesh.collisionFunction) {
-                        registeredMesh.collisionFunction = function (e) {
-                            //find the mesh that collided with the registered mesh
-                            for (var idx = 0; idx < _this._registeredMeshes.length; idx++) {
-                                if (_this._registeredMeshes[idx].body == e.body) {
-                                    registeredMesh.mesh.onPhysicsCollide(_this._registeredMeshes[idx].mesh, e.contact);
-                                }
-                            }
-                        };
-                        registeredMesh.body.addEventListener("collide", registeredMesh.collisionFunction);
-                    }
+                return;
+            }
+            //should a new body be created for this impostor?
+            if (impostor.isBodyInitRequired()) {
+                if (!impostor.mesh.rotationQuaternion) {
+                    impostor.mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(impostor.mesh.rotation.y, impostor.mesh.rotation.x, impostor.mesh.rotation.z);
+                }
+                var shape = this._createShape(impostor);
+                //unregister events, if body is being changed
+                var oldBody = impostor.physicsBody;
+                if (oldBody) {
+                    this.removePhysicsBody(impostor);
                 }
-                else {
-                    //unregister, in case the function was removed for some reason
-                    if (registeredMesh.collisionFunction) {
-                        registeredMesh.body.removeEventListener("collide", registeredMesh.collisionFunction);
+                //create the body and material
+                var material = this._addMaterial(impostor.getOptions().friction, impostor.getOptions().restitution);
+                var bodyCreationObject = {
+                    mass: impostor.getOptions().mass,
+                    material: material
+                };
+                // A simple extend, in case native options were used.
+                var nativeOptions = impostor.getOptions().nativeOptions;
+                for (var key in nativeOptions) {
+                    if (nativeOptions.hasOwnProperty(key)) {
+                        bodyCreationObject[key] = nativeOptions[key];
                     }
                 }
-            });
+                impostor.physicsBody = new CANNON.Body(bodyCreationObject);
+                impostor.physicsBody.addEventListener("collide", impostor.onCollide);
+                this.world.addEventListener("preStep", impostor.beforeStep);
+                this.world.addEventListener("postStep", impostor.afterStep);
+                impostor.physicsBody.addShape(shape);
+                this.world.add(impostor.physicsBody);
+                //try to keep the body moving in the right direction by taking old properties.
+                //Should be tested!
+                if (oldBody) {
+                    ['force', 'torque', 'velocity', 'angularVelocity'].forEach(function (param) {
+                        impostor.physicsBody[param].copy(oldBody[param]);
+                    });
+                }
+                this._processChildMeshes(impostor);
+            }
+            //now update the body's transformation
+            this._updatePhysicsBodyTransformation(impostor);
         };
-        CannonJSPlugin.prototype.setGravity = function (gravity) {
-            this._gravity = gravity;
-            this._world.gravity.set(gravity.x, gravity.y, gravity.z);
+        CannonJSPlugin.prototype._processChildMeshes = function (mainImpostor) {
+            var _this = this;
+            var meshChildren = mainImpostor.mesh.getChildMeshes();
+            if (meshChildren.length) {
+                var processMesh = function (localPosition, mesh) {
+                    var childImpostor = mesh.getPhysicsImpostor();
+                    if (childImpostor) {
+                        var parent = childImpostor.parent;
+                        if (parent !== mainImpostor) {
+                            var localPosition = mesh.position;
+                            if (childImpostor.physicsBody) {
+                                _this.removePhysicsBody(childImpostor);
+                                childImpostor.physicsBody = null;
+                            }
+                            childImpostor.parent = mainImpostor;
+                            childImpostor.resetUpdateFlags();
+                            mainImpostor.physicsBody.addShape(_this._createShape(childImpostor), new CANNON.Vec3(localPosition.x, localPosition.y, localPosition.z));
+                            //Add the mass of the children.
+                            mainImpostor.physicsBody.mass += childImpostor.getParam("mass");
+                        }
+                    }
+                    mesh.getChildMeshes().forEach(processMesh.bind(_this, mesh.position));
+                };
+                meshChildren.forEach(processMesh.bind(this, BABYLON.Vector3.Zero()));
+            }
         };
-        CannonJSPlugin.prototype.getGravity = function () {
-            return this._gravity;
+        CannonJSPlugin.prototype.removePhysicsBody = function (impostor) {
+            impostor.physicsBody.removeEventListener("collide", impostor.onCollide);
+            this.world.removeEventListener("preStep", impostor.beforeStep);
+            this.world.removeEventListener("postStep", impostor.afterStep);
+            this.world.remove(impostor.physicsBody);
         };
-        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);
+        CannonJSPlugin.prototype.generateJoint = function (impostorJoint) {
+            var mainBody = impostorJoint.mainImpostor.physicsBody;
+            var connectedBody = impostorJoint.connectedImpostor.physicsBody;
+            if (!mainBody || !connectedBody) {
+                return;
             }
-            mesh.computeWorldMatrix(true);
-            var shape = this._createShape(mesh, impostor);
-            return this._createRigidBodyFromShape(shape, mesh, options);
+            var constraint;
+            var jointData = impostorJoint.joint.jointData;
+            //TODO - https://github.com/schteppe/cannon.js/blob/gh-pages/demos/collisionFilter.html
+            var constraintData = {
+                pivotA: jointData.mainPivot ? new CANNON.Vec3().copy(jointData.mainPivot) : null,
+                pivotB: jointData.connectedPivot ? new CANNON.Vec3().copy(jointData.connectedPivot) : null,
+                axisA: jointData.mainAxis ? new CANNON.Vec3().copy(jointData.mainAxis) : null,
+                axisB: jointData.connectedAxis ? new CANNON.Vec3().copy(jointData.connectedAxis) : null,
+                maxForce: jointData.nativeParams.maxForce
+            };
+            if (!jointData.collision) {
+                //add 1st body to a collision group of its own, if it is not in 1
+                if (mainBody.collisionFilterGroup === 1) {
+                    mainBody.collisionFilterGroup = this._currentCollisionGroup;
+                    this._currentCollisionGroup <<= 1;
+                }
+                if (connectedBody.collisionFilterGroup === 1) {
+                    connectedBody.collisionFilterGroup = this._currentCollisionGroup;
+                    this._currentCollisionGroup <<= 1;
+                }
+                //add their mask to the collisionFilterMask of each other:
+                connectedBody.collisionFilterMask = connectedBody.collisionFilterMask | ~mainBody.collisionFilterGroup;
+                mainBody.collisionFilterMask = mainBody.collisionFilterMask | ~connectedBody.collisionFilterGroup;
+            }
+            switch (impostorJoint.joint.type) {
+                case BABYLON.PhysicsJoint.HingeJoint:
+                    constraint = new CANNON.HingeConstraint(mainBody, connectedBody, constraintData);
+                    break;
+                case BABYLON.PhysicsJoint.DistanceJoint:
+                    constraint = new CANNON.DistanceConstraint(mainBody, connectedBody, jointData.maxDistance || 2);
+                    break;
+                default:
+                    constraint = new CANNON.PointToPointConstraint(mainBody, constraintData.pivotA, connectedBody, constraintData.pivotA, constraintData.maxForce);
+                    break;
+            }
+            impostorJoint.joint.physicsJoint = constraint;
+            this.world.addConstraint(constraint);
+        };
+        CannonJSPlugin.prototype.removeJoint = function (joint) {
+            //TODO
+        };
+        CannonJSPlugin.prototype._addMaterial = function (friction, restitution) {
+            var index;
+            var mat;
+            for (index = 0; index < this._physicsMaterials.length; index++) {
+                mat = this._physicsMaterials[index];
+                if (mat.friction === friction && mat.restitution === restitution) {
+                    return mat;
+                }
+            }
+            var currentMat = new CANNON.Material("mat");
+            this._physicsMaterials.push(currentMat);
+            for (index = 0; index < this._physicsMaterials.length; index++) {
+                mat = this._physicsMaterials[index];
+                var contactMaterial = new CANNON.ContactMaterial(mat, currentMat, { friction: friction, restitution: restitution });
+                this.world.addContactMaterial(contactMaterial);
+            }
+            return currentMat;
+        };
+        CannonJSPlugin.prototype._checkWithEpsilon = function (value) {
+            return value < BABYLON.PhysicsEngine.Epsilon ? BABYLON.PhysicsEngine.Epsilon : value;
         };
-        CannonJSPlugin.prototype._createShape = function (mesh, impostor) {
+        CannonJSPlugin.prototype._createShape = function (impostor) {
+            var mesh = impostor.mesh;
             //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) {
+            switch (impostor.type) {
                 case BABYLON.PhysicsEngine.SphereImpostor:
                     var bbox = mesh.getBoundingInfo().boundingBox;
                     var radiusX = bbox.maximumWorld.x - bbox.minimumWorld.x;
@@ -137,14 +219,14 @@ var BABYLON;
                     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");
+                    BABYLON.Tools.Warn("Attention, PlaneImposter might not behave as you expect. Consider using BoxImposter instead");
                     returnValue = new CANNON.Plane();
                     break;
                 case BABYLON.PhysicsEngine.MeshImpostor:
                     var rawVerts = mesh.getVerticesData(BABYLON.VertexBuffer.PositionKind);
                     var rawFaces = mesh.getIndices();
                     BABYLON.Tools.Warn("MeshImpostor only collides against spheres.");
-                    returnValue = new CANNON.Trimesh(rawVerts, rawFaces); //this._createConvexPolyhedron(rawVerts, rawFaces, mesh);
+                    returnValue = new CANNON.Trimesh(rawVerts, rawFaces);
                     break;
                 case BABYLON.PhysicsEngine.HeightmapImpostor:
                     returnValue = this._createHeightmap(mesh);
@@ -153,23 +235,6 @@ var BABYLON;
             mesh.rotationQuaternion = oldQuaternion;
             return returnValue;
         };
-        CannonJSPlugin.prototype._createConvexPolyhedron = function (rawVerts, rawFaces, mesh) {
-            var verts = [], faces = [];
-            mesh.computeWorldMatrix(true);
-            //reuse this variable
-            var transformed = BABYLON.Vector3.Zero();
-            // Get vertices
-            for (var i = 0; i < rawVerts.length; i += 3) {
-                BABYLON.Vector3.TransformNormalFromFloatsToRef(rawVerts[i], rawVerts[i + 1], rawVerts[i + 2], mesh.getWorldMatrix(), transformed);
-                verts.push(new CANNON.Vec3(transformed.x, transformed.y, transformed.z));
-            }
-            // Get faces
-            for (var j = 0; j < rawFaces.length; j += 3) {
-                faces.push([rawFaces[j], rawFaces[j + 2], rawFaces[j + 1]]);
-            }
-            var shape = new CANNON.ConvexPolyhedron(verts, faces);
-            return shape;
-        };
         CannonJSPlugin.prototype._createHeightmap = function (mesh, pointDepth) {
             var pos = mesh.getVerticesData(BABYLON.VertexBuffer.PositionKind);
             var matrix = [];
@@ -217,52 +282,28 @@ var BABYLON;
             shape.minY = minY;
             return shape;
         };
-        CannonJSPlugin.prototype._addMaterial = function (friction, restitution) {
-            var index;
-            var mat;
-            for (index = 0; index < this._physicsMaterials.length; index++) {
-                mat = this._physicsMaterials[index];
-                if (mat.friction === friction && mat.restitution === restitution) {
-                    return mat;
-                }
-            }
-            var currentMat = new CANNON.Material("mat");
-            this._physicsMaterials.push(currentMat);
-            for (index = 0; index < this._physicsMaterials.length; index++) {
-                mat = this._physicsMaterials[index];
-                var contactMaterial = new CANNON.ContactMaterial(mat, currentMat, { friction: friction, restitution: restitution });
-                this._world.addContactMaterial(contactMaterial);
-            }
-            return currentMat;
-        };
-        CannonJSPlugin.prototype._createRigidBodyFromShape = function (shape, mesh, options) {
-            if (!mesh.rotationQuaternion) {
-                mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
-            }
+        CannonJSPlugin.prototype._updatePhysicsBodyTransformation = function (impostor) {
+            var mesh = impostor.mesh;
+            //make sure it is updated...
+            impostor.mesh.computeWorldMatrix(true);
             // The delta between the mesh position and the mesh bounding box center
             var bbox = mesh.getBoundingInfo().boundingBox;
-            var deltaPosition = mesh.position.subtract(bbox.center);
-            var deltaRotation;
-            var material = this._addMaterial(options.friction, options.restitution);
-            var body = new CANNON.Body({
-                mass: options.mass,
-                material: material,
-                position: new CANNON.Vec3(bbox.center.x, bbox.center.y, bbox.center.z)
-            });
-            body.quaternion = new CANNON.Quaternion(mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z, mesh.rotationQuaternion.w);
+            this._tmpDeltaPosition.copyFrom(mesh.position.subtract(bbox.center));
+            var quaternion = mesh.rotationQuaternion;
+            this._tmpPosition.copyFrom(mesh.getBoundingInfo().boundingBox.center);
             //is shape is a plane or a heightmap, it must be rotated 90 degs in the X axis.
-            if (shape.type === CANNON.Shape.types.PLANE || shape.type === CANNON.Shape.types.HEIGHTFIELD) {
+            if (impostor.type === BABYLON.PhysicsEngine.PlaneImpostor || impostor.type === BABYLON.PhysicsEngine.HeightmapImpostor) {
                 //-90 DEG in X, precalculated
-                var tmpQ = new CANNON.Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
-                body.quaternion = body.quaternion.mult(tmpQ);
+                quaternion = quaternion.multiply(this._minus90X);
                 //Invert! (Precalculated, 90 deg in X)
-                deltaRotation = new BABYLON.Quaternion(0.7071067811865475, 0, 0, 0.7071067811865475);
+                //No need to clone. this will never change.
+                impostor.setDeltaRotation(this._plus90X);
             }
             //If it is a heightfield, if should be centered.
-            if (shape.type === CANNON.Shape.types.HEIGHTFIELD) {
+            if (impostor.type === BABYLON.PhysicsEngine.HeightmapImpostor) {
                 //calculate the correct body position:
                 var rotationQuaternion = mesh.rotationQuaternion;
-                mesh.rotationQuaternion = new BABYLON.Quaternion();
+                mesh.rotationQuaternion = this._tmpUnityRotation;
                 mesh.computeWorldMatrix(true);
                 //get original center with no rotation
                 var center = mesh.getBoundingInfo().boundingBox.center.clone();
@@ -275,115 +316,40 @@ var BABYLON;
                 mesh.computeWorldMatrix(true);
                 //calculate the translation
                 var translation = mesh.getBoundingInfo().boundingBox.center.subtract(center).subtract(mesh.position).negate();
-                body.position = new CANNON.Vec3(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
+                this._tmpPosition.copyFromFloats(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
                 //add it inverted to the delta 
-                deltaPosition = mesh.getBoundingInfo().boundingBox.center.subtract(center);
-                deltaPosition.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
+                this._tmpDeltaPosition.copyFrom(mesh.getBoundingInfo().boundingBox.center.subtract(center));
+                this._tmpDeltaPosition.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
                 mesh.setPivotMatrix(oldPivot);
                 mesh.computeWorldMatrix(true);
             }
-            else if (shape.type === CANNON.Shape.types.TRIMESH) {
-                deltaPosition = BABYLON.Vector3.Zero();
+            else if (impostor.type === BABYLON.PhysicsEngine.MeshImpostor) {
+                this._tmpDeltaPosition.copyFromFloats(0, 0, 0);
+                this._tmpPosition.copyFrom(mesh.position);
             }
-            //add the shape
-            body.addShape(shape);
-            this._world.add(body);
-            this._registeredMeshes.push({ mesh: mesh, body: body, material: material, delta: deltaPosition, deltaRotation: deltaRotation, type: shape.type });
-            return body;
+            impostor.setDeltaPosition(this._tmpDeltaPosition);
+            //Now update the impostor object
+            impostor.physicsBody.position.copy(this._tmpPosition);
+            impostor.physicsBody.quaternion.copy(quaternion);
         };
-        CannonJSPlugin.prototype.registerMeshesAsCompound = function (parts, options) {
-            var initialMesh = parts[0].mesh;
-            this.unregisterMesh(initialMesh);
-            initialMesh.computeWorldMatrix(true);
-            var initialShape = this._createShape(initialMesh, parts[0].impostor);
-            var body = this._createRigidBodyFromShape(initialShape, initialMesh, options);
-            for (var index = 1; index < parts.length; index++) {
-                var mesh = parts[index].mesh;
-                mesh.computeWorldMatrix(true);
-                var shape = this._createShape(mesh, parts[index].impostor);
-                var localPosition = mesh.position;
-                body.addShape(shape, new CANNON.Vec3(localPosition.x, localPosition.y, localPosition.z));
-            }
-            return body;
-        };
-        CannonJSPlugin.prototype._unbindBody = function (body) {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.body === body) {
-                    this._world.remove(registeredMesh.body);
-                    registeredMesh.body = null;
-                    registeredMesh.delta = null;
-                    registeredMesh.deltaRotation = null;
-                }
-            }
+        CannonJSPlugin.prototype.setTransformationFromPhysicsBody = function (impostor) {
+            impostor.mesh.position.copyFrom(impostor.physicsBody.position);
+            impostor.mesh.rotationQuaternion.copyFrom(impostor.physicsBody.quaternion);
         };
-        CannonJSPlugin.prototype.unregisterMesh = function (mesh) {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh) {
-                    // Remove body
-                    if (registeredMesh.body) {
-                        this._unbindBody(registeredMesh.body);
-                    }
-                    this._registeredMeshes.splice(index, 1);
-                    return;
-                }
-            }
-        };
-        CannonJSPlugin.prototype.applyImpulse = function (mesh, force, contactPoint) {
-            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
-            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh) {
-                    registeredMesh.body.applyImpulse(impulse, worldPoint);
-                    return;
-                }
-            }
-        };
-        CannonJSPlugin.prototype.createLink = function (mesh1, mesh2, pivot1, pivot2) {
-            var body1 = null, body2 = null;
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh1) {
-                    body1 = registeredMesh.body;
-                    if (body2)
-                        break;
-                }
-                else if (registeredMesh.mesh === mesh2) {
-                    body2 = registeredMesh.body;
-                    if (body1)
-                        break;
-                }
-            }
-            if (!body1 || !body2) {
-                return false;
-            }
-            var constraint = new CANNON.PointToPointConstraint(body1, new CANNON.Vec3(pivot1.x, pivot1.y, pivot1.z), body2, new CANNON.Vec3(pivot2.x, pivot2.y, pivot2.z));
-            this._world.addConstraint(constraint);
-            return true;
-        };
-        CannonJSPlugin.prototype.dispose = function () {
-            while (this._registeredMeshes.length) {
-                this.unregisterMesh(this._registeredMeshes[0].mesh);
-            }
+        CannonJSPlugin.prototype.setPhysicsBodyTransformation = function (impostor, newPosition, newRotation) {
+            impostor.physicsBody.position.copy(newPosition);
+            impostor.physicsBody.quaternion.copy(newRotation);
         };
         CannonJSPlugin.prototype.isSupported = function () {
             return window.CANNON !== undefined;
         };
-        CannonJSPlugin.prototype.getWorldObject = function () {
-            return this._world;
+        CannonJSPlugin.prototype.setVelocity = function (impostor, velocity) {
+            impostor.physicsBody.velocity.copy(velocity);
         };
-        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;
+        CannonJSPlugin.prototype.dispose = function () {
+            //nothing to do, actually.
         };
         return CannonJSPlugin;
-    })();
+    }());
     BABYLON.CannonJSPlugin = CannonJSPlugin;
 })(BABYLON || (BABYLON = {}));

src/Physics/Plugins/babylon.cannonJSPlugin.ts

@@ -1,109 +1,222 @@
 module BABYLON {
     declare var CANNON;
 
-    interface IRegisteredMesh {
-        mesh: AbstractMesh;
-        body: any; //Cannon body
-        material: any;
-        delta: Vector3;
-        deltaRotation: Quaternion;
-        type: any;
-        collisionFunction?: (event: any) => void;
-
-    }
-
     export class CannonJSPlugin implements IPhysicsEnginePlugin {
 
-        private _world: any;
-        private _registeredMeshes: Array<IRegisteredMesh> = [];
+        public world: any; //CANNON.World
+        public name: string = "CannonJSPlugin";
         private _physicsMaterials = [];
-        private _gravity: Vector3;
         private _fixedTimeStep: number = 1 / 60;
-        //private _maxSubSteps : number = 15;
-
-        public name = "CannonJS";
-
-        public constructor(private _useDeltaForWorldStep: boolean = true) {
+        //See https://github.com/schteppe/cannon.js/blob/gh-pages/demos/collisionFilter.html
+        private _currentCollisionGroup = 2;
 
+        public constructor(private _useDeltaForWorldStep: boolean = true, iterations: number = 10) {
+            if (!this.isSupported()) {
+                Tools.Error("CannonJS is not available. Please make sure you included the js file.");
+                return;
+            }
+            this.world = new CANNON.World();
+            this.world.broadphase = new CANNON.NaiveBroadphase();
+            this.world.solver.iterations = iterations;
         }
 
-        public initialize(iterations: number = 10): void {
-            this._world = new CANNON.World();
-            this._world.broadphase = new CANNON.NaiveBroadphase();
-            this._world.solver.iterations = iterations;
+        public setGravity(gravity: Vector3): void {
+            this.world.gravity.copy(gravity);
         }
 
-        private _checkWithEpsilon(value: number): number {
-            return value < PhysicsEngine.Epsilon ? PhysicsEngine.Epsilon : value;
+        public executeStep(delta: number, impostors: Array<PhysicsImpostor>): void {
+            this.world.step(this._fixedTimeStep, this._useDeltaForWorldStep ? delta * 1000 : 0);
         }
 
-        public runOneStep(delta: number): void {
+        public applyImpulse(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3) {
+            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
+            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
 
-            this._world.step(this._fixedTimeStep, this._useDeltaForWorldStep ? delta * 1000 : 0);
+            impostor.physicsBody.applyImpulse(impulse, worldPoint);
+        }
 
-            this._registeredMeshes.forEach((registeredMesh) => {
+        public applyForce(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3) {
+            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
+            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
 
-                // Body position
-                var bodyX = registeredMesh.body.position.x,
-                    bodyY = registeredMesh.body.position.y,
-                    bodyZ = registeredMesh.body.position.z;
+            impostor.physicsBody.applyImpulse(impulse, worldPoint);
+        }
 
-                registeredMesh.mesh.position.x = bodyX + registeredMesh.delta.x;
-                registeredMesh.mesh.position.y = bodyY + registeredMesh.delta.y;
-                registeredMesh.mesh.position.z = bodyZ + registeredMesh.delta.z;
+        public generatePhysicsBody(impostor: PhysicsImpostor) {
+            //parent-child relationship. Does this impostor has a parent impostor?
+            if (impostor.parent) {
+                if (impostor.physicsBody) {
+                    this.removePhysicsBody(impostor);
+                    //TODO is that needed?
+                    impostor.forceUpdate();
+                }
+                return;
+            }
 
-                registeredMesh.mesh.rotationQuaternion.copyFrom(registeredMesh.body.quaternion);
-                if (registeredMesh.deltaRotation) {
-                    registeredMesh.mesh.rotationQuaternion.multiplyInPlace(registeredMesh.deltaRotation);
+            //should a new body be created for this impostor?
+            if (impostor.isBodyInitRequired()) {
+                if (!impostor.mesh.rotationQuaternion) {
+                    impostor.mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(impostor.mesh.rotation.y, impostor.mesh.rotation.x, impostor.mesh.rotation.z);
                 }
 
-                //is the physics collision callback is set?
-                if (registeredMesh.mesh.onPhysicsCollide) {
-                    if (!registeredMesh.collisionFunction) {
-                        registeredMesh.collisionFunction = (e) => {
-                            //find the mesh that collided with the registered mesh
-                            for (var idx = 0; idx < this._registeredMeshes.length; idx++) {
-                                if (this._registeredMeshes[idx].body == e.body) {
-                                    registeredMesh.mesh.onPhysicsCollide(this._registeredMeshes[idx].mesh, e.contact);
-                                }
+                var shape = this._createShape(impostor);
+                
+                //unregister events, if body is being changed
+                var oldBody = impostor.physicsBody;
+                if (oldBody) {
+                    this.removePhysicsBody(impostor);
+                }
+                
+                //create the body and material
+                var material = this._addMaterial(impostor.getOptions().friction, impostor.getOptions().restitution);
+
+                var bodyCreationObject = {
+                    mass: impostor.getOptions().mass,
+                    material: material
+                };
+                // A simple extend, in case native options were used.
+                var nativeOptions = impostor.getOptions().nativeOptions;
+                for (var key in nativeOptions) {
+                    if (nativeOptions.hasOwnProperty(key)) {
+                        bodyCreationObject[key] = nativeOptions[key];
+                    }
+                }
+                impostor.physicsBody = new CANNON.Body(bodyCreationObject);
+                impostor.physicsBody.addEventListener("collide", impostor.onCollide);
+                this.world.addEventListener("preStep", impostor.beforeStep);
+                this.world.addEventListener("postStep", impostor.afterStep);
+                impostor.physicsBody.addShape(shape);
+                this.world.add(impostor.physicsBody);
+                
+                //try to keep the body moving in the right direction by taking old properties.
+                //Should be tested!
+                if (oldBody) {
+                    ['force', 'torque', 'velocity', 'angularVelocity'].forEach(function(param) {
+                        impostor.physicsBody[param].copy(oldBody[param]);
+                    });
+                }
+                this._processChildMeshes(impostor);
+            }
+
+            //now update the body's transformation
+            this._updatePhysicsBodyTransformation(impostor);
+        }
+
+        private _processChildMeshes(mainImpostor: PhysicsImpostor) {
+            var meshChildren = mainImpostor.mesh.getChildMeshes();
+            if (meshChildren.length) {
+                var processMesh = (localPosition: Vector3, mesh: AbstractMesh) => {
+                    var childImpostor = mesh.getPhysicsImpostor();
+                    if (childImpostor) {
+                        var parent = childImpostor.parent;
+                        if (parent !== mainImpostor) {
+                            var localPosition = mesh.position;
+                            if (childImpostor.physicsBody) {
+                                this.removePhysicsBody(childImpostor);
+                                childImpostor.physicsBody = null;
                             }
+                            childImpostor.parent = mainImpostor;
+                            childImpostor.resetUpdateFlags();
+                            mainImpostor.physicsBody.addShape(this._createShape(childImpostor), new CANNON.Vec3(localPosition.x, localPosition.y, localPosition.z));
+                            //Add the mass of the children.
+                            mainImpostor.physicsBody.mass += childImpostor.getParam("mass");
                         }
-                        registeredMesh.body.addEventListener("collide", registeredMesh.collisionFunction);
-                    }
-                } else {
-                    //unregister, in case the function was removed for some reason
-                    if (registeredMesh.collisionFunction) {
-                        registeredMesh.body.removeEventListener("collide", registeredMesh.collisionFunction);
                     }
+                    mesh.getChildMeshes().forEach(processMesh.bind(this, mesh.position));
                 }
-            });
+                meshChildren.forEach(processMesh.bind(this, Vector3.Zero()));
+            }
         }
 
-        public setGravity(gravity: Vector3): void {
-            this._gravity = gravity;
-            this._world.gravity.set(gravity.x, gravity.y, gravity.z);
+        public removePhysicsBody(impostor: PhysicsImpostor) {
+            impostor.physicsBody.removeEventListener("collide", impostor.onCollide);
+            this.world.removeEventListener("preStep", impostor.beforeStep);
+            this.world.removeEventListener("postStep", impostor.afterStep);
+            this.world.remove(impostor.physicsBody);
+        }
+
+        public generateJoint(impostorJoint: PhysicsImpostorJoint) {
+            var mainBody = impostorJoint.mainImpostor.physicsBody;
+            var connectedBody = impostorJoint.connectedImpostor.physicsBody;
+            if (!mainBody || !connectedBody) {
+                return;
+            }
+            var constraint;
+            var jointData = impostorJoint.joint.jointData;
+            //TODO - https://github.com/schteppe/cannon.js/blob/gh-pages/demos/collisionFilter.html
+            var constraintData = {
+                pivotA: jointData.mainPivot ? new CANNON.Vec3().copy(jointData.mainPivot) : null,
+                pivotB: jointData.connectedPivot ? new CANNON.Vec3().copy(jointData.connectedPivot) : null,
+                axisA: jointData.mainAxis ? new CANNON.Vec3().copy(jointData.mainAxis) : null,
+                axisB: jointData.connectedAxis ? new CANNON.Vec3().copy(jointData.connectedAxis) : null,
+                maxForce: jointData.nativeParams.maxForce
+            };
+            if(!jointData.collision) {
+                //add 1st body to a collision group of its own, if it is not in 1
+                if(mainBody.collisionFilterGroup === 1) {
+                    mainBody.collisionFilterGroup = this._currentCollisionGroup;
+                    this._currentCollisionGroup <<= 1;
+                }
+                if(connectedBody.collisionFilterGroup === 1) {
+                    connectedBody.collisionFilterGroup = this._currentCollisionGroup;
+                    this._currentCollisionGroup <<= 1;
+                }
+                //add their mask to the collisionFilterMask of each other:
+                connectedBody.collisionFilterMask = connectedBody.collisionFilterMask | ~mainBody.collisionFilterGroup;
+                mainBody.collisionFilterMask = mainBody.collisionFilterMask | ~connectedBody.collisionFilterGroup;
+            }
+            switch (impostorJoint.joint.type) {
+                case PhysicsJoint.HingeJoint:
+                    constraint = new CANNON.HingeConstraint(mainBody, connectedBody, constraintData);
+                    break;
+                case PhysicsJoint.DistanceJoint:
+                    constraint = new CANNON.DistanceConstraint(mainBody, connectedBody, (<DistanceJointData>jointData).maxDistance || 2)
+                    break;
+                default:
+                    constraint = new CANNON.PointToPointConstraint(mainBody, constraintData.pivotA, connectedBody, constraintData.pivotA, constraintData.maxForce);
+                    break;
+            }
+            impostorJoint.joint.physicsJoint = constraint;
+            this.world.addConstraint(constraint);
         }
 
-        public getGravity(): Vector3 {
-            return this._gravity;
+        public removeJoint(joint: PhysicsImpostorJoint) {
+            //TODO
         }
 
-        public registerMesh(mesh: AbstractMesh, impostor: number, options?: PhysicsBodyCreationOptions): any {
-            this.unregisterMesh(mesh);
+        private _addMaterial(friction: number, restitution: number) {
+            var index;
+            var mat;
 
-            if (!mesh.rotationQuaternion) {
-                mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
+            for (index = 0; index < this._physicsMaterials.length; index++) {
+                mat = this._physicsMaterials[index];
+
+                if (mat.friction === friction && mat.restitution === restitution) {
+                    return mat;
+                }
             }
 
-            mesh.computeWorldMatrix(true);
+            var currentMat = new CANNON.Material("mat");
+            this._physicsMaterials.push(currentMat);
+
+            for (index = 0; index < this._physicsMaterials.length; index++) {
+                mat = this._physicsMaterials[index];
 
-            var shape = this._createShape(mesh, impostor);
+                var contactMaterial = new CANNON.ContactMaterial(mat, currentMat, { friction: friction, restitution: restitution });
+
+                this.world.addContactMaterial(contactMaterial);
+            }
 
-            return this._createRigidBodyFromShape(shape, mesh, options);
+            return currentMat;
         }
 
-        private _createShape(mesh: AbstractMesh, impostor: number) {
-		
+        private _checkWithEpsilon(value: number): number {
+            return value < PhysicsEngine.Epsilon ? PhysicsEngine.Epsilon : value;
+        }
+
+        private _createShape(impostor: PhysicsImpostor) {
+            var mesh = impostor.mesh;
+        
             //get the correct bounding box
             var oldQuaternion = mesh.rotationQuaternion;
             mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
@@ -111,7 +224,7 @@
 
             var returnValue;
 
-            switch (impostor) {
+            switch (impostor.type) {
                 case PhysicsEngine.SphereImpostor:
                     var bbox = mesh.getBoundingInfo().boundingBox;
                     var radiusX = bbox.maximumWorld.x - bbox.minimumWorld.x;
@@ -132,14 +245,14 @@
                     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");
+                    Tools.Warn("Attention, PlaneImposter might not behave as you expect. Consider using BoxImposter instead");
                     returnValue = new CANNON.Plane();
                     break;
                 case PhysicsEngine.MeshImpostor:
                     var rawVerts = mesh.getVerticesData(VertexBuffer.PositionKind);
                     var rawFaces = mesh.getIndices();
                     Tools.Warn("MeshImpostor only collides against spheres.");
-                    returnValue = new CANNON.Trimesh(rawVerts, rawFaces); //this._createConvexPolyhedron(rawVerts, rawFaces, mesh);
+                    returnValue = new CANNON.Trimesh(rawVerts, rawFaces);
                     break;
                 case PhysicsEngine.HeightmapImpostor:
                     returnValue = this._createHeightmap(mesh);
@@ -152,29 +265,6 @@
             return returnValue;
         }
 
-        private _createConvexPolyhedron(rawVerts: number[] | Float32Array, rawFaces: number[] | Int32Array, mesh: AbstractMesh): any {
-            var verts = [], faces = [];
-
-            mesh.computeWorldMatrix(true);
-
-            //reuse this variable
-            var transformed = Vector3.Zero();
-            // Get vertices
-            for (var i = 0; i < rawVerts.length; i += 3) {
-                Vector3.TransformNormalFromFloatsToRef(rawVerts[i], rawVerts[i + 1], rawVerts[i + 2], mesh.getWorldMatrix(), transformed);
-                verts.push(new CANNON.Vec3(transformed.x, transformed.y, transformed.z));
-            }
-
-            // Get faces
-            for (var j = 0; j < rawFaces.length; j += 3) {
-                faces.push([rawFaces[j], rawFaces[j + 2], rawFaces[j + 1]]);
-            }
-
-            var shape = new CANNON.ConvexPolyhedron(verts, faces);
-
-            return shape;
-        }
-
         private _createHeightmap(mesh: AbstractMesh, pointDepth?: number) {
             var pos = mesh.getVerticesData(VertexBuffer.PositionKind);
             var matrix = [];
@@ -235,65 +325,39 @@
             return shape;
         }
 
-        private _addMaterial(friction: number, restitution: number) {
-            var index;
-            var mat;
-
-            for (index = 0; index < this._physicsMaterials.length; index++) {
-                mat = this._physicsMaterials[index];
-
-                if (mat.friction === friction && mat.restitution === restitution) {
-                    return mat;
-                }
-            }
-
-            var currentMat = new CANNON.Material("mat");
-            this._physicsMaterials.push(currentMat);
-
-            for (index = 0; index < this._physicsMaterials.length; index++) {
-                mat = this._physicsMaterials[index];
-
-                var contactMaterial = new CANNON.ContactMaterial(mat, currentMat, { friction: friction, restitution: restitution });
-
-                this._world.addContactMaterial(contactMaterial);
-            }
-
-            return currentMat;
-        }
-
-        private _createRigidBodyFromShape(shape: any, mesh: AbstractMesh, options: PhysicsBodyCreationOptions): any {
-            if (!mesh.rotationQuaternion) {
-                mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(mesh.rotation.y, mesh.rotation.x, mesh.rotation.z);
-            }
-            
+        private _minus90X = new Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
+        private _plus90X = new Quaternion(0.7071067811865475, 0, 0, 0.7071067811865475);
+        private _tmpPosition: Vector3 = Vector3.Zero();
+        private _tmpQuaternion: Quaternion = new Quaternion();
+        private _tmpDeltaPosition: Vector3 = Vector3.Zero();
+        private _tmpDeltaRotation: Quaternion = new Quaternion();
+        private _tmpUnityRotation: Quaternion = new Quaternion();
+
+        private _updatePhysicsBodyTransformation(impostor: PhysicsImpostor) {
+            var mesh = impostor.mesh;
+            //make sure it is updated...
+            impostor.mesh.computeWorldMatrix(true);
             // The delta between the mesh position and the mesh bounding box center
             var bbox = mesh.getBoundingInfo().boundingBox;
-            var deltaPosition = mesh.position.subtract(bbox.center);
-            var deltaRotation;
-
-            var material = this._addMaterial(options.friction, options.restitution);
-            var body = new CANNON.Body({
-                mass: options.mass,
-                material: material,
-                position: new CANNON.Vec3(bbox.center.x, bbox.center.y, bbox.center.z)
-            });
+            this._tmpDeltaPosition.copyFrom(mesh.position.subtract(bbox.center));
 
-            body.quaternion = new CANNON.Quaternion(mesh.rotationQuaternion.x, mesh.rotationQuaternion.y, mesh.rotationQuaternion.z, mesh.rotationQuaternion.w);
+            var quaternion = mesh.rotationQuaternion;
+            this._tmpPosition.copyFrom(mesh.getBoundingInfo().boundingBox.center);
             //is shape is a plane or a heightmap, it must be rotated 90 degs in the X axis.
-            if (shape.type === CANNON.Shape.types.PLANE || shape.type === CANNON.Shape.types.HEIGHTFIELD) {
+            if (impostor.type === PhysicsEngine.PlaneImpostor || impostor.type === PhysicsEngine.HeightmapImpostor) {
                 //-90 DEG in X, precalculated
-                var tmpQ = new CANNON.Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
-                body.quaternion = body.quaternion.mult(tmpQ);
+                quaternion = quaternion.multiply(this._minus90X);
                 //Invert! (Precalculated, 90 deg in X)
-                deltaRotation = new Quaternion(0.7071067811865475, 0, 0, 0.7071067811865475);
+                //No need to clone. this will never change.
+                impostor.setDeltaRotation(this._plus90X);
             }
             
             //If it is a heightfield, if should be centered.
-            if (shape.type === CANNON.Shape.types.HEIGHTFIELD) {
+            if (impostor.type === PhysicsEngine.HeightmapImpostor) {
                 
                 //calculate the correct body position:
                 var rotationQuaternion = mesh.rotationQuaternion;
-                mesh.rotationQuaternion = new BABYLON.Quaternion();
+                mesh.rotationQuaternion = this._tmpUnityRotation;
                 mesh.computeWorldMatrix(true);
                 
                 //get original center with no rotation
@@ -312,194 +376,44 @@
                 //calculate the translation
                 var translation = mesh.getBoundingInfo().boundingBox.center.subtract(center).subtract(mesh.position).negate();
 
-                body.position = new CANNON.Vec3(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
+                this._tmpPosition.copyFromFloats(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
                 //add it inverted to the delta 
-                deltaPosition = mesh.getBoundingInfo().boundingBox.center.subtract(center);
-                deltaPosition.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
+                this._tmpDeltaPosition.copyFrom(mesh.getBoundingInfo().boundingBox.center.subtract(center));
+                this._tmpDeltaPosition.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
 
                 mesh.setPivotMatrix(oldPivot);
                 mesh.computeWorldMatrix(true);
-            } else if (shape.type === CANNON.Shape.types.TRIMESH) {
-                deltaPosition = Vector3.Zero();
-            }
-            
-            //add the shape
-            body.addShape(shape);
-
-            this._world.add(body);
-
-            this._registeredMeshes.push({ mesh: mesh, body: body, material: material, delta: deltaPosition, deltaRotation: deltaRotation, type: shape.type });
-
-            return body;
-        }
-
-        public registerMeshesAsCompound(parts: PhysicsCompoundBodyPart[], options: PhysicsBodyCreationOptions): any {
-
-            var initialMesh = parts[0].mesh;
-
-            this.unregisterMesh(initialMesh);
-
-            initialMesh.computeWorldMatrix(true);
-
-            var initialShape = this._createShape(initialMesh, parts[0].impostor);
-            var body = this._createRigidBodyFromShape(initialShape, initialMesh, options);
-
-            for (var index = 1; index < parts.length; index++) {
-                var mesh = parts[index].mesh;
-                mesh.computeWorldMatrix(true);
-                var shape = this._createShape(mesh, parts[index].impostor);
-                var localPosition = mesh.position;
-
-                body.addShape(shape, new CANNON.Vec3(localPosition.x, localPosition.y, localPosition.z));
-            }
-
-            return body;
-        }
-
-        private _unbindBody(body): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-
-                if (registeredMesh.body === body) {
-                    this._world.remove(registeredMesh.body);
-                    registeredMesh.body = null;
-                    registeredMesh.delta = null;
-                    registeredMesh.deltaRotation = null;
-                }
+            } else if (impostor.type === PhysicsEngine.MeshImpostor) {
+                this._tmpDeltaPosition.copyFromFloats(0, 0, 0);
+                this._tmpPosition.copyFrom(mesh.position);
             }
-        }
-
-        public unregisterMesh(mesh: AbstractMesh): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-
-                if (registeredMesh.mesh === mesh) {
-                    // Remove body
-                    if (registeredMesh.body) {
-                        this._unbindBody(registeredMesh.body);
-                    }
 
-                    this._registeredMeshes.splice(index, 1);
-                    return;
-                }
-            }
+            impostor.setDeltaPosition(this._tmpDeltaPosition);
+            //Now update the impostor object
+            impostor.physicsBody.position.copy(this._tmpPosition);
+            impostor.physicsBody.quaternion.copy(quaternion);
         }
 
-        public applyImpulse(mesh: AbstractMesh, force: Vector3, contactPoint: Vector3): void {
-            var worldPoint = new CANNON.Vec3(contactPoint.x, contactPoint.y, contactPoint.z);
-            var impulse = new CANNON.Vec3(force.x, force.y, force.z);
-
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-
-                if (registeredMesh.mesh === mesh) {
-                    registeredMesh.body.applyImpulse(impulse, worldPoint);
-                    return;
-                }
-            }
+        public setTransformationFromPhysicsBody(impostor: PhysicsImpostor) {
+            impostor.mesh.position.copyFrom(impostor.physicsBody.position);
+            impostor.mesh.rotationQuaternion.copyFrom(impostor.physicsBody.quaternion);
         }
 
-        public updateBodyPosition = function (mesh: AbstractMesh): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    var body = registeredMesh.body;
-
-                    var center = mesh.getBoundingInfo().boundingBox.center.clone();
-
-                    body.quaternion.copy(mesh.rotationQuaternion);
-
-                    if (registeredMesh.deltaRotation) {
-                        var tmpQ = new CANNON.Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865475);
-                        body.quaternion = body.quaternion.mult(tmpQ);
-                    }
-
-                    if (registeredMesh.type === CANNON.Shape.types.HEIGHTFIELD) {
-                        //calculate the correct body position:
-                        var rotationQuaternion = mesh.rotationQuaternion;
-                        mesh.rotationQuaternion = new BABYLON.Quaternion();
-                        mesh.computeWorldMatrix(true);
-                        
-                        //get original center with no rotation
-                        var center = mesh.getBoundingInfo().boundingBox.center.clone();
-
-                        var oldPivot = mesh.getPivotMatrix() || Matrix.Translation(0, 0, 0);
-                        
-                        //rotation is back
-                        mesh.rotationQuaternion = rotationQuaternion;
-                
-                        //calculate the new center using a pivot (since Cannon.js doesn't center height maps)
-                        var p = Matrix.Translation(mesh.getBoundingInfo().boundingBox.extendSize.x, 0, -mesh.getBoundingInfo().boundingBox.extendSize.z);
-                        mesh.setPivotMatrix(p);
-                        mesh.computeWorldMatrix(true);
-                
-                        //calculate the translation
-                        var translation = mesh.getBoundingInfo().boundingBox.center.subtract(center).subtract(mesh.position).negate();
-
-                        center.copyFromFloats(translation.x, translation.y - mesh.getBoundingInfo().boundingBox.extendSize.y, translation.z);
-                        //add it inverted to the delta 
-                        registeredMesh.delta = mesh.getBoundingInfo().boundingBox.center.subtract(center);
-                        registeredMesh.delta.y += mesh.getBoundingInfo().boundingBox.extendSize.y;
-
-                        mesh.setPivotMatrix(oldPivot);
-                        mesh.computeWorldMatrix(true);
-                    } else if (registeredMesh.type === CANNON.Shape.types.TRIMESH) {
-                        center.copyFromFloats(mesh.position.x, mesh.position.y, mesh.position.z);
-                    }
-
-                    body.position.set(center.x, center.y, center.z);
-
-                    return;
-                }
-            }
-        }
-
-        public createLink(mesh1: AbstractMesh, mesh2: AbstractMesh, pivot1: Vector3, pivot2: Vector3): boolean {
-            var body1 = null, body2 = null;
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-
-                if (registeredMesh.mesh === mesh1) {
-                    body1 = registeredMesh.body;
-                    if (body2) break;
-                } else if (registeredMesh.mesh === mesh2) {
-                    body2 = registeredMesh.body;
-                    if (body1) break;
-                }
-            }
-
-            if (!body1 || !body2) {
-                return false;
-            }
-
-            var constraint = new CANNON.PointToPointConstraint(body1, new CANNON.Vec3(pivot1.x, pivot1.y, pivot1.z), body2, new CANNON.Vec3(pivot2.x, pivot2.y, pivot2.z));
-            this._world.addConstraint(constraint);
-
-            return true;
-        }
-
-        public dispose(): void {
-            while (this._registeredMeshes.length) {
-                this.unregisterMesh(this._registeredMeshes[0].mesh);
-            }
+        public setPhysicsBodyTransformation(impostor: PhysicsImpostor, newPosition: Vector3, newRotation: Quaternion) {
+            impostor.physicsBody.position.copy(newPosition);
+            impostor.physicsBody.quaternion.copy(newRotation);
         }
 
         public isSupported(): boolean {
             return window.CANNON !== undefined;
         }
-
-        public getWorldObject(): any {
-            return this._world;
+        
+        public setVelocity(impostor: PhysicsImpostor, velocity: Vector3) {
+            impostor.physicsBody.velocity.copy(velocity);
         }
 
-        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;
+        public dispose() {
+            //nothing to do, actually.
         }
     }
 }

src/Physics/Plugins/babylon.oimoJSPlugin.js

@@ -1,279 +1,247 @@
 var BABYLON;
 (function (BABYLON) {
     var OimoJSPlugin = (function () {
-        function OimoJSPlugin() {
-            this._registeredMeshes = [];
-            this.name = "oimo";
-            /**
-             * Update the body position according to the mesh position
-             * @param mesh
-             */
-            this.updateBodyPosition = function (mesh) {
-                for (var index = 0; index < this._registeredMeshes.length; index++) {
-                    var registeredMesh = this._registeredMeshes[index];
-                    var body = registeredMesh.body.body;
-                    var updated = false;
-                    var newPosition;
-                    if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                        mesh.computeWorldMatrix(true);
-                        newPosition = mesh.getBoundingInfo().boundingBox.center;
-                        updated = true;
-                    }
-                    else if (registeredMesh.mesh.parent === mesh) {
-                        mesh.computeWorldMatrix(true);
-                        registeredMesh.mesh.computeWorldMatrix(true);
-                        newPosition = registeredMesh.mesh.getAbsolutePosition();
-                        updated = true;
-                    }
-                    if (updated) {
-                        body.setPosition(new OIMO.Vec3(newPosition.x, newPosition.y, newPosition.z));
-                        body.setQuaternion(mesh.rotationQuaternion);
-                        body.sleeping = false;
-                        //force Oimo to update the body's position
-                        body.updatePosition(1);
-                    }
-                }
-            };
+        function OimoJSPlugin(iterations) {
+            this.name = "OimoJSPlugin";
+            this._tmpImpostorsArray = [];
+            this._tmpPositionVector = BABYLON.Vector3.Zero();
+            this.world = new OIMO.World(1 / 60, 2, iterations, true);
+            this.world.clear();
+            //making sure no stats are calculated
+            this.world.isNoStat = true;
         }
-        OimoJSPlugin.prototype._checkWithEpsilon = function (value) {
-            return value < BABYLON.PhysicsEngine.Epsilon ? BABYLON.PhysicsEngine.Epsilon : value;
-        };
-        OimoJSPlugin.prototype.initialize = function (iterations) {
-            this._world = new OIMO.World(null, null, iterations);
-            this._world.clear();
-        };
         OimoJSPlugin.prototype.setGravity = function (gravity) {
-            this._gravity = this._world.gravity = gravity;
-        };
-        OimoJSPlugin.prototype.getGravity = function () {
-            return this._gravity;
+            this.world.gravity.copy(gravity);
         };
-        OimoJSPlugin.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 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 });
-            //get the correct bounding box
-            var oldQuaternion = mesh.rotationQuaternion;
-            mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
-            mesh.computeWorldMatrix(true);
-            var bodyConfig = {
-                name: mesh.uniqueId,
-                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:
-                    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;
-                    bodyConfig.type = 'sphere';
-                    bodyConfig.size = [size];
-                    break;
-                case BABYLON.PhysicsEngine.PlaneImpostor:
-                //Oimo "fakes" a cylinder as a box, so why don't we!
-                case BABYLON.PhysicsEngine.CylinderImpostor:
-                case BABYLON.PhysicsEngine.BoxImpostor:
-                    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);
-                    bodyConfig.type = 'box';
-                    bodyConfig.size = [sizeX, sizeY, sizeZ];
-                    break;
-            }
-            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) {
-            var types = [], sizes = [], positions = [], 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);
-                types.push(bodyParameters.type);
-                sizes.push.apply(sizes, bodyParameters.size);
-                positions.push.apply(positions, bodyParameters.pos);
-                rotations.push.apply(rotations, bodyParameters.rot);
-            }
-            var body = new OIMO.Body({
-                name: initialMesh.uniqueId,
-                type: types,
-                size: sizes,
-                pos: positions,
-                rot: rotations,
-                move: options.mass != 0,
-                config: [options.mass, options.friction, options.restitution],
-                world: this._world
+        OimoJSPlugin.prototype.executeStep = function (delta, impostors) {
+            var _this = this;
+            impostors.forEach(function (impostor) {
+                impostor.beforeStep();
             });
-            //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
+            this.world.step();
+            impostors.forEach(function (impostor) {
+                impostor.afterStep();
+                //update the ordered impostors array
+                _this._tmpImpostorsArray[impostor.mesh.uniqueId] = impostor;
             });
-            return body;
-        };
-        OimoJSPlugin.prototype._createBodyAsCompound = function (part, options, initialMesh) {
-            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(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 = {
-                name: mesh.uniqueId,
-                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;
-                    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;
-                    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;
-                    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);
-                    bodyParameters.type = 'box';
-                    bodyParameters.size = [sizeX, sizeY, sizeZ];
-                    break;
-            }
-            mesh.rotationQuaternion = oldQuaternion;
-            return bodyParameters;
-        };
-        OimoJSPlugin.prototype.unregisterMesh = function (mesh) {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    if (registeredMesh.body) {
-                        this._world.removeRigidBody(registeredMesh.body.body);
-                        this._unbindBody(registeredMesh.body);
-                    }
-                    this._registeredMeshes.splice(index, 1);
-                    return;
+            //check for collisions
+            var contact = this.world.contacts;
+            while (contact !== null) {
+                if (contact.touching && !contact.body1.sleeping && !contact.body2.sleeping) {
+                    contact = contact.next;
+                    continue;
                 }
-            }
-        };
-        OimoJSPlugin.prototype._unbindBody = function (body) {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.body === body) {
-                    registeredMesh.body = null;
+                //is this body colliding with any other? get the impostor
+                var mainImpostor = this._tmpImpostorsArray[+contact.body1.name];
+                var collidingImpostor = this._tmpImpostorsArray[+contact.body2.name];
+                if (!mainImpostor || !collidingImpostor) {
+                    contact = contact.next;
+                    continue;
                 }
+                mainImpostor.onCollide({ body: collidingImpostor.physicsBody });
+                collidingImpostor.onCollide({ body: mainImpostor.physicsBody });
+                contact = contact.next;
             }
         };
-        OimoJSPlugin.prototype.applyImpulse = function (mesh, force, contactPoint) {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    // Get object mass to have a behaviour similar to cannon.js
-                    var mass = registeredMesh.body.body.massInfo.mass;
-                    // The force is scaled with the mass of object
-                    registeredMesh.body.body.applyImpulse(contactPoint.scale(OIMO.INV_SCALE), force.scale(OIMO.INV_SCALE * mass));
-                    return;
+        OimoJSPlugin.prototype.applyImpulse = function (impostor, force, contactPoint) {
+            var mass = impostor.physicsBody.massInfo.mass;
+            impostor.physicsBody.applyImpulse(contactPoint.scale(OIMO.INV_SCALE), force.scale(OIMO.INV_SCALE * mass));
+        };
+        OimoJSPlugin.prototype.applyForce = function (impostor, force, contactPoint) {
+            BABYLON.Tools.Warn("Oimo doesn't support applying force. Using impule instead.");
+            this.applyImpulse(impostor, force, contactPoint);
+        };
+        OimoJSPlugin.prototype.generatePhysicsBody = function (impostor) {
+            var _this = this;
+            //parent-child relationship. Does this impostor has a parent impostor?
+            if (impostor.parent) {
+                if (impostor.physicsBody) {
+                    this.removePhysicsBody(impostor);
+                    //TODO is that needed?
+                    impostor.forceUpdate();
                 }
+                return;
             }
-        };
-        OimoJSPlugin.prototype.createLink = function (mesh1, mesh2, pivot1, pivot2, options) {
-            var body1 = null, body2 = null;
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh1) {
-                    body1 = registeredMesh.body.body;
+            impostor.mesh.computeWorldMatrix(true);
+            if (impostor.isBodyInitRequired()) {
+                if (!impostor.mesh.rotationQuaternion) {
+                    impostor.mesh.rotationQuaternion = BABYLON.Quaternion.RotationYawPitchRoll(impostor.mesh.rotation.y, impostor.mesh.rotation.x, impostor.mesh.rotation.z);
+                }
+                var bodyConfig = {
+                    name: impostor.mesh.uniqueId,
+                    //Oimo must have mass, also for static objects.
+                    config: [impostor.getParam("mass") || 1, impostor.getParam("friction"), impostor.getParam("restitution")],
+                    size: [],
+                    type: [],
+                    pos: [],
+                    rot: [],
+                    move: impostor.getParam("mass") !== 0,
+                    //Supporting older versions of Oimo
+                    world: this.world
+                };
+                var impostors = [impostor];
+                function addToArray(parent) {
+                    parent.getChildMeshes().forEach(function (m) {
+                        if (m.physicsImpostor) {
+                            impostors.push(m.physicsImpostor);
+                            m.physicsImpostor._init();
+                        }
+                        addToArray(m);
+                    });
                 }
-                else if (registeredMesh.mesh === mesh2) {
-                    body2 = registeredMesh.body.body;
+                addToArray(impostor.mesh);
+                function checkWithEpsilon(value) {
+                    return Math.max(value, BABYLON.PhysicsEngine.Epsilon);
                 }
+                impostors.forEach(function (i) {
+                    //get the correct bounding box
+                    var oldQuaternion = i.mesh.rotationQuaternion;
+                    var rot = new OIMO.Euler().setFromQuaternion({ x: impostor.mesh.rotationQuaternion.x, y: impostor.mesh.rotationQuaternion.y, z: impostor.mesh.rotationQuaternion.z, s: impostor.mesh.rotationQuaternion.w });
+                    i.mesh.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1);
+                    i.mesh.computeWorldMatrix(true);
+                    var bbox = i.mesh.getBoundingInfo().boundingBox;
+                    if (i === impostor) {
+                        impostor.mesh.position.subtractToRef(impostor.mesh.getBoundingInfo().boundingBox.center, _this._tmpPositionVector);
+                        //Can also use Array.prototype.push.apply
+                        bodyConfig.pos.push(bbox.center.x);
+                        bodyConfig.pos.push(bbox.center.y);
+                        bodyConfig.pos.push(bbox.center.z);
+                        //tmp solution
+                        bodyConfig.rot.push(rot.x / (OIMO.degtorad || OIMO.TO_RAD));
+                        bodyConfig.rot.push(rot.y / (OIMO.degtorad || OIMO.TO_RAD));
+                        bodyConfig.rot.push(rot.z / (OIMO.degtorad || OIMO.TO_RAD));
+                    }
+                    else {
+                        bodyConfig.pos.push(i.mesh.position.x);
+                        bodyConfig.pos.push(i.mesh.position.y);
+                        bodyConfig.pos.push(i.mesh.position.z);
+                        //tmp solution until https://github.com/lo-th/Oimo.js/pull/37 is merged
+                        bodyConfig.rot.push(0);
+                        bodyConfig.rot.push(0);
+                        bodyConfig.rot.push(0);
+                    }
+                    // register mesh
+                    switch (i.type) {
+                        case BABYLON.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;
+                            var size = Math.max(checkWithEpsilon(radiusX), checkWithEpsilon(radiusY), checkWithEpsilon(radiusZ)) / 2;
+                            bodyConfig.type.push('sphere');
+                            //due to the way oimo works with compounds, add 3 times
+                            bodyConfig.size.push(size);
+                            bodyConfig.size.push(size);
+                            bodyConfig.size.push(size);
+                            break;
+                        case BABYLON.PhysicsEngine.PlaneImpostor:
+                        //TODO Oimo now supports cylinder!
+                        case BABYLON.PhysicsEngine.CylinderImpostor:
+                        case BABYLON.PhysicsEngine.BoxImpostor:
+                        default:
+                            var min = bbox.minimumWorld;
+                            var max = bbox.maximumWorld;
+                            var box = max.subtract(min);
+                            var sizeX = checkWithEpsilon(box.x);
+                            var sizeY = checkWithEpsilon(box.y);
+                            var sizeZ = checkWithEpsilon(box.z);
+                            bodyConfig.type.push('box');
+                            bodyConfig.size.push(sizeX);
+                            bodyConfig.size.push(sizeY);
+                            bodyConfig.size.push(sizeZ);
+                            break;
+                    }
+                    //actually not needed, but hey...
+                    i.mesh.rotationQuaternion = oldQuaternion;
+                });
+                impostor.physicsBody = new OIMO.Body(bodyConfig).body; //this.world.add(bodyConfig);
             }
-            if (!body1 || !body2) {
-                return false;
+            else {
+                this._tmpPositionVector.copyFromFloats(0, 0, 0);
             }
-            if (!options) {
-                options = {};
+            impostor.setDeltaPosition(this._tmpPositionVector);
+            //this._tmpPositionVector.addInPlace(impostor.mesh.getBoundingInfo().boundingBox.center);
+            //this.setPhysicsBodyTransformation(impostor, this._tmpPositionVector, impostor.mesh.rotationQuaternion);
+        };
+        OimoJSPlugin.prototype.removePhysicsBody = function (impostor) {
+            //impostor.physicsBody.dispose();
+            //Same as : (older oimo versions)
+            this.world.removeRigidBody(impostor.physicsBody);
+        };
+        OimoJSPlugin.prototype.generateJoint = function (impostorJoint) {
+            var mainBody = impostorJoint.mainImpostor.physicsBody;
+            var connectedBody = impostorJoint.connectedImpostor.physicsBody;
+            if (!mainBody || !connectedBody) {
+                return;
             }
-            new OIMO.Link({
-                type: options.type,
-                body1: body1,
-                body2: body2,
+            var jointData = impostorJoint.joint.jointData;
+            var options = jointData.nativeParams || {};
+            var type;
+            var nativeJointData = {
+                body1: mainBody,
+                body2: connectedBody,
+                axe1: options.axe1 || (jointData.mainAxis ? jointData.mainAxis.asArray() : null),
+                axe2: options.axe2 || (jointData.connectedAxis ? jointData.connectedAxis.asArray() : null),
+                pos1: options.pos1 || (jointData.mainPivot ? jointData.mainPivot.asArray() : null),
+                pos2: options.pos2 || (jointData.connectedPivot ? jointData.connectedPivot.asArray() : null),
                 min: options.min,
                 max: options.max,
-                axe1: options.axe1,
-                axe2: options.axe2,
-                pos1: [pivot1.x, pivot1.y, pivot1.z],
-                pos2: [pivot2.x, pivot2.y, pivot2.z],
-                collision: options.collision,
+                collision: options.collision || jointData.collision,
                 spring: options.spring,
-                world: this._world
-            });
-            return true;
-        };
-        OimoJSPlugin.prototype.dispose = function () {
-            this._world.clear();
-            while (this._registeredMeshes.length) {
-                this.unregisterMesh(this._registeredMeshes[0].mesh);
+                //supporting older version of Oimo
+                world: this.world
+            };
+            switch (impostorJoint.joint.type) {
+                case BABYLON.PhysicsJoint.BallAndSocketJoint:
+                    type = "jointBall";
+                    break;
+                case BABYLON.PhysicsJoint.DistanceJoint:
+                    type = "jointDistance";
+                    nativeJointData.max = jointData.maxDistance;
+                    break;
+                case BABYLON.PhysicsJoint.PrismaticJoint:
+                    type = "jointPrisme";
+                    break;
+                case BABYLON.PhysicsJoint.SliderJoint:
+                    type = "jointSlide";
+                    break;
+                case BABYLON.PhysicsJoint.WheelJoint:
+                    type = "jointWheel";
+                    break;
+                case BABYLON.PhysicsJoint.HingeJoint:
+                default:
+                    type = "jointHinge";
+                    break;
             }
+            nativeJointData.type = type;
+            impostorJoint.joint.physicsJoint = new OIMO.Link(nativeJointData).joint; //this.world.add(nativeJointData);
+        };
+        OimoJSPlugin.prototype.removeJoint = function (joint) {
+            joint.joint.physicsJoint.dispose();
         };
         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;
+        OimoJSPlugin.prototype.setTransformationFromPhysicsBody = function (impostor) {
+            if (!impostor.physicsBody.sleeping) {
+                //TODO check that
+                if (impostor.physicsBody.shapes.next) {
+                    var parentShape = this._getLastShape(impostor.physicsBody);
+                    impostor.mesh.position.x = parentShape.position.x * OIMO.WORLD_SCALE;
+                    impostor.mesh.position.y = parentShape.position.y * OIMO.WORLD_SCALE;
+                    impostor.mesh.position.z = parentShape.position.z * OIMO.WORLD_SCALE;
                 }
+                else {
+                    impostor.mesh.position.copyFrom(impostor.physicsBody.getPosition());
+                }
+                impostor.mesh.rotationQuaternion.copyFrom(impostor.physicsBody.getQuaternion());
             }
-            return null;
+        };
+        OimoJSPlugin.prototype.setPhysicsBodyTransformation = function (impostor, newPosition, newRotation) {
+            var body = impostor.physicsBody;
+            body.position.init(newPosition.x * OIMO.INV_SCALE, newPosition.y * OIMO.INV_SCALE, newPosition.z * OIMO.INV_SCALE);
+            body.orientation.init(newRotation.w, newRotation.x, newRotation.y, newRotation.z);
+            body.syncShapes();
+            body.awake();
         };
         OimoJSPlugin.prototype._getLastShape = function (body) {
             var lastShape = body.shapes;
@@ -282,50 +250,13 @@ var BABYLON;
             }
             return lastShape;
         };
-        OimoJSPlugin.prototype.runOneStep = function (time) {
-            this._world.step();
-            // Update the position of all registered meshes
-            var i = this._registeredMeshes.length;
-            var m;
-            while (i--) {
-                var body = this._registeredMeshes[i].body.body;
-                var mesh = this._registeredMeshes[i].mesh;
-                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;
-                    }
-                    else {
-                        mesh.position.copyFrom(body.getPosition()).addInPlace(this._registeredMeshes[i].delta);
-                    }
-                    mesh.rotationQuaternion.copyFrom(body.getQuaternion());
-                    mesh.computeWorldMatrix();
-                }
-                //check if the collide callback is set. 
-                if (mesh.onPhysicsCollide) {
-                    var meshUniqueName = mesh.uniqueId;
-                    var contact = this._world.contacts;
-                    while (contact !== null) {
-                        //is this body colliding with any other?
-                        if ((contact.body1.name == mesh.uniqueId || contact.body2.name == mesh.uniqueId) && contact.touching && !contact.body1.sleeping && !contact.body2.sleeping) {
-                            var otherUniqueId = contact.body1.name == mesh.uniqueId ? contact.body2.name : contact.body1.name;
-                            //get the mesh and execute the callback
-                            var otherMesh = mesh.getScene().getMeshByUniqueID(otherUniqueId);
-                            if (otherMesh)
-                                mesh.onPhysicsCollide(otherMesh, contact);
-                        }
-                        contact = contact.next;
-                    }
-                }
-            }
+        OimoJSPlugin.prototype.setVelocity = function (impostor, velocity) {
+            impostor.physicsBody.linearVelocity.init(velocity.x, velocity.y, velocity.z);
+        };
+        OimoJSPlugin.prototype.dispose = function () {
+            this.world.clear();
         };
         return OimoJSPlugin;
-    })();
+    }());
     BABYLON.OimoJSPlugin = OimoJSPlugin;
 })(BABYLON || (BABYLON = {}));

src/Physics/Plugins/babylon.oimoJSPlugin.ts

@@ -1,356 +1,302 @@
 module BABYLON {
     declare var OIMO;
 
-    export class OimoJSPlugin implements IPhysicsEnginePlugin {
-        private _world;
-        private _registeredMeshes = [];
+    export class OimoJSPlugin {
 
-        public name = "oimo";
+        public world: any;
+        public name: string = "OimoJSPlugin";
 
-        private _gravity: Vector3;
-
-        private _checkWithEpsilon(value: number): number {
-            return value < PhysicsEngine.Epsilon ? PhysicsEngine.Epsilon : value;
-        }
-
-        public initialize(iterations?: number): void {
-            this._world = new OIMO.World(null, null, iterations);
-            this._world.clear();
+        constructor(iterations?: number) {
+            this.world = new OIMO.World(1 / 60, 2, iterations, true);
+            this.world.clear();
+            //making sure no stats are calculated
+            this.world.isNoStat = true;
         }
 
-        public setGravity(gravity: Vector3): void {
-            this._gravity = this._world.gravity = gravity;
+        public setGravity(gravity: Vector3) {
+            this.world.gravity.copy(gravity);
         }
 
-        public getGravity(): Vector3 {
-            return this._gravity;
-        }
-
-        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 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 });
-
-            //get the correct bounding box
-            var oldQuaternion = mesh.rotationQuaternion;
-            mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
-            mesh.computeWorldMatrix(true);
-
-            var bodyConfig: any = {
-                name: mesh.uniqueId,
-                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;
-
-                    var size = Math.max(
-                        this._checkWithEpsilon(radiusX),
-                        this._checkWithEpsilon(radiusY),
-                        this._checkWithEpsilon(radiusZ)) / 2;
-
-                    bodyConfig.type = 'sphere';
-                    bodyConfig.size = [size];
-                    break;
+        private _tmpImpostorsArray: Array<PhysicsImpostor> = [];
 
-                case PhysicsEngine.PlaneImpostor:
-                //Oimo "fakes" a cylinder as a box, so why don't we!
-                case PhysicsEngine.CylinderImpostor:
-                case PhysicsEngine.BoxImpostor:
+        public executeStep(delta: number, impostors: Array<PhysicsImpostor>) {
 
-                    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);
-
-                    bodyConfig.type = 'box';
-                    bodyConfig.size = [sizeX, sizeY, sizeZ];
-                    break;
-            }
-
-            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
+            impostors.forEach(function(impostor) {
+                impostor.beforeStep();
             });
-			
-            //for the sake of consistency.
-            mesh.rotationQuaternion = oldQuaternion;
 
-            return body;
-        }
-
-        public registerMeshesAsCompound(parts: PhysicsCompoundBodyPart[], options: PhysicsBodyCreationOptions): any {
-            var types = [],
-                sizes = [],
-                positions = [],
-                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);
-                types.push(bodyParameters.type);
-                sizes.push.apply(sizes, bodyParameters.size);
-                positions.push.apply(positions, bodyParameters.pos);
-                rotations.push.apply(rotations, bodyParameters.rot);
-            }
+            this.world.step();
 
-            var body = new OIMO.Body({
-                name: initialMesh.uniqueId,
-                type: types,
-                size: sizes,
-                pos: positions,
-                rot: rotations,
-                move: options.mass != 0,
-                config: [options.mass, options.friction, options.restitution],
-                world: this._world
+            impostors.forEach((impostor) => {
+                impostor.afterStep();
+                //update the ordered impostors array
+                this._tmpImpostorsArray[impostor.mesh.uniqueId] = impostor;
             });
             
-            //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
-            });
-
-            return body;
-        }
-
-        private _createBodyAsCompound(part: PhysicsCompoundBodyPart, options: PhysicsBodyCreationOptions, initialMesh: AbstractMesh): any {
-            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(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 = {
-                name: mesh.uniqueId,
-                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;
-                    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;
-
-                    bodyParameters.type = 'sphere';
-                    bodyParameters.size = [size, size, size];
+            //check for collisions
+            var contact = this.world.contacts;
 
-                    break;
-
-                case PhysicsEngine.PlaneImpostor:
-                case PhysicsEngine.CylinderImpostor:
-                case PhysicsEngine.BoxImpostor:
-                    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);
+            while (contact !== null) {
+                if (contact.touching && !contact.body1.sleeping && !contact.body2.sleeping) {
+                    contact = contact.next;
+                    continue;
+                }
+                //is this body colliding with any other? get the impostor
+                var mainImpostor = this._tmpImpostorsArray[+contact.body1.name];
+                var collidingImpostor = this._tmpImpostorsArray[+contact.body2.name];
 
-                    bodyParameters.type = 'box';
-                    bodyParameters.size = [sizeX, sizeY, sizeZ];
+                if (!mainImpostor || !collidingImpostor) {
+                    contact = contact.next;
+                    continue;
+                }
 
-                    break;
+                mainImpostor.onCollide({ body: collidingImpostor.physicsBody });
+                collidingImpostor.onCollide({ body: mainImpostor.physicsBody });
+                contact = contact.next;
             }
 
-            mesh.rotationQuaternion = oldQuaternion;
-
-            return bodyParameters;
         }
 
-        public unregisterMesh(mesh: AbstractMesh): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    if (registeredMesh.body) {
-                        this._world.removeRigidBody(registeredMesh.body.body);
-                        this._unbindBody(registeredMesh.body);
-                    }
-                    this._registeredMeshes.splice(index, 1);
-                    return;
-                }
-            }
+        public applyImpulse(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3) {
+            var mass = impostor.physicsBody.massInfo.mass;
+            impostor.physicsBody.applyImpulse(contactPoint.scale(OIMO.INV_SCALE), force.scale(OIMO.INV_SCALE * mass));
         }
-
-        private _unbindBody(body: any): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.body === body) {
-                    registeredMesh.body = null;
+        public applyForce(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3) {
+            Tools.Warn("Oimo doesn't support applying force. Using impule instead.");
+            this.applyImpulse(impostor, force, contactPoint);
+        }
+        public generatePhysicsBody(impostor: PhysicsImpostor) {
+            //parent-child relationship. Does this impostor has a parent impostor?
+            if (impostor.parent) {
+                if (impostor.physicsBody) {
+                    this.removePhysicsBody(impostor);
+                    //TODO is that needed?
+                    impostor.forceUpdate();
                 }
+                return;
             }
-        }
 
-        /**
-         * Update the body position according to the mesh position
-         * @param mesh
-         */
-        public updateBodyPosition = function (mesh: AbstractMesh): void {
+            impostor.mesh.computeWorldMatrix(true);
 
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                var body = registeredMesh.body.body;
-                var updated: boolean = false;
-                var newPosition: Vector3;
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    mesh.computeWorldMatrix(true);
+            if (impostor.isBodyInitRequired()) {
+                if (!impostor.mesh.rotationQuaternion) {
+                    impostor.mesh.rotationQuaternion = Quaternion.RotationYawPitchRoll(impostor.mesh.rotation.y, impostor.mesh.rotation.x, impostor.mesh.rotation.z);
+                }
 
-                    newPosition = mesh.getBoundingInfo().boundingBox.center;
+                
+                var bodyConfig: any = {
+                    name: impostor.mesh.uniqueId,
+                    //Oimo must have mass, also for static objects.
+                    config: [impostor.getParam("mass") || 1, impostor.getParam("friction"), impostor.getParam("restitution")],
+                    size: [],
+                    type: [],
+                    pos: [],
+                    rot: [],
+                    move: impostor.getParam("mass") !== 0,
+                    //Supporting older versions of Oimo
+                    world: this.world
+                };
+
+                var impostors = [impostor];
+                function addToArray(parent: AbstractMesh) {
+                    parent.getChildMeshes().forEach(function(m) {
+                        if (m.physicsImpostor) {
+                            impostors.push(m.physicsImpostor);
+                            m.physicsImpostor._init();
+                        }
+                        addToArray(m);
+                    });
+                }
+                addToArray(impostor.mesh)
 
-                    updated = true;
+                function checkWithEpsilon(value: number): number {
+                    return Math.max(value, PhysicsEngine.Epsilon);
                 }
-                // Case where the parent has been updated
-                else if (registeredMesh.mesh.parent === mesh) {
-                    mesh.computeWorldMatrix(true);
-                    registeredMesh.mesh.computeWorldMatrix(true);
 
-                    newPosition = registeredMesh.mesh.getAbsolutePosition();
+                impostors.forEach((i) => {
+                    
+                    //get the correct bounding box
+                    var oldQuaternion = i.mesh.rotationQuaternion;
+                    var rot = new OIMO.Euler().setFromQuaternion({ x: impostor.mesh.rotationQuaternion.x, y: impostor.mesh.rotationQuaternion.y, z: impostor.mesh.rotationQuaternion.z, s: impostor.mesh.rotationQuaternion.w });
+
+                    i.mesh.rotationQuaternion = new Quaternion(0, 0, 0, 1);
+                    i.mesh.computeWorldMatrix(true);
+                    
+                    var bbox = i.mesh.getBoundingInfo().boundingBox;
+
+                    if (i === impostor) {
+                        
+                        impostor.mesh.position.subtractToRef(impostor.mesh.getBoundingInfo().boundingBox.center, this._tmpPositionVector);
+
+                        //Can also use Array.prototype.push.apply
+                        bodyConfig.pos.push(bbox.center.x);
+                        bodyConfig.pos.push(bbox.center.y);
+                        bodyConfig.pos.push(bbox.center.z);
+                        
+                        //tmp solution
+                        bodyConfig.rot.push(rot.x / (OIMO.degtorad || OIMO.TO_RAD));
+                        bodyConfig.rot.push(rot.y / (OIMO.degtorad || OIMO.TO_RAD));
+                        bodyConfig.rot.push(rot.z / (OIMO.degtorad || OIMO.TO_RAD));
+                    } else {
+                        bodyConfig.pos.push(i.mesh.position.x);
+                        bodyConfig.pos.push(i.mesh.position.y);
+                        bodyConfig.pos.push(i.mesh.position.z);
+                        
+                        //tmp solution until https://github.com/lo-th/Oimo.js/pull/37 is merged
+                        bodyConfig.rot.push(0);
+                        bodyConfig.rot.push(0);
+                        bodyConfig.rot.push(0);
+                    }
+                    
+                    // register mesh
+                    switch (i.type) {
+                        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;
+
+                            var size = Math.max(
+                                checkWithEpsilon(radiusX),
+                                checkWithEpsilon(radiusY),
+                                checkWithEpsilon(radiusZ)) / 2;
+
+                            bodyConfig.type.push('sphere');
+                            //due to the way oimo works with compounds, add 3 times
+                            bodyConfig.size.push(size);
+                            bodyConfig.size.push(size);
+                            bodyConfig.size.push(size);
+                            break;
+
+                        case PhysicsEngine.PlaneImpostor:
+                        //TODO Oimo now supports cylinder!
+                        case PhysicsEngine.CylinderImpostor:
+                        case PhysicsEngine.BoxImpostor:
+                        default:
+
+                            var min = bbox.minimumWorld;
+                            var max = bbox.maximumWorld;
+                            var box = max.subtract(min);
+                            var sizeX = checkWithEpsilon(box.x);
+                            var sizeY = checkWithEpsilon(box.y);
+                            var sizeZ = checkWithEpsilon(box.z);
+
+                            bodyConfig.type.push('box');
+                            bodyConfig.size.push(sizeX);
+                            bodyConfig.size.push(sizeY);
+                            bodyConfig.size.push(sizeZ);
+                            break;
+                    }
+                    
+                    //actually not needed, but hey...
+                    i.mesh.rotationQuaternion = oldQuaternion;
+                });
 
-                    updated = true;
-                }
+                impostor.physicsBody = new OIMO.Body(bodyConfig).body//this.world.add(bodyConfig);
 
-                if (updated) {
-                    body.setPosition(new OIMO.Vec3(newPosition.x, newPosition.y, newPosition.z));
-                    body.setQuaternion(mesh.rotationQuaternion);
-                    body.sleeping = false;
-                    //force Oimo to update the body's position
-                    body.updatePosition(1);
-                }
+            } else {
+                this._tmpPositionVector.copyFromFloats(0, 0, 0);
             }
+            
+            impostor.setDeltaPosition(this._tmpPositionVector);
+
+            //this._tmpPositionVector.addInPlace(impostor.mesh.getBoundingInfo().boundingBox.center);
+            //this.setPhysicsBodyTransformation(impostor, this._tmpPositionVector, impostor.mesh.rotationQuaternion);
         }
 
-        public applyImpulse(mesh: AbstractMesh, force: Vector3, contactPoint: Vector3): void {
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh || registeredMesh.mesh === mesh.parent) {
-                    // Get object mass to have a behaviour similar to cannon.js
-                    var mass = registeredMesh.body.body.massInfo.mass;
-                    // The force is scaled with the mass of object
-                    registeredMesh.body.body.applyImpulse(contactPoint.scale(OIMO.INV_SCALE), force.scale(OIMO.INV_SCALE * mass));
-                    return;
-                }
-            }
+        private _tmpPositionVector: Vector3 = Vector3.Zero();
+
+        public removePhysicsBody(impostor: PhysicsImpostor) {
+            //impostor.physicsBody.dispose();
+            //Same as : (older oimo versions)
+            this.world.removeRigidBody(impostor.physicsBody);
         }
 
-        public createLink(mesh1: AbstractMesh, mesh2: AbstractMesh, pivot1: Vector3, pivot2: Vector3, options?: any): boolean {
-            var body1 = null,
-                body2 = null;
-            for (var index = 0; index < this._registeredMeshes.length; index++) {
-                var registeredMesh = this._registeredMeshes[index];
-                if (registeredMesh.mesh === mesh1) {
-                    body1 = registeredMesh.body.body;
-                } else if (registeredMesh.mesh === mesh2) {
-                    body2 = registeredMesh.body.body;
-                }
-            }
-            if (!body1 || !body2) {
-                return false;
-            }
-            if (!options) {
-                options = {};
+        public generateJoint(impostorJoint: PhysicsImpostorJoint) {
+            var mainBody = impostorJoint.mainImpostor.physicsBody;
+            var connectedBody = impostorJoint.connectedImpostor.physicsBody;
+
+            if (!mainBody || !connectedBody) {
+                return;
             }
+            var jointData = impostorJoint.joint.jointData;
+            var options = jointData.nativeParams || {};
+            var type;
+            var nativeJointData: any = {
+                body1: mainBody,
+                body2: connectedBody,
+
+                axe1: options.axe1 || (jointData.mainAxis ? jointData.mainAxis.asArray() : null),
+                axe2: options.axe2 || (jointData.connectedAxis ? jointData.connectedAxis.asArray() : null),
+                pos1: options.pos1 || (jointData.mainPivot ? jointData.mainPivot.asArray() : null),
+                pos2: options.pos2 || (jointData.connectedPivot ? jointData.connectedPivot.asArray() : null),
 
-            new OIMO.Link({
-                type: options.type,
-                body1: body1,
-                body2: body2,
                 min: options.min,
                 max: options.max,
-                axe1: options.axe1,
-                axe2: options.axe2,
-                pos1: [pivot1.x, pivot1.y, pivot1.z],
-                pos2: [pivot2.x, pivot2.y, pivot2.z],
-                collision: options.collision,
+                collision: options.collision || jointData.collision,
                 spring: options.spring,
-                world: this._world
-            });
-
-            return true;
+                
+                //supporting older version of Oimo
+                world: this.world
 
+            }
+            switch (impostorJoint.joint.type) {
+                case PhysicsJoint.BallAndSocketJoint:
+                    type = "jointBall";
+                    break;
+                case PhysicsJoint.DistanceJoint:
+                    type = "jointDistance";
+                    nativeJointData.max = (<DistanceJointData>jointData).maxDistance
+                    break;
+                case PhysicsJoint.PrismaticJoint:
+                    type = "jointPrisme";
+                    break;
+                case PhysicsJoint.SliderJoint:
+                    type = "jointSlide";
+                    break;
+                case PhysicsJoint.WheelJoint:
+                    type = "jointWheel";
+                    break;
+                case PhysicsJoint.HingeJoint:
+                default:
+                    type = "jointHinge";
+                    break;
+            }
+            nativeJointData.type = type;
+            impostorJoint.joint.physicsJoint = new OIMO.Link(nativeJointData).joint//this.world.add(nativeJointData);
         }
 
-        public dispose(): void {
-            this._world.clear();
-            while (this._registeredMeshes.length) {
-                this.unregisterMesh(this._registeredMeshes[0].mesh);
-            }
+        public removeJoint(joint: PhysicsImpostorJoint) {
+            joint.joint.physicsJoint.dispose();
         }
 
         public isSupported(): boolean {
             return OIMO !== undefined;
         }
 
-        public getWorldObject(): any {
-            return this._world;
-        }
+        public setTransformationFromPhysicsBody(impostor: PhysicsImpostor) {
+            if (!impostor.physicsBody.sleeping) {
+                //TODO check that
+                if (impostor.physicsBody.shapes.next) {
+                    var parentShape = this._getLastShape(impostor.physicsBody);
+                    impostor.mesh.position.x = parentShape.position.x * OIMO.WORLD_SCALE;
+                    impostor.mesh.position.y = parentShape.position.y * OIMO.WORLD_SCALE;
+                    impostor.mesh.position.z = parentShape.position.z * OIMO.WORLD_SCALE;
+                } else {
+                    impostor.mesh.position.copyFrom(impostor.physicsBody.getPosition());
 
-        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;
                 }
+                impostor.mesh.rotationQuaternion.copyFrom(impostor.physicsBody.getQuaternion());
             }
-            return null;
+        }
+
+        public setPhysicsBodyTransformation(impostor: PhysicsImpostor, newPosition: Vector3, newRotation: Quaternion) {
+            var body = impostor.physicsBody;
+
+            body.position.init(newPosition.x * OIMO.INV_SCALE, newPosition.y * OIMO.INV_SCALE, newPosition.z * OIMO.INV_SCALE);
+            
+            body.orientation.init(newRotation.w, newRotation.x, newRotation.y, newRotation.z);
+            body.syncShapes();
+            body.awake();
         }
 
         private _getLastShape(body: any): any {
@@ -360,54 +306,13 @@ module BABYLON {
             }
             return lastShape;
         }
+        
+        public setVelocity(impostor: PhysicsImpostor, velocity: Vector3) {
+            impostor.physicsBody.linearVelocity.init(velocity.x, velocity.y, velocity.z);
+        }
 
-        public runOneStep(time: number): void {
-            this._world.step();
-
-            // Update the position of all registered meshes
-            var i = this._registeredMeshes.length;
-            var m;
-            while (i--) {
-
-                var body = this._registeredMeshes[i].body.body;
-                var mesh = this._registeredMeshes[i].mesh;
-
-                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;
-                    } else {
-                        mesh.position.copyFrom(body.getPosition()).addInPlace(this._registeredMeshes[i].delta);
-
-                    }
-                    mesh.rotationQuaternion.copyFrom(body.getQuaternion());
-                    mesh.computeWorldMatrix();
-                }
-                
-                //check if the collide callback is set. 
-                if (mesh.onPhysicsCollide) {
-                    var meshUniqueName = mesh.uniqueId;
-                    var contact = this._world.contacts;
-                    while (contact !== null) {
-                        //is this body colliding with any other?
-                        if ((contact.body1.name == mesh.uniqueId || contact.body2.name == mesh.uniqueId) && contact.touching && /* !contact.sleeping*/ !contact.body1.sleeping && !contact.body2.sleeping) {
-                            var otherUniqueId = contact.body1.name == mesh.uniqueId ? contact.body2.name : contact.body1.name;
-                            //get the mesh and execute the callback
-                            var otherMesh = mesh.getScene().getMeshByUniqueID(otherUniqueId);
-                            if (otherMesh)
-                                mesh.onPhysicsCollide(otherMesh, contact); 
-                        }
-                        contact = contact.next;
-                    }
-                }
-            }
+        public dispose() {
+            this.world.clear();
         }
     }
 }

src/Physics/babylon.physicsEngine.js

@@ -1,75 +1,115 @@
 var BABYLON;
 (function (BABYLON) {
     var PhysicsEngine = (function () {
-        function PhysicsEngine(plugin) {
-            this._currentPlugin = plugin || new BABYLON.OimoJSPlugin();
-        }
-        PhysicsEngine.prototype._initialize = function (gravity) {
-            this._currentPlugin.initialize();
-            this._setGravity(gravity);
-        };
-        PhysicsEngine.prototype._runOneStep = function (delta) {
-            if (delta > 0.1) {
-                delta = 0.1;
-            }
-            else if (delta <= 0) {
-                delta = 1.0 / 60.0;
+        function PhysicsEngine(gravity, _physicsPlugin) {
+            if (_physicsPlugin === void 0) { _physicsPlugin = new BABYLON.CannonJSPlugin(); }
+            this._physicsPlugin = _physicsPlugin;
+            //new methods and parameters
+            this._impostors = [];
+            this._joints = [];
+            if (!this._physicsPlugin.isSupported()) {
+                throw new Error("Physics Engine " + this._physicsPlugin.name + " cannot be found. "
+                    + "Please make sure it is included.");
             }
-            this._currentPlugin.runOneStep(delta);
-        };
-        PhysicsEngine.prototype._setGravity = function (gravity) {
-            this.gravity = gravity || new BABYLON.Vector3(0, -9.807, 0);
-            this._currentPlugin.setGravity(this.gravity);
-        };
-        PhysicsEngine.prototype._getGravity = function () {
-            return this._currentPlugin.getGravity();
-        };
-        PhysicsEngine.prototype._registerMesh = function (mesh, impostor, options) {
-            return this._currentPlugin.registerMesh(mesh, impostor, options);
-        };
-        PhysicsEngine.prototype._registerMeshesAsCompound = function (parts, options) {
-            return this._currentPlugin.registerMeshesAsCompound(parts, options);
+            gravity = gravity || new BABYLON.Vector3(0, -9.807, 0);
+            this.setGravity(gravity);
+        }
+        PhysicsEngine.prototype.setGravity = function (gravity) {
+            this.gravity = gravity;
+            this._physicsPlugin.setGravity(this.gravity);
         };
-        PhysicsEngine.prototype._unregisterMesh = function (mesh) {
-            this._currentPlugin.unregisterMesh(mesh);
+        PhysicsEngine.prototype.dispose = function () {
+            this._impostors.forEach(function (impostor) {
+                impostor.dispose();
+            });
+            this._physicsPlugin.dispose();
         };
-        PhysicsEngine.prototype._applyImpulse = function (mesh, force, contactPoint) {
-            this._currentPlugin.applyImpulse(mesh, force, contactPoint);
+        PhysicsEngine.prototype.getPhysicsPluginName = function () {
+            return this._physicsPlugin.name;
         };
-        PhysicsEngine.prototype._createLink = function (mesh1, mesh2, pivot1, pivot2, options) {
-            return this._currentPlugin.createLink(mesh1, mesh2, pivot1, pivot2, options);
+        PhysicsEngine.prototype.addImpostor = function (impostor) {
+            this._impostors.push(impostor);
+            //if no parent, generate the body
+            if (!impostor.parent) {
+                this._physicsPlugin.generatePhysicsBody(impostor);
+            }
         };
-        PhysicsEngine.prototype._updateBodyPosition = function (mesh) {
-            this._currentPlugin.updateBodyPosition(mesh);
+        PhysicsEngine.prototype.removeImpostor = function (impostor) {
+            var index = this._impostors.indexOf(impostor);
+            if (index > -1) {
+                var removed = this._impostors.splice(index, 1);
+                //Is it needed?
+                if (removed.length) {
+                    //this will also remove it from the world.
+                    removed[0].physicsBody = null;
+                }
+            }
         };
-        PhysicsEngine.prototype.dispose = function () {
-            this._currentPlugin.dispose();
+        PhysicsEngine.prototype.addJoint = function (mainImpostor, connectedImpostor, joint) {
+            var impostorJoint = {
+                mainImpostor: mainImpostor,
+                connectedImpostor: connectedImpostor,
+                joint: joint
+            };
+            this._joints.push(impostorJoint);
+            this._physicsPlugin.generateJoint(impostorJoint);
         };
-        PhysicsEngine.prototype.isSupported = function () {
-            return this._currentPlugin.isSupported();
+        PhysicsEngine.prototype.removeJoint = function (mainImpostor, connectedImpostor, joint) {
+            var matchingJoints = this._joints.filter(function (impostorJoint) {
+                return (impostorJoint.connectedImpostor === connectedImpostor
+                    && impostorJoint.joint === joint
+                    && impostorJoint.mainImpostor === mainImpostor);
+            });
+            if (matchingJoints.length) {
+                this._physicsPlugin.removeJoint(matchingJoints[0]);
+            }
         };
-        PhysicsEngine.prototype.getPhysicsBodyOfMesh = function (mesh) {
-            return this._currentPlugin.getPhysicsBodyOfMesh(mesh);
+        /**
+         * Called by the scene. no need to call it.
+         */
+        PhysicsEngine.prototype._step = function (delta) {
+            var _this = this;
+            //check if any mesh has no body / requires an update
+            this._impostors.forEach(function (impostor) {
+                if (impostor.isBodyInitRequired()) {
+                    _this._physicsPlugin.generatePhysicsBody(impostor);
+                }
+            });
+            if (delta > 0.1) {
+                delta = 0.1;
+            }
+            else if (delta <= 0) {
+                delta = 1.0 / 60.0;
+            }
+            this._physicsPlugin.executeStep(delta, this._impostors);
         };
-        PhysicsEngine.prototype.getPhysicsPluginName = function () {
-            return this._currentPlugin.name;
+        PhysicsEngine.prototype.getPhysicsPlugin = function () {
+            return this._physicsPlugin;
         };
-        PhysicsEngine.prototype.getWorldObject = function () {
-            return this._currentPlugin.getWorldObject();
+        PhysicsEngine.prototype.getImpostorWithPhysicsBody = function (body) {
+            for (var i = 0; i < this._impostors.length; ++i) {
+                if (this._impostors[i].physicsBody === body) {
+                    return this._impostors[i];
+                }
+            }
         };
-        // Statics
-        PhysicsEngine.NoImpostor = 0;
-        PhysicsEngine.SphereImpostor = 1;
-        PhysicsEngine.BoxImpostor = 2;
-        PhysicsEngine.PlaneImpostor = 3;
-        PhysicsEngine.MeshImpostor = 4;
-        PhysicsEngine.CapsuleImpostor = 5;
-        PhysicsEngine.ConeImpostor = 6;
-        PhysicsEngine.CylinderImpostor = 7;
-        PhysicsEngine.ConvexHullImpostor = 8;
-        PhysicsEngine.HeightmapImpostor = 9;
+        // Statics, Legacy support.
+        /**
+         * @Deprecated
+         *
+         */
+        PhysicsEngine.NoImpostor = BABYLON.PhysicsImpostor.NoImpostor;
+        PhysicsEngine.SphereImpostor = BABYLON.PhysicsImpostor.SphereImpostor;
+        PhysicsEngine.BoxImpostor = BABYLON.PhysicsImpostor.BoxImpostor;
+        PhysicsEngine.PlaneImpostor = BABYLON.PhysicsImpostor.PlaneImpostor;
+        PhysicsEngine.MeshImpostor = BABYLON.PhysicsImpostor.MeshImpostor;
+        PhysicsEngine.CapsuleImpostor = BABYLON.PhysicsImpostor.CapsuleImpostor;
+        PhysicsEngine.ConeImpostor = BABYLON.PhysicsImpostor.ConeImpostor;
+        PhysicsEngine.CylinderImpostor = BABYLON.PhysicsImpostor.CylinderImpostor;
+        PhysicsEngine.ConvexHullImpostor = BABYLON.PhysicsImpostor.ConvexHullImpostor;
+        PhysicsEngine.HeightmapImpostor = BABYLON.PhysicsImpostor.HeightmapImpostor;
         PhysicsEngine.Epsilon = 0.001;
         return PhysicsEngine;
-    })();
+    }());
     BABYLON.PhysicsEngine = PhysicsEngine;
 })(BABYLON || (BABYLON = {}));

src/Physics/babylon.physicsEngine.ts

@@ -1,121 +1,155 @@
 module BABYLON {
-    export interface IPhysicsEnginePlugin {
-        name: string;
-        initialize(iterations?: number);
-        setGravity(gravity: Vector3): void;
-        getGravity(): Vector3;
-        runOneStep(delta: number): void;
-        registerMesh(mesh: AbstractMesh, impostor: number, options: PhysicsBodyCreationOptions): any;
-        registerMeshesAsCompound(parts: PhysicsCompoundBodyPart[], options: PhysicsBodyCreationOptions): any;
-        unregisterMesh(mesh: AbstractMesh);
-        applyImpulse(mesh: AbstractMesh, force: Vector3, contactPoint: Vector3): void;
-        createLink(mesh1: AbstractMesh, mesh2: AbstractMesh, pivot1: Vector3, pivot2: Vector3, options?: any): boolean;
-        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 {
-        mass: number;
-        friction: number;
-        restitution: number;
-    }
 
-    export interface PhysicsCompoundBodyPart {
-        mesh: Mesh;
-        impostor: number;
+    export interface PhysicsImpostorJoint {
+        mainImpostor: PhysicsImpostor;
+        connectedImpostor: PhysicsImpostor;
+        joint: PhysicsJoint;
     }
 
     export class PhysicsEngine {
-        public gravity: Vector3;
-
-        private _currentPlugin: IPhysicsEnginePlugin;
 
-        constructor(plugin?: IPhysicsEnginePlugin) {
-            this._currentPlugin = plugin || new OimoJSPlugin();
-        }
-
-        public _initialize(gravity?: Vector3) {
-            this._currentPlugin.initialize();
-            this._setGravity(gravity);
-        }
+        public gravity: Vector3;
 
-        public _runOneStep(delta: number): void {
-            if (delta > 0.1) {
-                delta = 0.1;
-            } else if (delta <= 0) {
-                delta = 1.0 / 60.0;
+        constructor(gravity?: Vector3, private _physicsPlugin: IPhysicsEnginePlugin = new CannonJSPlugin()) {
+            if (!this._physicsPlugin.isSupported()) {
+                throw new Error("Physics Engine " + this._physicsPlugin.name + " cannot be found. "
+                    + "Please make sure it is included.")
             }
-
-            this._currentPlugin.runOneStep(delta);
+            gravity = gravity || new Vector3(0, -9.807, 0)
+            this.setGravity(gravity);
         }
 
-        public _setGravity(gravity: Vector3): void {
-            this.gravity = gravity || new Vector3(0, -9.807, 0);
-            this._currentPlugin.setGravity(this.gravity);
+        public setGravity(gravity: Vector3): void {
+            this.gravity = gravity;
+            this._physicsPlugin.setGravity(this.gravity);
         }
 
-        public _getGravity(): Vector3 {
-            return this._currentPlugin.getGravity();
-        }
-
-        public _registerMesh(mesh: AbstractMesh, impostor: number, options: PhysicsBodyCreationOptions): any {
-            return this._currentPlugin.registerMesh(mesh, impostor, options);
+        public dispose(): void {
+            this._impostors.forEach(function(impostor) {
+                impostor.dispose();
+            })
+            this._physicsPlugin.dispose();
         }
 
-        public _registerMeshesAsCompound(parts: PhysicsCompoundBodyPart[], options: PhysicsBodyCreationOptions): any {
-            return this._currentPlugin.registerMeshesAsCompound(parts, options);
-        }
+        public getPhysicsPluginName(): string {
+            return this._physicsPlugin.name;
+        }
+
+        // Statics, Legacy support.
+        /**
+         * @Deprecated
+         *  
+         */
+        public static NoImpostor = PhysicsImpostor.NoImpostor;
+        public static SphereImpostor = PhysicsImpostor.SphereImpostor;
+        public static BoxImpostor = PhysicsImpostor.BoxImpostor;
+        public static PlaneImpostor = PhysicsImpostor.PlaneImpostor;
+        public static MeshImpostor = PhysicsImpostor.MeshImpostor;
+        public static CapsuleImpostor = PhysicsImpostor.CapsuleImpostor;
+        public static ConeImpostor = PhysicsImpostor.ConeImpostor;
+        public static CylinderImpostor = PhysicsImpostor.CylinderImpostor;
+        public static ConvexHullImpostor = PhysicsImpostor.ConvexHullImpostor;
+        public static HeightmapImpostor = PhysicsImpostor.HeightmapImpostor;
 
-        public _unregisterMesh(mesh: AbstractMesh): void {
-            this._currentPlugin.unregisterMesh(mesh);
+        public static Epsilon = 0.001;
+        
+        //new methods and parameters
+        
+        private _impostors: Array<PhysicsImpostor> = [];
+        private _joints: Array<PhysicsImpostorJoint> = [];
+
+        public addImpostor(impostor: PhysicsImpostor) {
+            this._impostors.push(impostor);
+            //if no parent, generate the body
+            if(!impostor.parent) {
+                this._physicsPlugin.generatePhysicsBody(impostor);
+            }
         }
 
-        public _applyImpulse(mesh: AbstractMesh, force: Vector3, contactPoint: Vector3): void {
-            this._currentPlugin.applyImpulse(mesh, force, contactPoint);
+        public removeImpostor(impostor: PhysicsImpostor) {
+            var index = this._impostors.indexOf(impostor);
+            if (index > -1) {
+                var removed = this._impostors.splice(index, 1);
+                //Is it needed?
+                if(removed.length) {
+                    //this will also remove it from the world.
+                    removed[0].physicsBody = null;
+                }
+            }
         }
 
-        public _createLink(mesh1: AbstractMesh, mesh2: AbstractMesh, pivot1: Vector3, pivot2: Vector3, options?: any): boolean {
-            return this._currentPlugin.createLink(mesh1, mesh2, pivot1, pivot2, options);
+        public addJoint(mainImpostor: PhysicsImpostor, connectedImpostor: PhysicsImpostor, joint: PhysicsJoint) {
+            var impostorJoint = {
+                mainImpostor: mainImpostor,
+                connectedImpostor: connectedImpostor,
+                joint: joint
+            }
+            this._joints.push(impostorJoint);
+            this._physicsPlugin.generateJoint(impostorJoint);
+        }
+
+        public removeJoint(mainImpostor: PhysicsImpostor, connectedImpostor: PhysicsImpostor, joint: PhysicsJoint) {
+            var matchingJoints = this._joints.filter(function(impostorJoint) {
+                return (impostorJoint.connectedImpostor === connectedImpostor
+                    && impostorJoint.joint === joint
+                    && impostorJoint.mainImpostor === mainImpostor)
+            });
+            if(matchingJoints.length) {
+                this._physicsPlugin.removeJoint(matchingJoints[0]);
+                //TODO remove it from the list as well
+                
+            }
         }
 
-        public _updateBodyPosition(mesh: AbstractMesh): void {
-            this._currentPlugin.updateBodyPosition(mesh);
-        }
+        /**
+         * Called by the scene. no need to call it.
+         */
+        public _step(delta: number) {
+            //check if any mesh has no body / requires an update
+            this._impostors.forEach((impostor) => {
 
-        public dispose(): void {
-            this._currentPlugin.dispose();
-        }
+                if (impostor.isBodyInitRequired()) {
+                    this._physicsPlugin.generatePhysicsBody(impostor);
+                }
+            });
 
-        public isSupported(): boolean {
-            return this._currentPlugin.isSupported();
-        }
+            if (delta > 0.1) {
+                delta = 0.1;
+            } else if (delta <= 0) {
+                delta = 1.0 / 60.0;
+            }
 
-        public getPhysicsBodyOfMesh(mesh: AbstractMesh) {
-            return this._currentPlugin.getPhysicsBodyOfMesh(mesh);
+            this._physicsPlugin.executeStep(delta, this._impostors);
         }
 
-        public getPhysicsPluginName(): string {
-            return this._currentPlugin.name;
+        public getPhysicsPlugin(): IPhysicsEnginePlugin {
+            return this._physicsPlugin;
         }
 
-        public getWorldObject(): any {
-            return this._currentPlugin.getWorldObject();
+        public getImpostorWithPhysicsBody(body: any): PhysicsImpostor {
+            for (var i = 0; i < this._impostors.length; ++i) {
+                if (this._impostors[i].physicsBody === body) {
+                    return this._impostors[i];
+                }
+            }
         }
+    }
 
-        // Statics
-        public static NoImpostor = 0;
-        public static SphereImpostor = 1;
-        public static BoxImpostor = 2;
-        public static PlaneImpostor = 3;
-        public static MeshImpostor = 4;
-        public static CapsuleImpostor = 5;
-        public static ConeImpostor = 6;
-        public static CylinderImpostor = 7;
-        public static ConvexHullImpostor = 8;
-        public static HeightmapImpostor = 9;
-        public static Epsilon = 0.001;
+    export interface IPhysicsEnginePlugin {
+        world: any;
+        name: string;
+        setGravity(gravity: Vector3);
+        executeStep(delta: number, impostors: Array<PhysicsImpostor>): void; //not forgetting pre and post events
+        applyImpulse(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3);
+        applyForce(impostor: PhysicsImpostor, force: Vector3, contactPoint: Vector3);
+        generatePhysicsBody(impostor: PhysicsImpostor);
+        removePhysicsBody(impostor: PhysicsImpostor);
+        generateJoint(joint: PhysicsImpostorJoint);
+        removeJoint(joint: PhysicsImpostorJoint)
+        isSupported(): boolean;
+        setTransformationFromPhysicsBody(impostor: PhysicsImpostor);
+        setPhysicsBodyTransformation(impostor: PhysicsImpostor, newPosition:Vector3, newRotation: Quaternion);
+        setVelocity(impostor: PhysicsImpostor, velocity: Vector3);
+        dispose();
     }
 }

src/Physics/babylon.physicsImpostor.js

@@ -0,0 +1,221 @@
+var BABYLON;
+(function (BABYLON) {
+    var PhysicsImpostor = (function () {
+        function PhysicsImpostor(_mesh, type, _options) {
+            var _this = this;
+            if (_options === void 0) { _options = { mass: 0 }; }
+            this._mesh = _mesh;
+            this.type = type;
+            this._options = _options;
+            this._bodyUpdateRequired = false;
+            this._onBeforePhysicsStepCallbacks = new Array();
+            this._onAfterPhysicsStepCallbacks = new Array();
+            this._onPhysicsCollideCallbacks = new Array();
+            this._deltaPosition = BABYLON.Vector3.Zero();
+            this._deltaRotation = new BABYLON.Quaternion();
+            this._tmpPositionWithDelta = BABYLON.Vector3.Zero();
+            this._tmpRotationWithDelta = new BABYLON.Quaternion();
+            this.beforeStep = function () {
+                _this.mesh.position.subtractToRef(_this._deltaPosition, _this._tmpPositionWithDelta);
+                //conjugate deltaRotation
+                _this._tmpRotationWithDelta.copyFrom(_this._deltaRotation);
+                _this._tmpRotationWithDelta.multiplyInPlace(_this.mesh.rotationQuaternion);
+                _this._physicsEngine.getPhysicsPlugin().setPhysicsBodyTransformation(_this, _this._tmpPositionWithDelta, _this._tmpRotationWithDelta);
+                _this._onBeforePhysicsStepCallbacks.forEach(function (func) {
+                    func(_this);
+                });
+            };
+            this.afterStep = function () {
+                _this._onAfterPhysicsStepCallbacks.forEach(function (func) {
+                    func(_this);
+                });
+                _this._physicsEngine.getPhysicsPlugin().setTransformationFromPhysicsBody(_this);
+                _this.mesh.position.addInPlace(_this._deltaPosition);
+                _this.mesh.rotationQuaternion.multiplyInPlace(_this._deltaRotation);
+            };
+            //event and body object due to cannon's event-based architecture.
+            this.onCollide = function (e) {
+                var otherImpostor = _this._physicsEngine.getImpostorWithPhysicsBody(e.body);
+                if (otherImpostor) {
+                    _this._onPhysicsCollideCallbacks.forEach(function (func) {
+                        func(_this, otherImpostor);
+                    });
+                }
+            };
+            //default options params
+            this._options.mass = (_options.mass === void 0) ? 0 : _options.mass;
+            this._options.friction = (_options.friction === void 0) ? 0.2 : _options.friction;
+            this._options.restitution = (_options.restitution === void 0) ? 0.2 : _options.restitution;
+            this._physicsEngine = this._mesh.getScene().getPhysicsEngine();
+            this._joints = [];
+            //If the mesh has a parent, don't initialize the physicsBody. Instead wait for the parent to do that.
+            if (!this._mesh.parent) {
+                this._init();
+            }
+        }
+        PhysicsImpostor.prototype._init = function () {
+            this._physicsEngine.removeImpostor(this);
+            this.physicsBody = null;
+            this._parent = this._parent || this._getPhysicsParent();
+            if (!this.parent) {
+                this._physicsEngine.addImpostor(this);
+            }
+        };
+        PhysicsImpostor.prototype._getPhysicsParent = function () {
+            if (this.mesh.parent instanceof BABYLON.AbstractMesh) {
+                var parentMesh = this.mesh.parent;
+                return parentMesh.getPhysicsImpostor();
+            }
+            return;
+        };
+        PhysicsImpostor.prototype.isBodyInitRequired = function () {
+            return this._bodyUpdateRequired || (!this._physicsBody && !this._parent);
+        };
+        PhysicsImpostor.prototype.setScalingUpdated = function (updated) {
+            this.forceUpdate();
+        };
+        PhysicsImpostor.prototype.forceUpdate = function () {
+            this._init();
+            if (this.parent) {
+                this.parent.forceUpdate();
+            }
+        };
+        Object.defineProperty(PhysicsImpostor.prototype, "mesh", {
+            get: function () {
+                return this._mesh;
+            },
+            enumerable: true,
+            configurable: true
+        });
+        Object.defineProperty(PhysicsImpostor.prototype, "physicsBody", {
+            get: function () {
+                return this.parent ? this.parent.physicsBody : this._physicsBody;
+            },
+            set: function (physicsBody) {
+                if (this._physicsBody) {
+                    this._physicsEngine.getPhysicsPlugin().removePhysicsBody(this);
+                }
+                this._physicsBody = physicsBody;
+                this.resetUpdateFlags();
+            },
+            enumerable: true,
+            configurable: true
+        });
+        Object.defineProperty(PhysicsImpostor.prototype, "parent", {
+            get: function () {
+                return this._parent;
+            },
+            enumerable: true,
+            configurable: true
+        });
+        PhysicsImpostor.prototype.resetUpdateFlags = function () {
+            this._bodyUpdateRequired = false;
+        };
+        PhysicsImpostor.prototype.getOptions = function () {
+            return this._options;
+        };
+        PhysicsImpostor.prototype.getParam = function (paramName) {
+            return this._options[paramName];
+        };
+        PhysicsImpostor.prototype.setParam = function (paramName, value) {
+            this._options[paramName] = value;
+            this._bodyUpdateRequired = true;
+        };
+        PhysicsImpostor.prototype.setVelocity = function (velocity) {
+            this._physicsEngine.getPhysicsPlugin().setVelocity(this, velocity);
+        };
+        PhysicsImpostor.prototype.executeNativeFunction = function (func) {
+            func(this._physicsEngine.getPhysicsPlugin().world, this.physicsBody);
+        };
+        PhysicsImpostor.prototype.registerBeforePhysicsStep = function (func) {
+            this._onBeforePhysicsStepCallbacks.push(func);
+        };
+        PhysicsImpostor.prototype.unregisterBeforePhysicsStep = function (func) {
+            var index = this._onBeforePhysicsStepCallbacks.indexOf(func);
+            if (index > -1) {
+                this._onBeforePhysicsStepCallbacks.splice(index, 1);
+            }
+            else {
+                BABYLON.Tools.Warn("Function to remove was not found");
+            }
+        };
+        PhysicsImpostor.prototype.registerAfterPhysicsStep = function (func) {
+            this._onAfterPhysicsStepCallbacks.push(func);
+        };
+        PhysicsImpostor.prototype.unregisterAfterPhysicsStep = function (func) {
+            var index = this._onAfterPhysicsStepCallbacks.indexOf(func);
+            if (index > -1) {
+                this._onAfterPhysicsStepCallbacks.splice(index, 1);
+            }
+            else {
+                BABYLON.Tools.Warn("Function to remove was not found");
+            }
+        };
+        PhysicsImpostor.prototype.registerOnPhysicsCollide = function (func) {
+            this._onPhysicsCollideCallbacks.push(func);
+        };
+        PhysicsImpostor.prototype.unregisterOnPhysicsCollide = function (func) {
+            var index = this._onPhysicsCollideCallbacks.indexOf(func);
+            if (index > -1) {
+                this._onPhysicsCollideCallbacks.splice(index, 1);
+            }
+            else {
+                BABYLON.Tools.Warn("Function to remove was not found");
+            }
+        };
+        PhysicsImpostor.prototype.applyForce = function (force, contactPoint) {
+            this._physicsEngine.getPhysicsPlugin().applyForce(this, force, contactPoint);
+        };
+        PhysicsImpostor.prototype.applyImpulse = function (force, contactPoint) {
+            this._physicsEngine.getPhysicsPlugin().applyImpulse(this, force, contactPoint);
+        };
+        PhysicsImpostor.prototype.createJoint = function (otherImpostor, jointType, jointData) {
+            var joint = new BABYLON.PhysicsJoint(jointType, jointData);
+            this.addJoint(otherImpostor, joint);
+        };
+        PhysicsImpostor.prototype.addJoint = function (otherImpostor, joint) {
+            this._joints.push({
+                otherImpostor: otherImpostor,
+                joint: joint
+            });
+            this._physicsEngine.addJoint(this, otherImpostor, joint);
+        };
+        //TODO
+        PhysicsImpostor.prototype.dispose = function (disposeChildren) {
+            if (disposeChildren === void 0) { disposeChildren = true; }
+            this.physicsBody = null;
+            if (this.parent) {
+                this.parent.forceUpdate();
+            }
+            else {
+                this.mesh.getChildMeshes().forEach(function (mesh) {
+                    if (mesh.physicsImpostor) {
+                        if (disposeChildren) {
+                            mesh.physicsImpostor.dispose();
+                            mesh.physicsImpostor = null;
+                        }
+                    }
+                });
+            }
+        };
+        PhysicsImpostor.prototype.setDeltaPosition = function (position) {
+            this._deltaPosition.copyFrom(position);
+        };
+        PhysicsImpostor.prototype.setDeltaRotation = function (rotation) {
+            this._deltaRotation.copyFrom(rotation);
+        };
+        //Impostor types
+        PhysicsImpostor.NoImpostor = 0;
+        PhysicsImpostor.SphereImpostor = 1;
+        PhysicsImpostor.BoxImpostor = 2;
+        PhysicsImpostor.PlaneImpostor = 3;
+        PhysicsImpostor.MeshImpostor = 4;
+        PhysicsImpostor.CapsuleImpostor = 5;
+        PhysicsImpostor.ConeImpostor = 6;
+        PhysicsImpostor.CylinderImpostor = 7;
+        PhysicsImpostor.ConvexHullImpostor = 8;
+        PhysicsImpostor.HeightmapImpostor = 9;
+        return PhysicsImpostor;
+    }());
+    BABYLON.PhysicsImpostor = PhysicsImpostor;
+})(BABYLON || (BABYLON = {}));

src/Physics/babylon.physicsImpostor.ts

@@ -0,0 +1,315 @@
+module BABYLON {
+
+    export interface PhysicsImpostorParameters {
+        mass: number;
+        friction?: number;
+        restitution?: number;
+        nativeOptions?: any;
+    }
+
+    export class PhysicsImpostor {
+
+        private _physicsEngine: PhysicsEngine;
+        //The native cannon/oimo/energy physics body object.
+        private _physicsBody: any;
+        private _bodyUpdateRequired: boolean = false;
+
+        private _onBeforePhysicsStepCallbacks = new Array<(impostor: PhysicsImpostor) => void>();
+        private _onAfterPhysicsStepCallbacks = new Array<(impostor: PhysicsImpostor) => void>();
+        private _onPhysicsCollideCallbacks = new Array<(collider: PhysicsImpostor, collidedAgainst: PhysicsImpostor) => void>();
+
+        private _deltaPosition: Vector3 = Vector3.Zero();
+        private _deltaRotation: Quaternion = new Quaternion();
+        
+        //If set, this is this impostor's parent
+        private _parent: PhysicsImpostor;
+
+        private _joints: Array<{
+            joint: PhysicsJoint,
+            otherImpostor: PhysicsImpostor
+        }>;
+
+        constructor(private _mesh: AbstractMesh, public type: number, private _options: PhysicsImpostorParameters = { mass: 0 }) {
+            //default options params
+            this._options.mass = (_options.mass === void 0) ? 0 : _options.mass
+            this._options.friction = (_options.friction === void 0) ? 0.2 : _options.friction
+            this._options.restitution = (_options.restitution === void 0) ? 0.2 : _options.restitution
+            this._physicsEngine = this._mesh.getScene().getPhysicsEngine();
+            this._joints = [];
+            //If the mesh has a parent, don't initialize the physicsBody. Instead wait for the parent to do that.
+            if (!this._mesh.parent) {
+                this._init();
+            }
+        }
+
+        /**
+         * This function will completly initialize this impostor.
+         * It will create a new body - but only if this mesh has no parent.
+         * If it has, this impostor will not be used other than to define the impostor
+         * of the child mesh.
+         */
+        public _init() {
+            this._physicsEngine.removeImpostor(this);
+            this.physicsBody = null;
+            this._parent = this._parent || this._getPhysicsParent();
+            if (!this.parent) {
+                this._physicsEngine.addImpostor(this);
+            }
+        }
+
+        private _getPhysicsParent(): PhysicsImpostor {
+            if (this.mesh.parent instanceof AbstractMesh) {
+                var parentMesh: AbstractMesh = <AbstractMesh>this.mesh.parent;
+                return parentMesh.getPhysicsImpostor();
+            }
+            return;
+        }
+
+        /**
+         * Should a new body be generated.
+         */
+        public isBodyInitRequired(): boolean {
+            return this._bodyUpdateRequired || (!this._physicsBody && !this._parent);
+        }
+
+        public setScalingUpdated(updated: boolean) {
+            this.forceUpdate();
+        }
+
+        /**
+         * Force a regeneration of this or the parent's impostor's body.
+         * Use under cautious - This will remove all joints already implemented.
+         */
+        public forceUpdate() {
+            this._init();
+            if (this.parent) {
+                this.parent.forceUpdate();
+            }
+        }
+
+        public get mesh(): AbstractMesh {
+            return this._mesh;
+        }
+
+        /**
+         * Gets the body that holds this impostor. Either its own, or its parent.
+         */
+        public get physicsBody(): any {
+            return this._parent ? this._parent.physicsBody : this._physicsBody;
+        }
+
+        public get parent() {
+            return this._parent;
+        }
+
+        /**
+         * Set the physics body. Used mainly by the physics engine/plugin
+         */
+        public set physicsBody(physicsBody: any) {
+            if (this._physicsBody) {
+                this._physicsEngine.getPhysicsPlugin().removePhysicsBody(this);
+            }
+            this._physicsBody = physicsBody;
+            this.resetUpdateFlags();
+        }
+
+        public resetUpdateFlags() {
+            this._bodyUpdateRequired = false;
+        }
+
+        /**
+         * Get a specific parametes from the options parameter.
+         */
+        public getParam(paramName: string) {
+            return this._options[paramName];
+        }
+
+        /**
+         * Sets a specific parameter in the options given to the physics plugin
+         */
+        public setParam(paramName: string, value: number) {
+            this._options[paramName] = value;
+            this._bodyUpdateRequired = true;
+        }
+
+        /**
+         * Set the body's velocity.
+         */
+        public setVelocity(velocity: Vector3) {
+            this._physicsEngine.getPhysicsPlugin().setVelocity(this, velocity);
+        }
+        
+        /**
+         * Execute a function with the physics plugin native code.
+         * Provide a function the will have two variables - the world object and the physics body object.
+         */
+        public executeNativeFunction(func: (world: any, physicsBody:any) => void) {
+            func(this._physicsEngine.getPhysicsPlugin().world, this.physicsBody);
+        }
+
+        /**
+         * Register a function that will be executed before the physics world is stepping forward.
+         */
+        public registerBeforePhysicsStep(func: (impostor: PhysicsImpostor) => void): void {
+            this._onBeforePhysicsStepCallbacks.push(func);
+        }
+
+        public unregisterBeforePhysicsStep(func: (impostor: PhysicsImpostor) => void): void {
+            var index = this._onBeforePhysicsStepCallbacks.indexOf(func);
+
+            if (index > -1) {
+                this._onBeforePhysicsStepCallbacks.splice(index, 1);
+            } else {
+                Tools.Warn("Function to remove was not found");
+            }
+        }
+
+        /**
+         * Register a function that will be executed after the physics step
+         */
+        public registerAfterPhysicsStep(func: (impostor: PhysicsImpostor) => void): void {
+            this._onAfterPhysicsStepCallbacks.push(func);
+        }
+
+        public unregisterAfterPhysicsStep(func: (impostor: PhysicsImpostor) => void): void {
+            var index = this._onAfterPhysicsStepCallbacks.indexOf(func);
+
+            if (index > -1) {
+                this._onAfterPhysicsStepCallbacks.splice(index, 1);
+            } else {
+                Tools.Warn("Function to remove was not found");
+            }
+        }
+
+        /**
+         * register a function that will be executed when this impostor collides against a different body.
+         */
+        public registerOnPhysicsCollide(func: (collider: PhysicsImpostor, collidedAgainst: PhysicsImpostor) => void): void {
+            this._onPhysicsCollideCallbacks.push(func);
+        }
+
+        public unregisterOnPhysicsCollide(func: (collider: PhysicsImpostor, collidedAgainst: PhysicsImpostor) => void): void {
+            var index = this._onPhysicsCollideCallbacks.indexOf(func);
+
+            if (index > -1) {
+                this._onPhysicsCollideCallbacks.splice(index, 1);
+            } else {
+                Tools.Warn("Function to remove was not found");
+            }
+        }
+
+        private _tmpPositionWithDelta: Vector3 = Vector3.Zero();
+        private _tmpRotationWithDelta: Quaternion = new Quaternion();
+
+        /**
+         * this function is executed by the physics engine.
+         */
+        public beforeStep = () => {
+
+            this.mesh.position.subtractToRef(this._deltaPosition, this._tmpPositionWithDelta);
+            //conjugate deltaRotation
+            this._tmpRotationWithDelta.copyFrom(this._deltaRotation);
+            this._tmpRotationWithDelta.multiplyInPlace(this.mesh.rotationQuaternion);
+
+            this._physicsEngine.getPhysicsPlugin().setPhysicsBodyTransformation(this, this._tmpPositionWithDelta, this._tmpRotationWithDelta);
+
+            this._onBeforePhysicsStepCallbacks.forEach((func) => {
+                func(this);
+            });
+        }
+
+        /**
+         * this function is executed by the physics engine.
+         */
+        public afterStep = () => {
+            this._onAfterPhysicsStepCallbacks.forEach((func) => {
+                func(this);
+            });
+
+            this._physicsEngine.getPhysicsPlugin().setTransformationFromPhysicsBody(this);
+
+            this.mesh.position.addInPlace(this._deltaPosition)
+            this.mesh.rotationQuaternion.multiplyInPlace(this._deltaRotation);
+        }
+        
+        //event and body object due to cannon's event-based architecture.
+        public onCollide = (e: { body: any }) => {
+            var otherImpostor = this._physicsEngine.getImpostorWithPhysicsBody(e.body);
+            if (otherImpostor) {
+                this._onPhysicsCollideCallbacks.forEach((func) => {
+                    func(this, otherImpostor);
+                })
+            }
+        }
+
+        /**
+         * Apply a force 
+         */
+        public applyForce(force: Vector3, contactPoint: Vector3) {
+            this._physicsEngine.getPhysicsPlugin().applyForce(this, force, contactPoint);
+        }
+
+        /**
+         * Apply an impulse
+         */
+        public applyImpulse(force: Vector3, contactPoint: Vector3) {
+            this._physicsEngine.getPhysicsPlugin().applyImpulse(this, force, contactPoint);
+        }
+
+        /**
+         * A help function to create a joint.
+         */
+        public createJoint(otherImpostor: PhysicsImpostor, jointType: number, jointData: PhysicsJointData) {
+            var joint = new PhysicsJoint(jointType, jointData);
+            this.addJoint(otherImpostor, joint);
+        }
+
+        /**
+         * Add a joint to this impostor with a different impostor.
+         */
+        public addJoint(otherImpostor: PhysicsImpostor, joint: PhysicsJoint) {
+            this._joints.push({
+                otherImpostor: otherImpostor,
+                joint: joint
+            })
+            this._physicsEngine.addJoint(this, otherImpostor, joint);
+        }
+
+        public dispose(disposeChildren: boolean = true) {
+            this.physicsBody = null;
+            if (this.parent) {
+                this.parent.forceUpdate();
+            } else {
+                this.mesh.getChildMeshes().forEach(function(mesh) {
+                    if (mesh.physicsImpostor) {
+                        if (disposeChildren) {
+                            mesh.physicsImpostor.dispose();
+                            mesh.physicsImpostor = null;
+                        }
+                    }
+                })
+            }
+        }
+
+        public setDeltaPosition(position: Vector3) {
+            this._deltaPosition.copyFrom(position);
+        }
+
+        public setDeltaRotation(rotation: Quaternion) {
+            this._deltaRotation.copyFrom(rotation);
+        }
+        
+        //Impostor types
+        public static NoImpostor = 0;
+        public static SphereImpostor = 1;
+        public static BoxImpostor = 2;
+        public static PlaneImpostor = 3;
+        public static MeshImpostor = 4;
+        public static CapsuleImpostor = 5;
+        public static ConeImpostor = 6;
+        public static CylinderImpostor = 7;
+        public static ConvexHullImpostor = 8;
+        public static HeightmapImpostor = 9;
+    }
+
+}

src/Physics/babylon.physicsJoint.js

@@ -0,0 +1,40 @@
+var BABYLON;
+(function (BABYLON) {
+    var PhysicsJoint = (function () {
+        function PhysicsJoint(type, jointData) {
+            this.type = type;
+            this.jointData = jointData;
+            jointData.nativeParams = jointData.nativeParams || {};
+        }
+        Object.defineProperty(PhysicsJoint.prototype, "physicsJoint", {
+            get: function () {
+                return this._physicsJoint;
+            },
+            set: function (newJoint) {
+                if (this._physicsJoint) {
+                }
+                this._physicsJoint = newJoint;
+            },
+            enumerable: true,
+            configurable: true
+        });
+        //TODO check if the native joints are the same
+        //Joint Types
+        PhysicsJoint.DistanceJoint = 0;
+        PhysicsJoint.HingeJoint = 1;
+        PhysicsJoint.BallAndSocketJoint = 2;
+        PhysicsJoint.WheelJoint = 3;
+        PhysicsJoint.SliderJoint = 4;
+        //OIMO
+        PhysicsJoint.PrismaticJoint = 5;
+        //ENERGY FTW! (compare with this - http://ode-wiki.org/wiki/index.php?title=Manual:_Joint_Types_and_Functions)
+        PhysicsJoint.UniversalJoint = 6;
+        PhysicsJoint.Hinge2Joint = 7;
+        //Cannon
+        //Similar to a Ball-Joint. Different in params
+        //TODO check!!
+        PhysicsJoint.PointToPointJoint = 8;
+        return PhysicsJoint;
+    }());
+    BABYLON.PhysicsJoint = PhysicsJoint;
+})(BABYLON || (BABYLON = {}));

src/Physics/babylon.physicsJoint.ts

@@ -0,0 +1,61 @@
+module BABYLON {
+
+    export interface PhysicsJointData {
+        //Important for some engines, optional!
+        mainPivot?: Vector3;
+        connectedPivot?: Vector3;
+        mainAxis?: Vector3,
+        connectedAxis?: Vector3,
+        collision?: boolean //native in oimo, needs this - https://github.com/schteppe/cannon.js/blob/gh-pages/demos/collisionFilter.html in cannon
+        //Native Oimo/Cannon/Energy data
+        nativeParams?: any;
+    }
+
+    export class PhysicsJoint {
+        
+        private _physicsJoint;
+        
+        constructor(public type: number, public jointData: PhysicsJointData) {
+            jointData.nativeParams = jointData.nativeParams || {};
+        }
+        
+        public get physicsJoint() {
+            return this._physicsJoint;
+        }
+        
+        public set physicsJoint(newJoint: any) {
+            
+            if(this._physicsJoint) {
+                //remove from the wolrd
+            }
+            
+            this._physicsJoint = newJoint;
+        }
+        
+        
+        //TODO check if the native joints are the same
+        
+        //Joint Types
+        public static DistanceJoint = 0;
+        public static HingeJoint = 1;
+        public static BallAndSocketJoint = 2;
+        public static WheelJoint = 3;
+        public static SliderJoint = 4;
+        //OIMO
+        public static PrismaticJoint = 5;
+        //ENERGY FTW! (compare with this - http://ode-wiki.org/wiki/index.php?title=Manual:_Joint_Types_and_Functions)
+        public static UniversalJoint = 6;
+        public static Hinge2Joint = 7;
+        //Cannon
+        //Similar to a Ball-Joint. Different in params
+        //TODO check!!
+        public static PointToPointJoint = 8;
+
+    }
+    
+    export interface DistanceJointData extends PhysicsJointData {
+        maxDistance: number;
+        //Oimo - minDistance
+        //Cannon - maxForce
+    }
+}

src/Tools/babylon.sceneSerializer.js

@@ -96,11 +96,11 @@ var BABYLON;
             serializationObject.skeletonId = mesh.skeleton.id;
         }
         // Physics
-        if (mesh.getPhysicsImpostor() !== BABYLON.PhysicsEngine.NoImpostor) {
+        if (mesh.getPhysicsImpostor().type !== BABYLON.PhysicsEngine.NoImpostor) {
             serializationObject.physicsMass = mesh.getPhysicsMass();
             serializationObject.physicsFriction = mesh.getPhysicsFriction();
             serializationObject.physicsRestitution = mesh.getPhysicsRestitution();
-            switch (mesh.getPhysicsImpostor()) {
+            switch (mesh.getPhysicsImpostor().type) {
                 case BABYLON.PhysicsEngine.BoxImpostor:
                     serializationObject.physicsImpostor = 1;
                     break;
@@ -207,7 +207,7 @@ var BABYLON;
             //Physics
             if (scene.isPhysicsEnabled()) {
                 serializationObject.physicsEnabled = true;
-                serializationObject.physicsGravity = scene.getPhysicsEngine()._getGravity().asArray();
+                serializationObject.physicsGravity = scene.getPhysicsEngine().gravity.asArray();
                 serializationObject.physicsEngine = scene.getPhysicsEngine().getPhysicsPluginName();
             }
             // Lights

src/Tools/babylon.sceneSerializer.ts

@@ -112,12 +112,12 @@
         }
 
         // Physics
-        if (mesh.getPhysicsImpostor() !== PhysicsEngine.NoImpostor) {
+        if (mesh.getPhysicsImpostor().type !== PhysicsEngine.NoImpostor) {
             serializationObject.physicsMass = mesh.getPhysicsMass();
             serializationObject.physicsFriction = mesh.getPhysicsFriction();
             serializationObject.physicsRestitution = mesh.getPhysicsRestitution();
 
-            switch (mesh.getPhysicsImpostor()) {
+            switch (mesh.getPhysicsImpostor().type) {
                 case PhysicsEngine.BoxImpostor:
                     serializationObject.physicsImpostor = 1;
                     break;
@@ -235,7 +235,7 @@
             //Physics
             if (scene.isPhysicsEnabled()) {
                 serializationObject.physicsEnabled = true;
-                serializationObject.physicsGravity = scene.getPhysicsEngine()._getGravity().asArray();
+                serializationObject.physicsGravity = scene.getPhysicsEngine().gravity.asArray();
                 serializationObject.physicsEngine = scene.getPhysicsEngine().getPhysicsPluginName();
             }
 

src/babylon.scene.js

@@ -1488,7 +1488,7 @@ var BABYLON;
             // Physics
             if (this._physicsEngine) {
                 BABYLON.Tools.StartPerformanceCounter("Physics");
-                this._physicsEngine._runOneStep(deltaTime / 1000.0);
+                this._physicsEngine._step(deltaTime / 1000.0);
                 BABYLON.Tools.EndPerformanceCounter("Physics");
             }
             // Before render
@@ -1992,13 +1992,14 @@ var BABYLON;
             if (this._physicsEngine) {
                 return true;
             }
-            this._physicsEngine = new BABYLON.PhysicsEngine(plugin);
-            if (!this._physicsEngine.isSupported()) {
-                this._physicsEngine = null;
+            try {
+                this._physicsEngine = new BABYLON.PhysicsEngine(gravity, plugin);
+                return true;
+            }
+            catch (e) {
+                BABYLON.Tools.Error(e.message);
                 return false;
             }
-            this._physicsEngine._initialize(gravity);
-            return true;
         };
         Scene.prototype.disablePhysicsEngine = function () {
             if (!this._physicsEngine) {
@@ -2011,38 +2012,43 @@ var BABYLON;
             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) {
+            BABYLON.Tools.Warn("Deprecated, please use 'scene.getPhysicsEngine().setGravity()'");
             if (!this._physicsEngine) {
                 return;
             }
-            this._physicsEngine._setGravity(gravity);
+            this._physicsEngine.setGravity(gravity);
         };
+        /**
+         * Legacy support, using the new API
+         * @Deprecated
+         */
         Scene.prototype.createCompoundImpostor = function (parts, options) {
+            BABYLON.Tools.Warn("Scene.createCompoundImpostor is deprecated. Please use PhysicsImpostor parent/child");
             if (parts.parts) {
                 options = parts;
                 parts = parts.parts;
             }
-            if (!this._physicsEngine) {
-                return null;
-            }
-            for (var index = 0; index < parts.length; index++) {
+            var mainMesh = parts[0].mesh;
+            mainMesh.physicsImpostor = new BABYLON.PhysicsImpostor(mainMesh, parts[0].impostor, options);
+            for (var index = 1; index < parts.length; index++) {
                 var mesh = parts[index].mesh;
-                mesh._physicImpostor = parts[index].impostor;
-                mesh._physicsMass = options.mass / parts.length;
-                mesh._physicsFriction = options.friction;
-                mesh._physicRestitution = options.restitution;
+                if (mesh.parent !== mainMesh) {
+                    mesh.position = mesh.position.subtract(mainMesh.position);
+                    mesh.parent = mainMesh;
+                }
+                mesh.physicsImpostor = new BABYLON.PhysicsImpostor(mesh, parts[index].impostor, options);
             }
-            return this._physicsEngine._registerMeshesAsCompound(parts, options);
+            mainMesh.physicsImpostor.forceUpdate();
         };
         Scene.prototype.deleteCompoundImpostor = function (compound) {
-            for (var index = 0; index < compound.parts.length; index++) {
-                var mesh = compound.parts[index].mesh;
-                mesh._physicImpostor = BABYLON.PhysicsEngine.NoImpostor;
-                this._physicsEngine._unregisterMesh(mesh);
-            }
+            var mesh = compound.parts[0].mesh;
+            mesh.physicsImpostor.dispose(true);
+            mesh.physicsImpostor = null;
         };
         // Misc.
         Scene.prototype.createDefaultCameraOrLight = function () {
@@ -2098,6 +2104,6 @@ var BABYLON;
         Scene.MinDeltaTime = 1.0;
         Scene.MaxDeltaTime = 1000.0;
         return Scene;
-    })();
+    }());
     BABYLON.Scene = Scene;
 })(BABYLON || (BABYLON = {}));

src/babylon.scene.ts

@@ -556,7 +556,7 @@
 
                         if (pickResult.pickedMesh.actionManager.hasSpecificTrigger(ActionManager.OnLongPressTrigger)) {
                             var that = this;
-                            window.setTimeout(function () {
+                            window.setTimeout(function() {
                                 var pickResult = that.pick(that._unTranslatedPointerX, that._unTranslatedPointerY,
                                     (mesh: AbstractMesh): boolean => mesh.isPickable && mesh.isVisible && mesh.isReady() && mesh.actionManager && mesh.actionManager.hasSpecificTrigger(ActionManager.OnLongPressTrigger),
                                     false, that.cameraToUseForPointers);
@@ -1880,7 +1880,7 @@
             // Physics
             if (this._physicsEngine) {
                 Tools.StartPerformanceCounter("Physics");
-                this._physicsEngine._runOneStep(deltaTime / 1000.0);
+                this._physicsEngine._step(deltaTime / 1000.0);
                 Tools.EndPerformanceCounter("Physics");
             }
 
@@ -2491,16 +2491,14 @@
                 return true;
             }
 
-            this._physicsEngine = new PhysicsEngine(plugin);
-
-            if (!this._physicsEngine.isSupported()) {
-                this._physicsEngine = null;
+            try {
+                this._physicsEngine = new PhysicsEngine(gravity, plugin);
+                return true;
+            } catch (e) {
+                Tools.Error(e.message);
                 return false;
             }
 
-            this._physicsEngine._initialize(gravity);
-
-            return true;
         }
 
         public disablePhysicsEngine(): void {
@@ -2517,45 +2515,49 @@
         }
 
         /**
+         * 
          * 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 {
+            Tools.Warn("Deprecated, please use 'scene.getPhysicsEngine().setGravity()'")
             if (!this._physicsEngine) {
                 return;
             }
 
-            this._physicsEngine._setGravity(gravity);
+            this._physicsEngine.setGravity(gravity);
         }
 
-        public createCompoundImpostor(parts: any, options: PhysicsBodyCreationOptions): any {
+        /**
+         * Legacy support, using the new API
+         * @Deprecated
+         */
+        public createCompoundImpostor(parts: any, options: PhysicsImpostorParameters): any {
+            Tools.Warn("Scene.createCompoundImpostor is deprecated. Please use PhysicsImpostor parent/child")
+
             if (parts.parts) { // Old API
                 options = parts;
                 parts = parts.parts;
             }
 
-            if (!this._physicsEngine) {
-                return null;
-            }
-
-            for (var index = 0; index < parts.length; index++) {
-                var mesh = parts[index].mesh;
+            var mainMesh: AbstractMesh = parts[0].mesh;
+            mainMesh.physicsImpostor = new PhysicsImpostor(mainMesh, parts[0].impostor, options)
+            for (var index = 1; index < parts.length; index++) {
+                var mesh: AbstractMesh = parts[index].mesh;
+                if (mesh.parent !== mainMesh) {
+                    mesh.position = mesh.position.subtract(mainMesh.position);
+                    mesh.parent = mainMesh;
+                }
+                mesh.physicsImpostor = new PhysicsImpostor(mesh, parts[index].impostor, options)
 
-                mesh._physicImpostor = parts[index].impostor;
-                mesh._physicsMass = options.mass / parts.length;
-                mesh._physicsFriction = options.friction;
-                mesh._physicRestitution = options.restitution;
             }
-
-            return this._physicsEngine._registerMeshesAsCompound(parts, options);
+            mainMesh.physicsImpostor.forceUpdate();
         }
 
         public deleteCompoundImpostor(compound: any): void {
-            for (var index = 0; index < compound.parts.length; index++) {
-                var mesh = compound.parts[index].mesh;
-                mesh._physicImpostor = PhysicsEngine.NoImpostor;
-                this._physicsEngine._unregisterMesh(mesh);
-            }
+            var mesh: AbstractMesh = compound.parts[0].mesh;
+            mesh.physicsImpostor.dispose(true);
+            mesh.physicsImpostor = null;
         }
 
         // Misc.