From 4e19ab899d5ec9f52a2b9cfba65f32fb7215770a Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Tue, 15 Jun 2010 21:16:31 +0000
Subject: [PATCH] constraintBodies is now a std::set

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@333 92aac97c-a6ce-11dd-a772-7fcde58d38e6
---
 sources/reactphysics3d/engine/ConstraintSolver.cpp | 13 +++++++------
 sources/reactphysics3d/engine/ConstraintSolver.h   |  3 ++-
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index dfd309a2..f47eedac 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -54,8 +54,8 @@ void ConstraintSolver::allocate() {
             activeConstraints.push_back(constraint);
 
             // Add the two bodies of the constraint in the constraintBodies list
-            constraintBodies.push_back(constraint->getBody1());
-            constraintBodies.push_back(constraint->getBody2());
+            constraintBodies.insert(constraint->getBody1());
+            constraintBodies.insert(constraint->getBody2());
 
             // Fill in the body number maping
             bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
@@ -148,8 +148,9 @@ void ConstraintSolver::fillInMatrices() {
     Body* body;
     Vector v(6);
     Vector f(6);
-    for (uint b=0; b<nbBodies; b++) {
-        body = constraintBodies.at(b);
+    uint b=0;
+    for (std::set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
+        body = *it;
         uint bodyNumber = bodyNumberMapping.at(body);
         
         // TODO : Use polymorphism and remove this downcasting
@@ -283,8 +284,8 @@ void ConstraintSolver::solve(double dt) {
 
     // Update the velocity of each body
     // TODO : Put this code somewhere else
-    for (int i=0; i<nbBodies; i++) {
-        RigidBody* body = dynamic_cast<RigidBody*>(constraintBodies.at(i));
+    for (std::set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++) {
+        RigidBody* body = dynamic_cast<RigidBody*>(*it);
         //std::cout << "Velocity Y before : " << body->getCurrentBodyState().getLinearVelocity().getY() << std::endl;
         //std::cout << "Velocity Y after  : " << V[bodyNumberMapping[constraintBodies.at(i)]].getValue(1) << std::endl;
     }
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index cb1a08a6..6b9133b5 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -27,6 +27,7 @@
 #include "../integration/IntegrationAlgorithm.h"    // TODO : Delete this
 #include "PhysicsWorld.h"
 #include <map>
+#include <set>
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -47,7 +48,7 @@ class ConstraintSolver {
         LCPSolver* lcpSolver;                                   // LCP Solver
         std::vector<Constraint*> activeConstraints;             // Current active constraints in the physics world
         uint nbConstraints;                                     // Total number of constraints (with the auxiliary constraints)
-        std::vector<Body*> constraintBodies;                    // Bodies that are implied in some constraint
+        std::set<Body*> constraintBodies;                       // Bodies that are implied in some constraint
         uint nbBodies;                                          // Current number of bodies in the physics world
         std::map<Body*, uint> bodyNumberMapping;                // Map a body pointer with its index number
         Body*** bodyMapping;                                    // 2-dimensional array that contains the mapping of body reference