From dc5a82b4f5b268cf57f7fd1a29c838f6a11b9ccd Mon Sep 17 00:00:00 2001
From: Stefan Hedman <schteppe@gmail.com>
Date: Mon, 22 Dec 2014 14:46:28 +0100
Subject: [PATCH] added Body.prototype.applyLocalForce and .applyLocalImpulse,
 fixes https://github.com/schteppe/cannon.js/issues/140

---
 src/objects/Body.js | 46 ++++++++++++++++++++++++++++++
 test/Body.js        | 68 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 114 insertions(+)

diff --git a/src/objects/Body.js b/src/objects/Body.js
index 007732564..916023472 100644
--- a/src/objects/Body.js
+++ b/src/objects/Body.js
@@ -637,6 +637,29 @@ Body.prototype.applyForce = function(force,worldPoint){
     this.torque.vadd(rotForce,this.torque);
 };
 
+/**
+ * Apply force to a local point in the body.
+ * @method applyLocalForce
+ * @param  {Vec3} force The force vector to apply, defined locally in the body frame.
+ * @param  {Vec3} localPoint A local point in the body to apply the force on.
+ */
+var Body_applyLocalForce_worldForce = new Vec3();
+var Body_applyLocalForce_worldPoint = new Vec3();
+Body.prototype.applyLocalForce = function(localForce, localPoint){
+    if(this.type !== Body.DYNAMIC){
+        return;
+    }
+
+    var worldForce = Body_applyLocalForce_worldForce;
+    var worldPoint = Body_applyLocalForce_worldPoint;
+
+    // Transform the force vector to world space
+    this.vectorToWorldFrame(localForce, worldForce);
+    this.pointToWorldFrame(localPoint, worldPoint);
+
+    this.applyForce(worldForce, worldPoint);
+};
+
 /**
  * Apply impulse to a world point. This could for example be a point on the Body surface. An impulse is a force added to a body during a short period of time (impulse = force * time). Impulses will be added to Body.velocity and Body.angularVelocity.
  * @method applyImpulse
@@ -678,6 +701,29 @@ Body.prototype.applyImpulse = function(impulse, worldPoint){
     this.angularVelocity.vadd(rotVelo, this.angularVelocity);
 };
 
+/**
+ * Apply locally-defined impulse to a local point in the body.
+ * @method applyLocalImpulse
+ * @param  {Vec3} force The force vector to apply, defined locally in the body frame.
+ * @param  {Vec3} localPoint A local point in the body to apply the force on.
+ */
+var Body_applyLocalImpulse_worldImpulse = new Vec3();
+var Body_applyLocalImpulse_worldPoint = new Vec3();
+Body.prototype.applyLocalImpulse = function(localImpulse, localPoint){
+    if(this.type !== Body.DYNAMIC){
+        return;
+    }
+
+    var worldImpulse = Body_applyLocalImpulse_worldImpulse;
+    var worldPoint = Body_applyLocalImpulse_worldPoint;
+
+    // Transform the force vector to world space
+    this.vectorToWorldFrame(localImpulse, worldImpulse);
+    this.pointToWorldFrame(localPoint, worldPoint);
+
+    this.applyImpulse(worldImpulse, worldPoint);
+};
+
 /**
  * Should be called whenever you change the body mass.
  * @method updateMassProperties
diff --git a/test/Body.js b/test/Body.js
index 1c7bed691..0ba92e826 100644
--- a/test/Body.js
+++ b/test/Body.js
@@ -69,4 +69,72 @@ module.exports = {
 
         test.done();
     },
+
+    applyForce : function(test){
+        var sphereShape = new Sphere(1);
+        var body = new Body({
+            mass: 1,
+            shape: sphereShape
+        });
+
+        var worldPoint = new Vec3(1,0,0);
+        var forceVector = new Vec3(0,1,0);
+        body.applyForce(forceVector, worldPoint);
+        test.deepEqual(body.force, forceVector);
+        test.deepEqual(body.torque, new Vec3(0,0,1));
+
+        test.done();
+    },
+
+    applyLocalForce : function(test){
+        var sphereShape = new Sphere(1);
+        var body = new Body({
+            mass: 1,
+            shape: sphereShape
+        });
+        body.quaternion.setFromAxisAngle(new Vec3(1, 0, 0), Math.PI / 2);
+
+        var localPoint = new Vec3(1,0,0);
+        var localForceVector = new Vec3(0,1,0);
+        body.applyLocalForce(localForceVector, localPoint);
+        test.ok(body.force.almostEquals(new Vec3(0,0,1))); // The force is rotated to world space
+
+        test.done();
+    },
+
+    applyImpulse : function(test){
+        var sphereShape = new Sphere(1);
+        var body = new Body({
+            mass: 1,
+            shape: sphereShape
+        });
+
+        var f = 1000;
+        var dt = 1 / 60;
+        var worldPoint = new Vec3(0,0,0);
+        var impulse = new Vec3(f*dt,0,0);
+        body.applyImpulse(impulse, worldPoint);
+
+        test.ok(body.velocity.almostEquals(new Vec3(f*dt,0,0)));
+
+        test.done();
+    },
+
+    applyLocalImpulse : function(test){
+        var sphereShape = new Sphere(1);
+        var body = new Body({
+            mass: 1,
+            shape: sphereShape
+        });
+        body.quaternion.setFromAxisAngle(new Vec3(1, 0, 0), Math.PI / 2);
+
+        var f = 1000;
+        var dt = 1 / 60;
+        var localPoint = new Vec3(1,0,0);
+        var localImpulseVector = new Vec3(0,f*dt,0);
+        body.applyLocalImpulse(localImpulseVector, localPoint);
+        test.ok(body.velocity.almostEquals(new Vec3(0,0,f*dt))); // The force is rotated to world space
+
+        test.done();
+    },
 };