diff --git a/sources/reactphysics3d/body/BodyState.cpp b/sources/reactphysics3d/body/BodyState.cpp
index 37cf6de8..cc5713da 100644
--- a/sources/reactphysics3d/body/BodyState.cpp
+++ b/sources/reactphysics3d/body/BodyState.cpp
@@ -64,6 +64,29 @@ void BodyState::recalculate() {
spin = Quaternion(0, angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ()) * orientation * 0.5;
}
+// Compute the body state at time t + dt
+void computeAtTime(const Time& time, const Time& timeStep, const DerivativeBodyState& lastDerivativeBodyState) {
+
+ double t = time.getValue(); // Current time
+ 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.force * dt;
+
+ // Compute the orientation at time t + dt
+ orientation = orientation + lastDerivativeBodyState.getSpin() * dt;
+
+ // Compute the angular momentum at time t + dt
+ angularMomentum = angularMomentum + lastDerivativeBodyState.torque * dt;
+
+ // Recalculate the secondary values of the body state
+ recalculate();
+}
+
+
// Overloaded operator for the multiplication with a number
BodyState BodyState::operator*(double number) const {
// TODO : Implement this method
diff --git a/sources/reactphysics3d/body/BodyState.h b/sources/reactphysics3d/body/BodyState.h
index f45fba69..890fcc99 100644
--- a/sources/reactphysics3d/body/BodyState.h
+++ b/sources/reactphysics3d/body/BodyState.h
@@ -67,7 +67,10 @@ class BodyState {
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
+ void recalculate(); // Recalculate the secondary values
+ // of the BodyState from the primary ones
+ void computeAtTime(const Time& time, const Time& timeStep,
+ const DerivativeBodyState& lastDerivativeBodyState); // Compute the body state at time t + dt
// Overloaded operators
BodyState operator*(double number) const; // Overloaded operator for the multiplication with a number
diff --git a/sources/reactphysics3d/body/DerivativeBodyState.cpp b/sources/reactphysics3d/body/DerivativeBodyState.cpp
new file mode 100644
index 00000000..2a61acaa
--- /dev/null
+++ b/sources/reactphysics3d/body/DerivativeBodyState.cpp
@@ -0,0 +1,43 @@
+/****************************************************************************
+* 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 "DerivativeBodyState.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+DerivativeBodyState::DerivativeBodyState(const Vector3D& linearVelocity, const Vector3D& force, const Vector3D& torque,
+ const Quaternion& spin) : linearVelocity(linearVelocity), force(force), torque(torque),
+ spin(spin) {
+
+}
+
+// Copy-constructor
+DerivativeBodyState::DerivativeBodyState(const DerivativeBodyState& derivativeBodyState)
+ : linearVelocity(derivativeBodyState.linearVelocity), force(derivativeBodyState.force),
+ torque(derivativeBodyState.torque), spin(derivativeBodyState.spin) {
+
+}
+
+// Destructor
+DerivativeBodyState::~DerivativeBodyState() {
+
+}
diff --git a/sources/reactphysics3d/body/DerivativeBodyState.h b/sources/reactphysics3d/body/DerivativeBodyState.h
new file mode 100644
index 00000000..f624c697
--- /dev/null
+++ b/sources/reactphysics3d/body/DerivativeBodyState.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+* 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 DERIVATIVEBODYSTATE_H
+#define DERIVATIVEBODYSTATE_H
+
+// Libraries
+#include "../mathematics/mathematics.h"
+
+// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class DerivativeBodyState :
+ This class represents a derivative of a body state at time t.
+ This class is used in the numerical integrator to compute the
+ derivative of a body state at different times.
+ -------------------------------------------------------------------
+*/
+class DerivativeBodyState {
+ private :
+ Vector3D linearVelocity; // Linear velocity of the body
+ Vector3D force; // Force applied to the body
+ Vector3D torque; // Torque applied to the body
+ Quaternion spin; // Quaternion spin of the body
+
+ public :
+ DerivativeBodyState(const Vector3D& linearVelocity, const Vector3D& force, const Vector3D& torque,
+ const Quaternion& spin); // Constructor
+ DerivativeBodyState(const DerivativeBodyState& derivativeBodyState); // Copy-constructor
+ virtual ~DerivativeBodyState(); // Destructor
+
+ Vector3D getLinearVelocity() const; // Return the linear velocity
+ Vector3D getForce() const; // Return the force
+ Vector3D getTorque() const; // Return the torque
+ Quaternion getSpin() const; // Return the spin
+};
+
+// --- Inline functions --- //
+
+// Return the linear velocity
+inline Vector3D DerivativeBodyState::getLinearVelocity() const {
+ return linearVelocity;
+}
+
+// Return the force
+inline Vector3D DerivativeBodyState::getForce() const {
+ return force;
+}
+
+// Return the torque
+inline Vector3D DerivativeBodyState::getTorque() const {
+ return torque;
+}
+
+// Return the spin
+inline Quaternion DerivativeBodyState::getSpin() const {
+ return spin;
+}
+
+}
+
+#endif