diff --git a/sources/reactphysics3d/body/BodyState.cpp b/sources/reactphysics3d/body/BodyState.cpp
new file mode 100644
index 00000000..070af1da
--- /dev/null
+++ b/sources/reactphysics3d/body/BodyState.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ * Copyright (C) 2009 Daniel Chappuis *
+ ****************************************************************************
+ * This file is part of ReactPhysics3D. *
+ * *
+ * ReactPhysics3D is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * ReactPhysics3D is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with ReactPhysics3D. If not, see . *
+ ***************************************************************************/
+
+ // Libraries
+ #include "BodyState.h"
+ #include
+ #include
+
+ // We want to use the ReactPhysics3D namespace
+ using namespace reactphysics3d;
+
+ // Constructor
+ BodyState::BodyState(const Vector3D& position, const Matrix3x3& inertiaTensorInverse, const Kilogram& massInverse)
+ : position(position), linearMomentum(Vector3D()), orientation(Quaternion(1,0,0, 0)), angularMomentum(Vector3D()),
+ linearVelocity(Vector3D()), angularVelocity(Vector3D()), spin(Quaternion()), inertiaTensorInverse(inertiaTensorInverse),
+ massInverse(massInverse) {
+ // TODO : orientation will be initialized in another way
+ // TODO : linearMomentum will be initialized in another way
+ // TODO : angularMomentum will be initialize in another way
+ // TODO : linearVelocity will be initialize in another way
+ // TODO : angularVelocity will be initialize in another way
+ // TODO : spin will be initialize in another way
+ }
+
+// Copy-constructor
+BodyState::BodyState(const BodyState& bodyState)
+ : position(bodyState.position), linearMomentum(bodyState.linearMomentum), orientation(bodyState.orientation),
+ angularMomentum(bodyState.angularMomentum), linearVelocity(bodyState.linearVelocity),
+ angularVelocity(bodyState.angularVelocity), spin(bodyState.spin), inertiaTensorInverse(bodyState.inertiaTensorInverse),
+ massInverse(bodyState.massInverse) {
+}
+
+// Destructor
+BodyState::~BodyState() {
+
+}
+
+// Recalculate the secondary values of the BodyState when the primary values have changed
+void BodyState::recalculate() {
+ // Compute the linear velocity
+ linearVelocity = linearMomentum * massInverse.getValue();
+
+ // Compute the angular velocity
+ angularVelocity = inertiaTensorInverse * angularMomentum;
+
+ // Normalize the orientation quaternion
+ orientation = orientation.getUnit();
+
+ // Compute the spin quaternion
+ spin = Quaternion(0, angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ()) * orientation * 0.5;
+}
+
+// Compute the body state at time t + dt
+void BodyState::computeAtTime(const Time& timeStep, const DerivativeBodyState& lastDerivativeBodyState) {
+
+ double dt = timeStep.getValue(); // Timestep
+
+ // Compute the position at time t + dt
+ position = position + lastDerivativeBodyState.getLinearVelocity() * dt;
+
+ // Compute the linear momentum at time t + dt
+ linearMomentum = linearMomentum + lastDerivativeBodyState.getForce() * dt;
+
+ // Compute the orientation at time t + dt
+ orientation = orientation + lastDerivativeBodyState.getSpin() * dt;
+
+ // Compute the angular momentum at time t + dt
+ angularMomentum = angularMomentum + lastDerivativeBodyState.getTorque() * dt;
+
+ // Recalculate the secondary values of the body state
+ recalculate();
+}
+
+// Return the force on the body at time t
+Vector3D BodyState::computeForce(Time time) const {
+ // TODO : Implement this method
+ return Vector3D(sin(time.getValue()*0.9 + 0.5), sin(time.getValue()*0.5 + 0.4),
+ sin(time.getValue()*0.7f + 0.9f));
+}
+
+// Return the torque on the body at time
+Vector3D BodyState::computeTorque(Time time) const {
+ // TODO : Implement this method
+ return Vector3D(-1.0, 0.0 ,0);
+}
diff --git a/sources/reactphysics3d/body/BodyState.h b/sources/reactphysics3d/body/BodyState.h
new file mode 100644
index 00000000..8ce52f84
--- /dev/null
+++ b/sources/reactphysics3d/body/BodyState.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * Copyright (C) 2009 Daniel Chappuis *
+ ****************************************************************************
+ * This file is part of ReactPhysics3D. *
+ * *
+ * ReactPhysics3D is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * ReactPhysics3D is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with ReactPhysics3D. If not, see . *
+ ***************************************************************************/
+
+#ifndef BODYSTATE_H
+#define BODYSTATE_H
+
+// Libraries
+#include "../mathematics/mathematics.h"
+#include "../physics/physics.h"
+#include "DerivativeBodyState.h"
+
+// Namespace reactphysics3d
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class BodyState :
+ A BodyState contains all the dynamics values of a body.
+ -------------------------------------------------------------------
+*/
+class BodyState {
+ private :
+ // Primary values
+ Vector3D position; // Position of the body
+ Vector3D linearMomentum; // Linear momentum of the body
+ Quaternion orientation; // Orientation quaternion of the body
+ Vector3D angularMomentum; // Angular momentum of the body
+
+ // Secondary values
+ Vector3D linearVelocity; // Linear velocity of the body
+ Vector3D angularVelocity; // Angular velocity of the body
+ Quaternion spin; // Spin is the derivative of orientation quaternion over time.
+
+ // Constants
+ Matrix3x3 inertiaTensorInverse; // Inverse of the inertia tensor of the body
+ Kilogram massInverse; // Inverse of the mass of the body
+
+ public :
+ BodyState(const Vector3D& position, const Matrix3x3& inertiaTensorInverse, const Kilogram& massInverse); // Constructor
+ BodyState(const BodyState& bodyState); // Copy-constructor
+ virtual ~BodyState(); // Destructor
+
+ Vector3D getPosition() const; // Return the position of the body
+ void setPosition(const Vector3D& position); // Set the position of the body
+ Vector3D getLinearMomentum() const; // Return the linear momemtum
+ void setLinearMomentum(const Vector3D& linearMomentum); // Set the linear momentum
+ Quaternion getOrientation() const; // Return the orientation quaternion
+ void setOrientation(const Quaternion& orientation); // Set the orientation quaternion
+ Vector3D getAngularMomentum() const; // Return the angular momentum
+ void setAngularMomentum(const Vector3D& angularMomentum); // Set the angular momentum
+ Vector3D getLinearVelocity() const; // Return the linear velocity
+ Vector3D getAngularVelocity() const; // Return the angular velocity
+ Quaternion getSpin() const; // Return the spin of the body
+ void setMassInverse(Kilogram massInverse); // Set the inverse of the mass
+ void setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse); // Set the inverse of the inertia tensor
+
+ void recalculate(); // Recalculate the secondary values
+ // of the BodyState from the primary ones
+ void computeAtTime(const Time& timeStep,
+ const DerivativeBodyState& lastDerivativeBodyState); // Compute the body state at time t + dt
+ Vector3D computeForce(Time time) const; // Return the force on the body at time t
+ Vector3D computeTorque(Time time) const; // Return the torque on the body at time t
+};
+
+// --- Inlines functions --- //
+
+// Return the position of the body
+inline Vector3D BodyState::getPosition() const {
+ return position;
+}
+
+// Set the position of the body
+inline void BodyState::setPosition(const Vector3D& position) {
+ this->position = position;
+}
+
+// Return the linear momentum of the body
+inline Vector3D BodyState::getLinearMomentum() const {
+ return linearMomentum;
+}
+
+// Set the linear momentum of the body
+inline void BodyState::setLinearMomentum(const Vector3D& linearMomentum) {
+ this->linearMomentum = linearMomentum;
+}
+
+// Return the orientation quaternion of the body
+inline Quaternion BodyState::getOrientation() const {
+ return orientation;
+}
+
+// Set the orientation quaternion
+inline void BodyState::setOrientation(const Quaternion& orientation) {
+ this->orientation = orientation;
+}
+
+// Return the angular momentum of the body
+inline Vector3D BodyState::getAngularMomentum() const {
+ return angularMomentum;
+}
+
+// Set the angular momentum of the body
+inline void BodyState::setAngularMomentum(const Vector3D& angularMomentum) {
+ this->angularMomentum = angularMomentum;
+}
+
+// Return the linear velocity
+inline Vector3D BodyState::getLinearVelocity() const {
+ return linearVelocity;
+}
+
+// Return the angular velocity of the body
+inline Vector3D BodyState::getAngularVelocity() const {
+ return angularVelocity;
+}
+
+// Return the spin of the body
+inline Quaternion BodyState::getSpin() const {
+ return spin;
+}
+
+// Set the inverse of the mass
+inline void BodyState::setMassInverse(Kilogram massInverse) {
+ this->massInverse = massInverse;
+}
+
+// Set the inverse of the inertia tensor
+inline void BodyState::setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse) {
+ this->inertiaTensorInverse = inertiaTensorInverse;
+}
+
+}
+
+#endif
diff --git a/sources/reactphysics3d/body/RigidBody.cpp b/sources/reactphysics3d/body/RigidBody.cpp
new file mode 100644
index 00000000..4e4d0b78
--- /dev/null
+++ b/sources/reactphysics3d/body/RigidBody.cpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+* Copyright (C) 2009 Daniel Chappuis *
+****************************************************************************
+* This file is part of ReactPhysics3D. *
+* *
+* ReactPhysics3D is free software: you can redistribute it and/or modify *
+* it under the terms of the GNU General Public License as published by *
+* the Free Software Foundation, either version 3 of the License, or *
+* (at your option) any later version. *
+* *
+* ReactPhysics3D is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+* GNU General Public License for more details. *
+* *
+* You should have received a copy of the GNU General Public License *
+* along with ReactPhysics3D. If not, see . *
+***************************************************************************/
+
+// Libraries
+#include "RigidBody.h"
+
+ // We want to use the ReactPhysics3D namespace
+ using namespace reactphysics3d;
+
+ // Constructor
+ RigidBody::RigidBody(const Vector3D& position, const Kilogram& mass, const Matrix3x3& inertiaTensor)
+ : Body(mass), inertiaTensor(inertiaTensor),
+ currentBodyState(position, inertiaTensor.getInverse(),Kilogram(1.0/mass.getValue())),
+ previousBodyState(position, inertiaTensor.getInverse(), Kilogram(1.0/mass.getValue())) {
+ isMotionEnabled = true;
+ isCollisionEnabled = true;
+ interpolationFactor = 0.0;
+ }
+
+ // Copy-constructor
+ RigidBody::RigidBody(const RigidBody& rigidBody) : Body(rigidBody), inertiaTensor(rigidBody.inertiaTensor),
+ currentBodyState(rigidBody.currentBodyState), previousBodyState(rigidBody.previousBodyState) {
+ 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;
+}
diff --git a/sources/reactphysics3d/body/RigidBody.h b/sources/reactphysics3d/body/RigidBody.h
new file mode 100644
index 00000000..ac1dfedb
--- /dev/null
+++ b/sources/reactphysics3d/body/RigidBody.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+ * Copyright (C) 2009 Daniel Chappuis *
+ ****************************************************************************
+ * This file is part of ReactPhysics3D. *
+ * *
+ * ReactPhysics3D is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * ReactPhysics3D is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with ReactPhysics3D. If not, see . *
+ ***************************************************************************/
+
+ #ifndef RIGIDBODY_H
+ #define RIGIDBODY_H
+
+ // Libraries
+ #include
+ #include "Body.h"
+ #include "BodyState.h"
+ #include "../mathematics/mathematics.h"
+ #include "../physics/physics.h"
+
+// Namespace reactphysics3d
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class RigidBody :
+ This class represents a rigid body of the physics
+ engine. A rigid body is a non-deformable body that
+ has a constant mass.
+ -------------------------------------------------------------------
+*/
+class RigidBody : public Body {
+ private :
+ Matrix3x3 inertiaTensor; // Inertia tensor of the body
+ BodyState currentBodyState; // Current body state
+ BodyState previousBodyState; // Previous body state
+ bool isMotionEnabled; // True if the body can move
+ bool isCollisionEnabled; // True if the body can collide with others bodies
+ double interpolationFactor; // Interpolation factor used for the state interpolation
+
+ public :
+ RigidBody(const Vector3D& position, const Kilogram& mass, const Matrix3x3& inertiaTensor); // Constructor
+ RigidBody(const RigidBody& rigidBody); // Copy-constructor
+ virtual ~RigidBody(); // Destructor
+
+ Matrix3x3 getInertiaTensor() const; // Return the inertia tensor of the body
+ void setInertiaTensor(const Matrix3x3& inertiaTensor); // Set the inertia tensor of the body
+ BodyState& getCurrentBodyState(); // Return a reference to the current state of the body
+ void setInterpolationFactor(double factor); // Set the interpolation factor of the body
+ BodyState getInterpolatedState() const; // Compute and return the interpolated state
+ bool getIsMotionEnabled() const; // Return if the rigid body can move
+ void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move
+ void updatePreviousBodyState(); // Update the previous body state of the body
+};
+
+// --- Inline functions --- //
+
+// Return the inertia tensor of the body
+inline Matrix3x3 RigidBody::getInertiaTensor() const {
+ return inertiaTensor;
+}
+
+// Set the inertia tensor of the body
+inline void RigidBody::setInertiaTensor(const Matrix3x3& inertiaTensor) {
+ this->inertiaTensor = inertiaTensor;
+}
+
+// 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;
+}
+
+// Set the interpolation factor of the body
+inline void RigidBody::setInterpolationFactor(double factor) {
+ assert(factor >= 0 && factor <= 1);
+
+ // Set the factor
+ interpolationFactor = factor;
+}
+
+// Return if the rigid body can move
+inline bool RigidBody::getIsMotionEnabled() const {
+ return isMotionEnabled;
+}
+
+// Set the value to true if the body can move
+inline void RigidBody::setIsMotionEnabled(bool isMotionEnabled) {
+ this->isMotionEnabled = isMotionEnabled;
+}
+
+// Update the previous body state of the body
+inline void RigidBody::updatePreviousBodyState() {
+ // The current body state becomes the previous body state
+ previousBodyState = currentBodyState;
+}
+
+} // End of the ReactPhyscis3D namespace
+
+ #endif