From 114360337c2b9e2a79061674468a4796418d50b6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 29 Jun 2014 21:59:06 +0200
Subject: [PATCH] Fix issue in RigidBody::applyForce() method

---
 src/body/RigidBody.cpp | 39 ++++++++++++++++++++++++++++++++++-----
 src/body/RigidBody.h   |  8 +++++++-
 2 files changed, 41 insertions(+), 6 deletions(-)

diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index c12daa54..ddad4bef 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -95,16 +95,45 @@ void RigidBody::setType(BodyType type) {
     mExternalTorque.setToZero();
 }
 
-// Set the local inertia tensor of the body (in body coordinates)
+// Set the local inertia tensor of the body (in local-space coordinates)
 void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
+
+    if (mType != DYNAMIC) return;
+
     mInertiaTensorLocal = inertiaTensorLocal;
 
-    // Recompute the inverse local inertia tensor
-    if (mType == DYNAMIC) {
-        mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+    // Compute the inverse local inertia tensor
+    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+}
+
+// Set the local center of mass of the body (in local-space coordinates)
+void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
+
+    if (mType != DYNAMIC) return;
+
+    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    mCenterOfMassLocal = centerOfMassLocal;
+
+    // Compute the center of mass in world-space coordinates
+    mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
+
+    // Update the linear velocity of the center of mass
+    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+}
+
+// Set the mass of the rigid body
+void RigidBody::setMass(decimal mass) {
+
+    if (mType != DYNAMIC) return;
+
+    mInitMass = mass;
+
+    if (mInitMass > decimal(0.0)) {
+        mMassInverse = decimal(1.0) / mInitMass;
     }
     else {
-        mInertiaTensorLocalInverse = Matrix3x3::zero();
+        mInitMass = decimal(1.0);
+        mMassInverse = decimal(1.0);
     }
 }
 
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 16913a8a..38d60b5d 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -149,6 +149,12 @@ class RigidBody : public CollisionBody {
         /// Set the local inertia tensor of the body (in body coordinates)
         void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
 
+        /// Set the local center of mass of the body (in local-space coordinates)
+        void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
+
+        /// Set the mass of the rigid body
+        void setMass(decimal mass);
+
         /// Return the inertia tensor in world coordinates.
         Matrix3x3 getInertiaTensorWorld() const;
 
@@ -386,7 +392,7 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
 
     // Add the force and torque
     mExternalForce += force;
-    mExternalTorque += (point - mTransform.getPosition()).cross(force);
+    mExternalTorque += (point - mCenterOfMassWorld).cross(force);
 }
 
 // Apply an external torque to the body.