diff --git a/sources/demo/Context.cpp b/sources/demo/Context.cpp
index 343914ee..0de07884 100755
--- a/sources/demo/Context.cpp
+++ b/sources/demo/Context.cpp
@@ -29,17 +29,19 @@ using namespace reactphysics3d;
// Constructor of the class Context
Context::Context() {
- Cube* cube1 = new Cube(Vector3D(5.0,14.0, 4.0), Quaternion(1.0, 1.0, 0.0, 0.0), 4.0, Kilogram(3.0));
- cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, -5.0, 0.0));
+ Cube* cube1 = new Cube(Vector3D(5.0, 13.0, 1), Quaternion(1.0, 1.0, 0.0, 0.0), 4.0, Kilogram(3.0));
+ Cube* cube2 = new Cube(Vector3D(5.0, 13.0, 9), Quaternion(0.5, 0.5, 0.5, 0.0), 4.0, Kilogram(3.0));
+ cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.5));
+ cube2->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, -0.5));
//Cube* cube2 = new Cube(Vector3D(0.0, 17, 8.0), Quaternion(0.0, 1.0, 0.0, 0.0), 3.0, Kilogram(2.0));
//Cube* cube3 = new Cube(Vector3D(4.0, 17, -2.0), Quaternion(0.0, 1.0, 0.0, 0.0), 2.0, Kilogram(11.0));
- Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.0, 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(10.0));
+ //Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.0, 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(10.0));
addObject(cube1);
- //addObject(cube2);
+ addObject(cube2);
//addObject(cube3);
- addObject(plane1);
+ //addObject(plane1);
}
diff --git a/sources/demo/Simulation.cpp b/sources/demo/Simulation.cpp
index 6e63fea5..6744bc61 100755
--- a/sources/demo/Simulation.cpp
+++ b/sources/demo/Simulation.cpp
@@ -27,7 +27,7 @@ using namespace reactphysics3d;
// Constructor of the class Simulation
Simulation::Simulation()
- :world(new PhysicsWorld(Vector3D(0.0, -0.6, 0.0))), engine(world, Time(0.01)), scene(this->world) {
+ :world(new PhysicsWorld(Vector3D(0.0, 0.0, 0.0))), engine(world, Time(0.01)), scene(this->world) {
simRunning = false;
mouseButtonPressed = false;
nbFrame = 0;
diff --git a/sources/reactphysics3d/body/BodyState.h b/sources/reactphysics3d/body/BodyState.h
index 51c9b113..2f6ba6fa 100644
--- a/sources/reactphysics3d/body/BodyState.h
+++ b/sources/reactphysics3d/body/BodyState.h
@@ -70,7 +70,9 @@ class BodyState {
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);
Quaternion getSpin() const; // Return the spin of the body
+ void setSpin(const Quaternion& spin);
void setMassInverse(Kilogram massInverse); // Set the inverse of the mass
Matrix3x3 getInertiaTensorInverse() const; // Get the inverse of the inertia tensor
void setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse); // Set the inverse of the inertia tensor
@@ -143,11 +145,19 @@ inline Vector3D BodyState::getAngularVelocity() const {
return angularVelocity;
}
+ inline void BodyState::setAngularVelocity(const Vector3D& angularVelocity) {
+ this->angularVelocity = angularVelocity;
+ }
+
// Return the spin of the body
inline Quaternion BodyState::getSpin() const {
return spin;
}
+inline void BodyState::setSpin(const Quaternion& spin) {
+ this->spin = spin;
+}
+
// Set the inverse of the mass
inline void BodyState::setMassInverse(Kilogram massInverse) {
this->massInverse = massInverse;
diff --git a/sources/reactphysics3d/collision/CollisionDetection.cpp b/sources/reactphysics3d/collision/CollisionDetection.cpp
index 45c8ae80..bbb30461 100644
--- a/sources/reactphysics3d/collision/CollisionDetection.cpp
+++ b/sources/reactphysics3d/collision/CollisionDetection.cpp
@@ -69,8 +69,8 @@ bool CollisionDetection::computeCollisionDetection() {
void CollisionDetection::computeBroadPhase() {
// For each pair of bodies in the physics world
- for(std::vector
::const_iterator it1 = world->getBodyListStartIterator(); it1 != world->getBodyListEndIterator(); ++it1) {
- for(std::vector::const_iterator it2 = it1; it2 != world->getBodyListEndIterator(); ++it2) {
+ for(std::vector::const_iterator it1 = world->getBodiesBeginIterator(); it1 != world->getBodiesEndIterator(); ++it1) {
+ for(std::vector::const_iterator it2 = it1; it2 != world->getBodiesEndIterator(); ++it2) {
// If both bodies are RigidBody and are different
RigidBody* rigidBody1 = dynamic_cast(*it1);
RigidBody* rigidBody2 = dynamic_cast(*it2);
diff --git a/sources/reactphysics3d/constraint/Contact.h b/sources/reactphysics3d/constraint/Contact.h
index c29f043f..1b4a1a96 100644
--- a/sources/reactphysics3d/constraint/Contact.h
+++ b/sources/reactphysics3d/constraint/Contact.h
@@ -62,6 +62,7 @@ class Contact : public Constraint {
virtual void evaluate(); // Evaluate the constraint
uint getNbAuxConstraints() const; // Return the number of auxiliary constraints
+ double getPenetrationDepth() const; // TODO : Delete this
void draw() const; // TODO : Delete this (Used to debug collision detection)
};
@@ -89,6 +90,9 @@ inline Vector3D Contact::getPoint() const {
return point;
}
+inline double Contact::getPenetrationDepth() const {
+ return penetrationDepth;
+}
} // End of the ReactPhysics3D namespace
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index f3996019..dfd309a2 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -21,18 +21,19 @@
#include "ConstraintSolver.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h"
+#include "../integration/SemiImplicitEuler.h" // TODO : Delete this
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
-
+ integrationAlgorithm = new SemiImplicitEuler();
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
-
+ delete integrationAlgorithm;
}
// Allocate all the matrices needed to solve the LCP problem
@@ -42,7 +43,7 @@ void ConstraintSolver::allocate() {
// For each constraint
std::vector::iterator it;
- for (it = physicsWorld->getConstraintsBeginIterator(); it getConstraintsEndIterator(); it++) {
+ for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
constraint = *it;
// Evaluate the constraint
@@ -288,5 +289,56 @@ void ConstraintSolver::solve(double dt) {
//std::cout << "Velocity Y after : " << V[bodyNumberMapping[constraintBodies.at(i)]].getValue(1) << std::endl;
}
+ for (std::vector::iterator it = physicsWorld->getBodiesBeginIterator(); it != physicsWorld->getBodiesEndIterator(); it++) {
+ // If this is a not constrained body
+ if (bodyNumberMapping.find(*it) == bodyNumberMapping.end()) {
+ RigidBody* rigidBody = dynamic_cast(*it);
+
+ // The current body state of the body becomes the previous body state
+ rigidBody->updatePreviousBodyState();
+
+ // Integrate the current body state at time t to get the next state at time t + dt
+ integrationAlgorithm->integrate(rigidBody->getCurrentBodyState(), dt, dt);
+
+ // If the body state has changed, we have to update some informations in the rigid body
+ rigidBody->update();
+ }
+ else {
+ RigidBody* rigidBody = dynamic_cast(*it);
+
+ // If the gravity force is on
+ /*
+ if(physicsWorld->getIsGravityOn()) {
+ // Apply the current gravity force to the body
+ rigidBody->getCurrentBodyState().setExternalForce(physicsWorld->getGravity());
+ }
+ */
+
+ // The current body state of the body becomes the previous body state
+ rigidBody->updatePreviousBodyState();
+
+ const Vector newLinVelocity = V[bodyNumberMapping[*it]].getSubVector(0, 3);
+ const Vector newAngVelocity = V[bodyNumberMapping[*it]].getSubVector(3, 3);
+ const Vector3D linVel(newLinVelocity.getValue(0), newLinVelocity.getValue(1), newLinVelocity.getValue(2));
+ const Vector3D angVel(newAngVelocity.getValue(0), newAngVelocity.getValue(1), newAngVelocity.getValue(2));
+ BodyState& bodyState = rigidBody->getCurrentBodyState();
+ rigidBody->getCurrentBodyState().setLinearVelocity(linVel);
+ rigidBody->getCurrentBodyState().setAngularVelocity(angVel);
+
+ // Normalize the orientation quaternion
+ rigidBody->getCurrentBodyState().setOrientation(rigidBody->getCurrentBodyState().getOrientation().getUnit());
+
+ // Compute the spin quaternion
+ const Vector3D angularVelocity = rigidBody->getCurrentBodyState().getAngularVelocity();
+ rigidBody->getCurrentBodyState().setSpin(Quaternion(angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5);
+
+ bodyState.setPosition(bodyState.getPosition() + bodyState.getLinearVelocity() * dt);
+ bodyState.setOrientation(bodyState.getOrientation() + bodyState.getSpin() * dt);
+
+ // If the body state has changed, we have to update some informations in the rigid body
+ rigidBody->update();
+ }
+ }
+
freeMemory();
}
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index 1ce3f1e0..cb1a08a6 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -24,6 +24,7 @@
#include "../typeDefinitions.h"
#include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPSolver.h"
+#include "../integration/IntegrationAlgorithm.h" // TODO : Delete this
#include "PhysicsWorld.h"
#include