Remove BodyState and DerivativeBodyState

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@366 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/body/Body.cpp

@@ -24,11 +24,11 @@
 using namespace reactphysics3d;
 
 // Constructor
-Body::Body(Kilogram mass) throw(std::invalid_argument) : mass(mass)  {
+Body::Body(double mass) throw(std::invalid_argument) : mass(mass)  {
-    // Check if the mass is different from zero
+    // Check if the mass is not larger than zero
-    if (mass.getValue() == 0) {
+    if (mass <= 0.0) {
         // We throw an exception
-        throw std::invalid_argument("Exception in Body constructor : the mass has to be different from zero");
+        throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero");
     }
 }
 

sources/reactphysics3d/body/Body.h

@@ -22,7 +22,6 @@
 
  // Libraries
 #include <stdexcept>
-#include "../physics/physics.h"
 
 // Namespace reactphysics3d
 namespace reactphysics3d {
@@ -35,17 +34,17 @@ namespace reactphysics3d {
 */
 class Body {
     protected :
-        Kilogram mass;                      // Mass of the body
+        double mass;                      // Mass of the body
-        bool isMotionEnabled;               // True if the body is able to move
+        bool isMotionEnabled;             // True if the body is able to move
-        bool isCollisionEnabled;            // True if the body can collide with others bodies
+        bool isCollisionEnabled;          // True if the body can collide with others bodies
 
     public :
-        Body(Kilogram mass) throw(std::invalid_argument);       // Constructor
+        Body(double mass) throw(std::invalid_argument);         // Constructor
         Body(const Body& body);                                 // Copy-constructor
         virtual ~Body();                                        // Destructor
 
-        Kilogram getMass() const;                               // Return the mass of the body
+        double getMass() const;                                 // Return the mass of the body
-        void setMass(Kilogram mass);                            // Set the mass of the body
+        void setMass(double mass);                              // Set the mass of the body
         bool getIsMotionEnabled() const;                        // Return if the rigid body can move
         void setIsMotionEnabled(bool isMotionEnabled);          // Set the value to true if the body can move
         bool getIsCollisionEnabled() const;                     // Return true if the body can collide with others bodies
@@ -55,7 +54,7 @@ class Body {
 // --- Inlines function --- //
 
 // Method that return the mass of the body
-inline Kilogram Body::getMass() const {
+inline double Body::getMass() const {
     return mass;
 };
 
@@ -70,7 +69,7 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) {
 }
 
 // Method that set the mass of the body
-inline void Body::setMass(Kilogram mass) {
+inline void Body::setMass(double mass) {
     this->mass = mass;
 }
 

sources/reactphysics3d/body/RigidBody.cpp

@@ -24,9 +24,9 @@
  using namespace reactphysics3d;
 
  // Constructor
- RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb)
+ RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb)
-           : Body(mass), inertiaTensorLocal(inertiaTensorLocal), currentBodyState(position, orientation, inertiaTensorLocal.getInverse(), Kilogram(1.0/mass.getValue())),
+           : Body(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
-             previousBodyState(position, orientation, inertiaTensorLocal.getInverse(), Kilogram(1.0/mass.getValue())), obb(obb) {
+             massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation), obb(obb) {
 
     restitution = 1.0;
     isMotionEnabled = true;
@@ -40,36 +40,7 @@
     this->obb.setBodyPointer(this);
 }
 
- // Copy-constructor
- RigidBody::RigidBody(const RigidBody& rigidBody) : Body(rigidBody), inertiaTensorLocal(rigidBody.inertiaTensorLocal),
-            currentBodyState(rigidBody.currentBodyState), previousBodyState(rigidBody.previousBodyState), obb(rigidBody.obb) {
-    this->isMotionEnabled = rigidBody.isMotionEnabled;
-    this->isCollisionEnabled = rigidBody.isCollisionEnabled;
-    interpolationFactor = rigidBody.interpolationFactor;
- }
-
 // Destructor
 RigidBody::~RigidBody() {
 
 };
-
-// Compute the linear interpolation state between the previous body state and the current body state
-// This is used to avoid visual stuttering when the display and physics framerates are out of synchronization
-BodyState RigidBody::getInterpolatedState() const {
-
-    // Get the interpolation factor
-    double alpha = interpolationFactor;
-
-    // Compute the linear interpolation state
-    BodyState interpolatedState = currentBodyState;     // Used to take massInverse, inertiaTensorInverse
-    interpolatedState.setPosition(previousBodyState.getPosition() * (1-alpha) + currentBodyState.getPosition() * alpha);
-    interpolatedState.setLinearMomentum(previousBodyState.getLinearMomentum() * (1-alpha) + currentBodyState.getLinearMomentum() * alpha);
-    interpolatedState.setOrientation(Quaternion::slerp(previousBodyState.getOrientation(), currentBodyState.getOrientation(), alpha));
-    interpolatedState.setAngularMomentum(previousBodyState.getAngularMomentum() * (1-alpha) + currentBodyState.getAngularMomentum() * alpha);
-
-    // Recalculate the secondary state values
-    interpolatedState.recalculate();
-
-    // Return the interpolated state
-    return interpolatedState;
-}

sources/reactphysics3d/body/RigidBody.h

@@ -23,7 +23,6 @@
  // Libraries
  #include <cassert>
  #include "Body.h"
- #include "BodyState.h"
  #include "OBB.h"
  #include "../mathematics/mathematics.h"
  #include "../physics/physics.h"
@@ -31,9 +30,6 @@
 // Namespace reactphysics3d
 namespace reactphysics3d {
 
-// TODO : Check if it is possible to put all the attributes from BodyState class in the RigidBody class (to do this, the
-//        class RungeKutta have to be changed a lot
-
 /*  -------------------------------------------------------------------
     Class RigidBody :
         This class represents a rigid body of the physics
@@ -43,36 +39,129 @@ namespace reactphysics3d {
 */
 class RigidBody : public Body {
     protected :
+        Vector3D position;                          // Position of the center of mass of the body
+        Vector3D oldPosition;                       // Old position used to compute the interpolated position
+        Quaternion orientation;                     // Orientation quaternion of the body
+        Quaternion oldOrientation;                  // Old orientation used to compute the interpolated orientation
+        Vector3D linearVelocity;                    // Linear velocity of the body
+        Vector3D angularVelocity;                   // Angular velocity of the body
+        Vector3D externalForce;                     // Current external force on the body
+        Vector3D externalTorque;                    // Current external torque on the body
         Matrix3x3 inertiaTensorLocal;               // Local inertia tensor of the body (in body coordinates)
-        BodyState currentBodyState;                 // Current body state
+        Matrix3x3 inertiaTensorLocalInverse;        // Inverse of the inertia tensor of the body (in body coordinates)
-        BodyState previousBodyState;                // Previous body state
+        double massInverse;                       // Inverse of the mass of the body
+        
         double interpolationFactor;                 // Interpolation factor used for the state interpolation
         double restitution;                         // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
         OBB obb;                                    // Oriented bounding box that contains the rigid body
 
     public :
-        RigidBody(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb);  // Constructor
+        RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb);  // Constructor                                                                                                         // Copy-constructor
-        RigidBody(const RigidBody& rigidBody);                                                                                                          // Copy-constructor
+        virtual ~RigidBody();                                                                                                                  // Destructor
-        virtual ~RigidBody();                                                                                                                           // Destructor
 
+        Vector3D getPosition() const;                                           // Return the position of the body
+        void setPosition(const Vector3D& position);                             // Set the position of the body
+        Quaternion getOrientation() const;                                      // Return the orientation quaternion
+        void setOrientation(const Quaternion& orientation);                     // Set the orientation quaternion
+        Vector3D getLinearVelocity() const;                                     // Return the linear velocity
+        void setLinearVelocity(const Vector3D& linearVelocity);                 // TODO : Delete this
+        Vector3D getAngularVelocity() const;                                    // Return the angular velocity
+        void setAngularVelocity(const Vector3D& angularVelocity);               // Set the angular velocity
+        void setMassInverse(double massInverse);                                // Set the inverse of the mass
+        Vector3D getExternalForce() const;                                      // Return the current external force of the body
+        void setExternalForce(const Vector3D& force);                           // Set the current external force on the body
+        Vector3D getExternalTorque() const;                                     // Return the current external torque of the body
+        void setExternalTorque(const Vector3D& torque);                         // Set the current external torque of the body
+        double getMassInverse() const;                                          // Return the inverse of the mass of the body
         Matrix3x3 getInertiaTensorLocal() const;                                // Return the local inertia tensor of the body (in body coordinates)
         void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);        // Set the local inertia tensor of the body (in body coordinates)
+        Matrix3x3 getInertiaTensorLocalInverse() const;                         // Get the inverse of the inertia tensor
         Matrix3x3 getInertiaTensorWorld() const;                                // Return the inertia tensor in world coordinates
         Matrix3x3 getInertiaTensorInverseWorld() const;                         // Return the inverse of the inertia tensor in world coordinates
-        BodyState& getCurrentBodyState();                                       // Return a reference to the current state of the body
-        BodyState& getPreviousBodyState();                                      // TODO : DELETE THIS
         void setInterpolationFactor(double factor);                             // Set the interpolation factor of the body
-        BodyState getInterpolatedState() const;                                 // Compute and return the interpolated state
+        Vector3D getInterpolatedPosition() const;                               // Return the interpolated position
-        void setLinearVelocity(const Vector3D& linearVelocity);                 // Set the linear velocity of the rigid body
+        Quaternion getInterpolatedOrientation() const;                          // Return the interpolated orientation
         double getRestitution() const;                                          // Get the restitution coefficient
         void setRestitution(double restitution) throw(std::invalid_argument);   // Set the restitution coefficient
-        void updatePreviousBodyState();                                         // Update the previous body state of the body
+        void updateOldPositionAndOrientation();                                 // Update the previous position and orientation of the body
         const OBB* const getOBB() const;                                        // Return the oriented bounding box of the rigid body
         void update();                                                          // Update the rigid body in order to reflect a change in the body state
 };
 
 // --- Inline functions --- //
 
+// Return the position of the body
+inline Vector3D RigidBody::getPosition() const {
+    return position;
+}
+
+// Set the position of the body
+inline void RigidBody::setPosition(const Vector3D& position) {
+    this->position = position;
+}
+
+// Return the orientation quaternion of the body
+inline Quaternion RigidBody::getOrientation() const {
+    return orientation;
+}
+
+// Set the orientation quaternion
+inline void RigidBody::setOrientation(const Quaternion& orientation) {
+    this->orientation = orientation;
+
+    // Normalize the orientation quaternion
+    orientation.getUnit();
+}
+
+// Return the linear velocity
+inline Vector3D RigidBody::getLinearVelocity() const {
+    return linearVelocity;
+}
+
+// Return the angular velocity of the body
+inline Vector3D RigidBody::getAngularVelocity() const {
+    return angularVelocity;
+}
+
+inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) {
+     this->angularVelocity = angularVelocity;
+}
+
+// Set the inverse of the mass
+inline void RigidBody::setMassInverse(double massInverse) {
+    this->massInverse = massInverse;
+}
+
+// Get the inverse of the inertia tensor
+inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const {
+    return inertiaTensorLocalInverse;
+}
+
+// Return the external force on the body
+inline Vector3D RigidBody::getExternalForce() const {
+    return externalForce;
+}
+
+// Set the external force on the body
+inline void RigidBody::setExternalForce(const Vector3D& force) {
+    this->externalForce = force;
+}
+
+// Return the current external torque on the body
+inline Vector3D RigidBody::getExternalTorque() const {
+    return externalTorque;
+}
+
+ // Set the current external torque on the body
+inline void RigidBody::setExternalTorque(const Vector3D& torque) {
+    this->externalTorque = torque;
+}
+
+// Return the inverse of the mass of the body
+inline double RigidBody::getMassInverse() const {
+    return massInverse;
+}
+
 // Return the local inertia tensor of the body (in body coordinates)
 inline Matrix3x3 RigidBody::getInertiaTensorLocal() const {
     return inertiaTensorLocal;
@@ -89,7 +178,7 @@ inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal
 // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
 inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
     // Compute and return the inertia tensor in world coordinates
-    return currentBodyState.getOrientation().getMatrix() * inertiaTensorLocal * currentBodyState.getOrientation().getMatrix().getTranspose();
+    return orientation.getMatrix() * inertiaTensorLocal * orientation.getMatrix().getTranspose();
 }
 
 // Return the inverse of the inertia tensor in world coordinates
@@ -98,21 +187,9 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
 // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
 inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
     // Compute and return the inertia tensor in world coordinates
-    return currentBodyState.getOrientation().getMatrix() * currentBodyState.getInertiaTensorInverse() * currentBodyState.getOrientation().getMatrix().getTranspose();
+    return orientation.getMatrix() * inertiaTensorLocalInverse * orientation.getMatrix().getTranspose();
 }
 
-// Return a reference to the current state of the body
-// This way the currentBodyState could be modify outside the rigid body
-inline BodyState& RigidBody::getCurrentBodyState() {
-    return currentBodyState;
-}
-
-// TODO : DELETE THIS
-inline BodyState& RigidBody::getPreviousBodyState() {
-    return previousBodyState;
-}
-
-
 // Set the interpolation factor of the body
 inline void RigidBody::setInterpolationFactor(double factor) {
     //assert(factor >= 0.0 && factor <= 1.0); // TODO : Remove the comment here
@@ -121,15 +198,24 @@ inline void RigidBody::setInterpolationFactor(double factor) {
     interpolationFactor = factor;
 }
 
+// Return the interpolated position
+inline Vector3D RigidBody::getInterpolatedPosition() const {
+    // Compute the interpolated position
+    return oldPosition * (1-interpolationFactor) + position * interpolationFactor;
+}
+
+ // Return the interpolated orientation
+inline Quaternion RigidBody::getInterpolatedOrientation() const {
+    // Compute the interpolated orientation
+    return Quaternion::slerp(oldOrientation, orientation, interpolationFactor);
+}
+
 // Set the linear velocity of the rigid body
 inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) {
     // If the body is able to move
     if (isMotionEnabled) {
         // Update the linear velocity of the current body state
-        currentBodyState.setLinearVelocity(linearVelocity);
+        this->linearVelocity = linearVelocity;
-
-        // Update the linear momentum of the current body state
-        currentBodyState.setLinearMomentum(linearVelocity * (1.0/currentBodyState.getMassInverse().getValue()));
     }
 }
 
@@ -150,9 +236,10 @@ inline void RigidBody::setRestitution(double restitution) throw(std::invalid_arg
 }
 
 // Update the previous body state of the body
-inline void RigidBody::updatePreviousBodyState() {
+// This is used to compute the interpolated position and orientation of the body
-    // The current body state becomes the previous body state
+inline void RigidBody::updateOldPositionAndOrientation() {
-    previousBodyState = currentBodyState;
+    oldPosition = position;
+    oldOrientation = orientation;
 }
 
 // Return the oriented bounding box of the rigid body
@@ -163,7 +250,7 @@ inline const OBB* const RigidBody::getOBB() const {
 // Update the rigid body in order to reflect a change in the body state
 inline void RigidBody::update() {
     // Update the orientation of the corresponding bounding volume of the rigid body
-    obb.updateOrientation(currentBodyState.getPosition(), currentBodyState.getOrientation());
+    obb.updateOrientation(position, orientation);
 }
 
 } // End of the ReactPhyscis3D namespace

sources/reactphysics3d/engine/ConstraintSolver.cpp

@@ -183,8 +183,8 @@ void ConstraintSolver::fillInMatrices() {
         assert(rigidBody != 0);
         
         // Compute the vector V1 with initial velocities values
-        v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity());
+        v.fillInSubVector(0, rigidBody->getLinearVelocity());
-        v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
+        v.fillInSubVector(3, rigidBody->getAngularVelocity());
         V1[bodyNumber].changeSize(6);
         V1[bodyNumber] = v;
 
@@ -193,8 +193,8 @@ void ConstraintSolver::fillInMatrices() {
         Vconstraint[bodyNumber].initWithValue(0.0);
         
         // Compute the vector with forces and torques values
-        f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
+        f.fillInSubVector(0, rigidBody->getExternalForce());
-        f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque());
+        f.fillInSubVector(3, rigidBody->getExternalTorque());
         Fext[bodyNumber].changeSize(6);
         Fext[bodyNumber] = f;
 
@@ -202,7 +202,7 @@ void ConstraintSolver::fillInMatrices() {
         Matrix mInv(6,6);
         mInv.initWithValue(0.0);
         if (rigidBody->getIsMotionEnabled()) {
-            mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
+            mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * Matrix::identity(3));
             mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
         }
         Minv_sp[bodyNumber].changeSize(6, 6);

sources/reactphysics3d/engine/PhysicsEngine.cpp

@@ -104,12 +104,12 @@ void PhysicsEngine::updateAllBodiesMotion() {
 
             // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
             // external forces and torques.
-            newLinearVelocity = newLinearVelocity + dt * rigidBody->getCurrentBodyState().getMassInverse().getValue() * rigidBody->getCurrentBodyState().getExternalForce();
+            newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
-            newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getCurrentBodyState().getExternalTorque();
+            newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
             // Add the velocity V1 to the new velocity
-            newLinearVelocity = newLinearVelocity + rigidBody->getCurrentBodyState().getLinearVelocity();
+            newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity();
-            newAngularVelocity = newAngularVelocity + rigidBody->getCurrentBodyState().getAngularVelocity();
+            newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity();
 
             // Update the position and the orientation of the body according to the new velocity
             updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
@@ -133,21 +133,19 @@ void PhysicsEngine::updateAllBodiesMotion() {
     RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
     assert(rigidBody);
 
-    // The current body state of the body becomes the previous body state
+    // Update the old position and orientation of the body
-    rigidBody->updatePreviousBodyState();
+    rigidBody->updateOldPositionAndOrientation();
-
-    BodyState& bodyState = rigidBody->getCurrentBodyState();
 
     // Normalize the orientation quaternion
-    bodyState.setOrientation(bodyState.getOrientation().getUnit());
+    rigidBody->setOrientation(rigidBody->getOrientation().getUnit());
 
     // Update the linear and angular velocity of the body
-    bodyState.setLinearVelocity(newLinVelocity);
+    rigidBody->setLinearVelocity(newLinVelocity);
-    bodyState.setAngularVelocity(newAngVelocity);
+    rigidBody->setAngularVelocity(newAngVelocity);
 
     // Update the position and the orientation of the body
-    bodyState.setPosition(bodyState.getPosition() + newLinVelocity * dt);
+    rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt);
-    bodyState.setOrientation(bodyState.getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5 * dt);
+    rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt);
 }
 
 // Apply the gravity force to all bodies of the physics world
@@ -162,7 +160,7 @@ void PhysicsEngine::applyGravity() {
         // If the gravity force is on
         if(world->getIsGravityOn()) {
             // Apply the current gravity force to the body
-            rigidBody->getCurrentBodyState().setExternalForce(world->getGravity());
+            rigidBody->setExternalForce(rigidBody->getMass() * world->getGravity());
         }
     }
 }

sources/reactphysics3d/engine/PhysicsEngine.h

@@ -42,8 +42,7 @@ class PhysicsEngine {
         Timer timer;                                    // Timer of the physics engine
         CollisionDetection collisionDetection;          // Collision detection
         ConstraintSolver constraintSolver;              // Constraint solver
-
+                                                // Update the state of a rigid body // TODO : Delete this
-        void updateBodyState(RigidBody* const rigidBody, const Time& timeStep);                                                 // Update the state of a rigid body // TODO : Delete this
         void updateAllBodiesMotion();                                                                                           // Compute the motion of all bodies and update their positions and orientations
         void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity);    // Update the position and orientation of a body
         void applyGravity();                                                                                                    // Apply the gravity force to all bodies