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 @@ -41,6 +42,7 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol */ class ConstraintSolver { protected: + IntegrationAlgorithm* integrationAlgorithm; // TODO : Delete this PhysicsWorld* physicsWorld; // Reference to the physics world LCPSolver* lcpSolver; // LCP Solver std::vector activeConstraints; // Current active constraints in the physics world diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index e30ebe18..b20ed074 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp @@ -54,7 +54,7 @@ void PhysicsEngine::updateDynamic() { // While the time accumulator is not empty while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { // For each body in the dynamic world - for(std::vector::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) { + for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { // If the body is a RigidBody and if the rigid body motion is enabled RigidBody* rigidBody = dynamic_cast(*it); if (rigidBody && rigidBody->getIsMotionEnabled()) { @@ -68,7 +68,7 @@ void PhysicsEngine::updateDynamic() { } // For each body in the dynamic world - for(std::vector::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) { + for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { // If the body is a RigidBody and if the rigid body motion is enabled RigidBody* rigidBody = dynamic_cast(*it); if (rigidBody && rigidBody->getIsMotionEnabled()) { @@ -89,6 +89,10 @@ void PhysicsEngine::updateCollision() { // Compute the collision detection if (collisionDetection.computeCollisionDetection()) { // TODO : Delete this ---------------------------------------------------------- + for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) { + Contact* contact = dynamic_cast(*it); + std::cout << "Const : " << contact << "pDepth before: " << contact->getPenetrationDepth() << std::endl; + } /* for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { RigidBody* rigidBody1 = dynamic_cast((*it)->getBody1()); @@ -102,26 +106,37 @@ void PhysicsEngine::updateCollision() { // Solve constraints constraintSolver.solve(timer.getTimeStep().getValue()); } - - // For each body in the dynamic world - for(std::vector::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) { - // If the body is a RigidBody and if the rigid body motion is enabled - RigidBody* rigidBody = dynamic_cast(*it); - if (rigidBody && rigidBody->getIsMotionEnabled()) { - // Update the state of the rigid body with an entire time step - updateBodyState(rigidBody, timer.getTimeStep()); + else { + for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { + // If the body is a RigidBody and if the rigid body motion is enabled + RigidBody* rigidBody = dynamic_cast(*it); + if (rigidBody && rigidBody->getIsMotionEnabled()) { + // Update the state of the rigid body with an entire time step + updateBodyState(rigidBody, timer.getTimeStep()); + } } } + // TODO : Delete this + collisionDetection.computeCollisionDetection(); + for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) { + Contact* contact = dynamic_cast(*it); + std::cout << "Const : " << contact << "pDepth after: " << contact->getPenetrationDepth() << std::endl; + RigidBody* rigidBody1 = dynamic_cast(contact->getBody1()); + RigidBody* rigidBody2 = dynamic_cast(contact->getBody2()); + rigidBody1->getCurrentBodyState().setPosition(rigidBody1->getCurrentBodyState().getPosition() - contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4); + rigidBody2->getCurrentBodyState().setPosition(rigidBody2->getCurrentBodyState().getPosition() + contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4); + } + // Update the timer timer.update(); } // For each body in the the dynamic world - for(std::vector::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) { + for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { // If the body is a RigidBody and if the rigid body motion is enabled RigidBody* rigidBody = dynamic_cast(*it); - if (rigidBody && rigidBody->getIsMotionEnabled()) { + if (rigidBody) { // Update the interpolation factor of the rigid body // This one will be used to compute the interpolated state rigidBody->setInterpolationFactor(timer.getInterpolationFactor()); diff --git a/sources/reactphysics3d/engine/PhysicsWorld.h b/sources/reactphysics3d/engine/PhysicsWorld.h index 3b16fb1a..bc1537e9 100644 --- a/sources/reactphysics3d/engine/PhysicsWorld.h +++ b/sources/reactphysics3d/engine/PhysicsWorld.h @@ -54,8 +54,6 @@ class PhysicsWorld { Vector3D getGravity() const; // Return the gravity vector of the world bool getIsGravityOn() const; // Return if the gravity is on void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute - std::vector::const_iterator getBodyListStartIterator() const; // Return a start iterator on the body list - std::vector::const_iterator getBodyListEndIterator() const; // Return a end iterator on the body list void addConstraint(Constraint* constraint) throw(std::invalid_argument); // Add a constraint void removeConstraint(Constraint* constraint) throw(std::invalid_argument); // Remove a constraint void removeAllContactConstraints(); // Remove all collision contacts constraints @@ -72,18 +70,6 @@ inline Vector3D PhysicsWorld::getGravity() const { return gravity; } -// Return a start iterator on the body list -inline std::vector::const_iterator PhysicsWorld::getBodyListStartIterator() const { - // Return an iterator on the start of the body list - return bodies.begin(); -} - -// Return a end iterator on the body list -inline std::vector::const_iterator PhysicsWorld::getBodyListEndIterator() const { - // Return an iterator on the end of the body list - return bodies.end(); -} - // Return if the gravity is on inline bool PhysicsWorld::getIsGravityOn() const { return isGravityOn; diff --git a/sources/reactphysics3d/mathematics/Vector.h b/sources/reactphysics3d/mathematics/Vector.h index 8e5fbcc0..0f0c1b78 100644 --- a/sources/reactphysics3d/mathematics/Vector.h +++ b/sources/reactphysics3d/mathematics/Vector.h @@ -139,7 +139,7 @@ inline void Vector::fillInSubVector(uint rowIndex, const Vector& subVector) { // Return a sub-vector of the current vector inline Vector Vector::getSubVector(uint index, uint nbElements) const throw(std::invalid_argument) { // Check if the arguments are valid - if (index < 0 || index+nbElements >= nbComponent) { + if (index < 0 || index+nbElements > nbComponent) { throw std::invalid_argument("Error : arguments are out of bounds"); }