bah ecoute ok

include/env_generator.h

@@ -24,7 +24,6 @@ void generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dar
 template <typename PFP>
 void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize, unsigned int nbSquares);
 
-
 template <typename PFP>
 void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize);
 

include/env_generator.hpp

@@ -63,6 +63,7 @@ void generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dar
 {
 	// mark the face as obstacle before extrusion
 	Dart dd = d;
+	buildingMark.mark(dd);
 	do
 	{
 		obstacleMark.mark(dd);
@@ -143,6 +144,7 @@ void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMar
 	}
 }
 
+
 template <typename PFP>
 void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize)
 {
@@ -154,7 +156,7 @@ void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMark
 
 	floor2->embedGrid(sideSize*side,sideSize*side);
 
-	for(unsigned int i=0;i<5;++i)
+	for(unsigned int i=0;i<3;++i)
 	{
 		float floorHeight = 100;
 		typename PFP::VEC3 transl(0,0,floorHeight);
@@ -194,7 +196,7 @@ void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMark
 	map.closeHole(boundary);
 	buildingMark.mark(map.phi2(boundary));
 
-	installGuardRail<PFP>(map,position,obstacleMark,buildingMark,sideSize/5.0f);
+	installGuardRail<PFP>(map,position,obstacleMark,buildingMark,sideSize/10.0f);
 }
 
 template <typename PFP>
@@ -434,7 +436,7 @@ bool isAnEar(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart dd, flo
 	typedef typename PFP::VEC3 VEC3;
 	
 	bool check = false;
-	if(Geom::testOrientation2D(position[dd], position[map.phi_1(dd)], position[map.phi1(dd)]) == Geom::RIGHT)
+	if(Geom::testOrientation2D(position[dd], position[map.phi_1(dd)], position[map.phi1(dd)]) == Geom::RIGHT && !map.sameFace(map.phi2(dd),map.phi2(map.phi_1(dd))))
 	{
 		VEC3 p = position[dd];
 		VEC3 p1 = position[map.phi1(dd)];
@@ -496,7 +498,8 @@ void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, C
 
 	float area;
 	CellMarker m(map, FACE);
-	for(Dart d = map.begin(); d != map.end(); map.next(d))
+
+	for(Dart d = map.begin();d != map.end(); map.next(d))
 	{
 		if(!m.isMarked(d) && !obstacleMark.isMarked(d) && !buildingMark.isMarked(d))
 		{
@@ -518,12 +521,16 @@ void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, C
 
 				if(liloo.find(map.phi_1(dd)) != liloo.end())
 				{
-					moultipass.erase(liloo[map.phi_1(dd)]);
+ 					moultipass.erase(liloo[map.phi_1(dd)]);
 					liloo.erase(map.phi_1(dd));
+
 				}
 
-				moultipass.erase(liloo[dd]);
-				liloo.erase(dd);
+				if(moultipass.size()>0)
+				{
+					moultipass.erase(liloo[dd]);
+					liloo.erase(dd);
+				}
 
 				if(liloo.find(map.phi1(dd))!=liloo.end())
 				{
@@ -548,9 +555,9 @@ void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, C
 	}
 
 	//simplification
-	for(Dart d = map.begin(); d != map.end(); map.next(d))
-		if(canRemoveEdgeConvex<PFP>(map, position, d, obstacleMark) && canRemoveEdgeConvex<PFP>(map, position, map.phi2(d), obstacleMark))
-			map.mergeFaces(d);
+//	for(Dart d = map.begin(); d != map.end(); map.next(d))
+//		if(canRemoveEdgeConvex<PFP>(map, position, d, obstacleMark) && canRemoveEdgeConvex<PFP>(map, position, map.phi2(d), obstacleMark))
+//			map.mergeFaces(d);
 }
 
 template <typename PFP>

include/env_map.h

@@ -16,6 +16,7 @@
 #include "Algo/Modelisation/subdivision.h"
 #include "Algo/Geometry/centroid.h"
 #include "Algo/Geometry/area.h"
+#include "Geometry/bounding_box.h"
 
 #include "Container/fakeAttribute.h"
 
@@ -52,8 +53,13 @@ public :
 	EnvMap();
 	
 	unsigned int mapMemoryCost();
+	void scale(float scaleVal);
+
 	Dart getBelongingCell(const PFP::VEC3& pos);
 
+	void subdivideAllToMaxLevel();
+	void subdivideToProperLevel();
+
 	void init();
 	
 	void foreach_neighborFace(Dart d, FunctorType& f);

include/env_render.h

@@ -48,9 +48,9 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
 	VEC3 pos = agent->part_.m_position;
 	float radius = agent->radius_;
 	
-//	Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
-//	pos[2] -= 1000;
-//	Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
+	Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
+	pos[2] -= 1000;
+	Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
 
 	glLineWidth(1.0f);
 	
@@ -63,7 +63,7 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
 	VEC3 dir = agent->velocity_;
 	dir.normalize();
 
-// 	//draw the speed vector
+ 	//draw the speed vector
 //	VEC3 base(0,0,1);
 //	VEC3 myAxisRot = base^dir;
 //	myAxisRot.normalize();

include/path_finder.hpp

@@ -63,6 +63,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
 
 				ddd = map.alpha1(ddd);
 			}
+
 			dd = map.phi1(dd);
 		} while(dd != toDev);
 

include/simulator.h

@@ -85,6 +85,10 @@ public:
 	
 	void doStep();
 	
+	bool reachedGoal();
+
+	void setupCircleScenario(unsigned int nbMaxAgent);
+	void setupCityScenario(float startX, float startY, int nbLines , int nbRank);
 	void setupScenario(unsigned int nbMaxAgent);
 	void addPathsToAgents();
 	
@@ -93,6 +97,8 @@ public:
 	
 	void swapAgentsGoals();
 	
+	Geom::BoundingBox<VEC3> getAgentsBB();
+
 	EnvMap envMap_;
 	std::vector<Agent*> agents_;
 	

include/utils.h

@@ -9,7 +9,7 @@
 
 typedef CGoGN::Geom::Vec3f VEC3;
 
-static const float RVO_EPSILON = 0.00001f;
+static const float RVO_EPSILON = 0.0001f;
 
 struct Line {
 	VEC3 point;

include/viewer.h

@@ -30,6 +30,7 @@
 
 #include "simulator.h"
 #include "env_render.h"
+#include "exportObstacles.h"
 #include "gl2ps.h"
 
 #include "Algo/Geometry/boundingbox.h"
@@ -55,6 +56,7 @@ public:
 	void cb_initGL() ;
 	void cb_redraw() ;
 
+	void exportInfoFace(std::ofstream& out, Dart d);
 	bool exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::string& filename, PFP::VEC3 cameraPos, PFP::VEC3 cameraLook, PFP::VEC3 translate, float angle_X, float angle_Y, float angle_Z,const FunctorSelect& good = SelectorTrue());
 	
 	void cb_keyPress(int keycode) ;
@@ -76,6 +78,8 @@ public:
 	int visu;
 	int nbGenerated;
 
+	double elapsedTime;
+
 	bool drawEnvLines ;
 	bool drawEnvFaces ;
 	bool drawEnvTopo ;

src/agent.cpp

@@ -49,7 +49,10 @@ void Agent::updateAgentNeighbors()
 		{
 			if(distSq > maxDist)
 				maxDist = distSq;
-			agentNeighbors_.push_back(std::make_pair(distSq, *it));
+
+//			if(		sim_->envMap_.map.sameFace(part_.d,(*it)->part_.d) ||
+//					!sim_->envMap_.buildingMark.isMarked(sim_->envMap_.map.phi2(part_.faceOrientationState((*it)->getPosition()))))
+					agentNeighbors_.push_back(std::make_pair(distSq, *it));
 		}
 	}
 
@@ -61,7 +64,10 @@ void Agent::updateAgentNeighbors()
 		{
 			if(distSq > maxDist)
 				maxDist = distSq;
-			agentNeighbors_.push_back(std::make_pair(distSq, *it));
+
+//			if(		sim_->envMap_.map.sameFace(part_.d,(*it)->part_.d) ||
+//					!sim_->envMap_.buildingMark.isMarked(sim_->envMap_.map.phi2(part_.faceOrientationState((*it)->getPosition()))))
+				agentNeighbors_.push_back(std::make_pair(distSq, *it));
 		}
 	}
 
@@ -94,10 +100,10 @@ void Agent::update()
 	meanSpeed_ *= 3.0f;
 	meanSpeed_ += velocity_;
 	meanSpeed_ /= 4.0f;
-
-	meanPos_ *= 9.0f;
-	meanPos_ += part_.m_position;
-	meanPos_ /= 10.0f;
+//
+//	meanPos_ *= 9.0f;
+//	meanPos_ += part_.m_position;
+//	meanPos_ /= 10.0f;
 
 	part_.move(target);
 }
@@ -107,7 +113,7 @@ void Agent::computePrefVelocity()
 	VEC3 goalVector = goals_[curGoal_] - part_.m_position;
 	float goalDist2 = goalVector.norm2();
 	
-	if(goalDist2 < 100.0f)
+	if(goalDist2 < 200.0f)
 	{
 		curGoal_ = (curGoal_ + 1) % goals_.size();
 		goalVector = goals_[curGoal_] - part_.m_position;
@@ -120,10 +126,11 @@ void Agent::computePrefVelocity()
 	/*
 	* Perturb a little to avoid deadlocks due to perfect symmetry.
 	*/
-	float angle = std::rand() * 2.0f * M_PI / RAND_MAX;
-	float dist = std::rand() * 0.0001f / RAND_MAX;
+//	float angle = std::rand() * 2.0f * M_PI / RAND_MAX;
+//	float dist = std::rand() * 0.0001f / RAND_MAX;
 	
-	prefVelocity_ = goalVector + VEC3(dist * cos(angle), dist * sin(angle), 0.0f);
+//	prefVelocity_ = goalVector + VEC3(dist * cos(angle), dist * sin(angle), 0.0f);
+	prefVelocity_ = goalVector;
 }
 
 void Agent::computeNewVelocity2()
@@ -517,6 +524,7 @@ bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo,
 			return false;
 	}
 
+
 	/* Optimize direction. */
 	if (directionOpt)
 	{
@@ -562,6 +570,8 @@ bool Agent::linearProgram2(const std::vector<Line>& lines, unsigned int num, flo
 	else
 		result = optVelocity;
 
+
+
 	for (unsigned int i = 0; i < num; ++i)
 	{
 		if (det2D(lines[i].direction, result - lines[i].point) < 0.0f)

src/env_map.cpp

@@ -6,6 +6,8 @@
 #include "Algo/MovingObjects/particle_cell_2D_memo.h"
 #include "Algo/Modelisation/subdivision.h"
 #include "Algo/Geometry/normal.h"
+#include "Algo/Import/importSvg.h"
+#include "Algo/BooleanOperator/mergeVertices.h"
 
 using namespace CGoGN;
 
@@ -21,6 +23,24 @@ EnvMap::EnvMap() :
 	neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE, "neighborAgents");
 	obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE, "obstacles");
 	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB>(FACE, "subdivisableFace");
+
+	refineCandidate.reserve(100);
+	coarsenCandidate.reserve(100);
+
+//	std::string filename = "/home/jund/Desktop/drawingQuads.svg";
+//	std::string filename = "/home/jund/Desktop/drawingTest.svg";
+//	std::string filename = "/home/jund/Desktop/drawingSimple.svg";
+//	std::string filename = "/home/jund/Desktop/drawingLines.svg";
+//	std::string filename = "/home/jund/Desktop/mapKrutSimple.svg";
+//	std::string filename = "/home/jund/Desktop/mapKrut.svg";
+//	Algo::Import::importSVG<PFP>(map,filename,position, obstacleMark, buildingMark);
+//	scale(3.2808399f);
+
+//	Algo::BooleanOperator::mergeVertices<PFP>(map,position);
+//	DartMarker amelk(map);
+//	map.closeMap(amelk);
+//	Algo::Modelisation::CatmullClarkSubdivision<PFP>(map,position);
+//	Algo::Modelisation::computeDual<PFP>(map);
 }
 
 unsigned int EnvMap::mapMemoryCost()
@@ -31,6 +51,106 @@ unsigned int EnvMap::mapMemoryCost()
 			+ (map.getAttributeContainer(FACE)).memorySize();
 }
 
+void EnvMap::scale(float scaleVal)
+{
+	Geom::BoundingBox<PFP::VEC3> bb(position.begin());
+
+	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
+	{
+		bb.addPoint(position[v]);
+	}
+
+	VEC3 center = bb.center();
+	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
+	{
+		position[v] -= center;
+		position[v] *= scaleVal;
+	}
+}
+
+void EnvMap::subdivideAllToMaxLevel()
+{
+	bool subdiv;
+	do
+	{
+		subdiv=false;
+		{
+			CellMarker subd(map,FACE);
+			for(Dart d = map.begin(); d!= map.end(); map.next(d))
+			{
+				if(!subd.isMarked(d))
+				{
+					subd.mark(d);
+					if(!buildingMark.isMarked(d))
+					{
+						//check if subdivision is authorized
+						float minDistSq = Agent::neighborDistSq_ ;
+						bool subdivisable = true;
+						Dart old = map.faceOldestDart(d) ;
+						unsigned int fLevel = map.faceLevel(old) ;
+						map.setCurrentLevel(fLevel) ;
+						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
+						Dart fd = old ;
+						do
+						{
+							PFP::VEC3& p = position[fd] ;
+							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
+							PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
+							if(proj.norm2() < minDistSq)
+							{
+								subdivisable = false ;
+								break ;
+							}
+							fd = map.phi1(fd) ;
+						} while(fd != old) ;
+
+						if(subdivisable)
+						{
+							map.setCurrentLevel(fLevel) ;
+							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
+							subdiv = true;
+						}
+						map.setCurrentLevel(map.getMaxLevel()) ;
+					}
+				}
+			}
+		}
+	} while(subdiv);
+}
+
+void EnvMap::subdivideToProperLevel()
+{
+	bool subdiv;
+	do
+	{
+		subdiv=false;
+		{
+			CellMarker subd(map,FACE);
+			for(Dart d = map.begin(); d!= map.end(); map.next(d))
+			{
+				if(!subd.isMarked(d))
+				{
+					subd.mark(d);
+					if(!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
+					{
+						std::pair<bool,bool>& sf = subdivisableFace[d];
+						if(sf.first == false || (sf.first == true && sf.second))
+						{
+							subdiv=true;
+							refineMark.mark(d);
+							refineCandidate.push_back(d);
+						}
+					}
+				}
+			}
+			subd.unmarkAll();
+		}
+		updateMap();
+		refineCandidate.clear();
+		map.setCurrentLevel(map.getMaxLevel());
+	} while(subdiv);
+}
+
 Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
 {
 	CellMarkerStore m(map, FACE);
@@ -51,12 +171,22 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
 void EnvMap::init()
 {
 	float sideSize = 70.0f;
-	unsigned int nbSquares = 14;
+	unsigned int nbSquares = 5;
+//	float sideSize = 420.0f;
+//	unsigned int nbSquares = 4;
+
+//	CityGenerator::generateGrid<PFP>(map, position,nbSquares,nbSquares, sideSize, obstacleMark,buildingMark);
+//	CityGenerator::generateCorridor<PFP>(map, position, obstacleMark, buildingMark, 50, nbSquares);
 	CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
 //	CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
 //	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
+//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
+//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);
 	map.init();
 	registerObstaclesInFaces();
+
+//	subdivideAllToMaxLevel();
+
 	for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
 		subdivisableFace[i].first = false ;
 }
@@ -251,7 +381,7 @@ void EnvMap::updateMap()
 
 		if(agentvect[d].size() > nbAgentsToSubdivide)
 		{
-			unsigned int fLevel = -1 ;
+			int fLevel = -1 ;
 			Dart old = map.faceOldestDart(d) ;
 
 			bool subdivisable = true ;
@@ -361,12 +491,11 @@ void EnvMap::updateMap()
 		}
 	}
 
-	for(std::vector<Dart>::iterator it = coarsenCandidate.begin(); it != coarsenCandidate.end(); ++it)
+	for(unsigned int it = 0; it < coarsenCandidate.size(); ++it)
 	{
-		Dart old = (*it) ;
+		Dart old = coarsenCandidate[it] ;
 		coarsenMark.unmark(old) ;
 
-//		Dart old = map.faceOldestDart(d) ;
 		unsigned int fLevel = map.faceLevel(old) ;
 
 		if(fLevel > 0 && map.getDartLevel(old) < fLevel)
@@ -401,13 +530,13 @@ void EnvMap::updateMap()
 					{
 						map.setCurrentLevel(fLevel) ;
 						coarsenMark.unmark(fit) ;
-						std::vector<Dart>::iterator start = it + 1 ;
+						unsigned int start = it + 1;
 						unsigned int fEmb = map.getEmbedding(FACE, fit) ;
-						while(start != coarsenCandidate.end())
+						while(start < coarsenCandidate.size())
 						{
-							if(map.getEmbedding(FACE, *start) == fEmb)
+							if(map.getEmbedding(FACE, coarsenCandidate[start]) == fEmb)
 							{
-								*start = coarsenCandidate.back() ;
+								coarsenCandidate[start] = coarsenCandidate.back() ;
 								coarsenCandidate.pop_back() ;
 							}
 							else
@@ -439,13 +568,13 @@ void EnvMap::updateMap()
 						map.setCurrentLevel(fLevel) ;
 						Dart centerFace = map.phi2(map.phi1(old)) ;
 						coarsenMark.unmark(centerFace) ;
-						std::vector<Dart>::iterator start = it + 1 ;
+						unsigned int start = it + 1;
 						unsigned int fEmb = map.getEmbedding(FACE, centerFace) ;
-						while(start != coarsenCandidate.end())
+						while(start < coarsenCandidate.size())
 						{
-							if(map.getEmbedding(FACE, *start) == fEmb)
+							if(map.getEmbedding(FACE, coarsenCandidate[start]) == fEmb)
 							{
-								*start = coarsenCandidate.back() ;
+								coarsenCandidate[start] = coarsenCandidate.back() ;
 								coarsenCandidate.pop_back() ;
 							}
 							else
@@ -468,10 +597,10 @@ void EnvMap::updateMap()
 					map.setCurrentLevel(map.getMaxLevel());
 
 					agentvect[old].clear();
-					for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
+					for(PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
 					{
-						(*it)->part_.d = old ;
-						pushAgentInCells(*it, old);
+						(*itA)->part_.d = old ;
+						pushAgentInCells(*itA, old);
 					}
 
 					Dart dd = old;
@@ -486,9 +615,17 @@ void EnvMap::updateMap()
 						dd = map.phi1(dd);
 					} while(dd != old);
 
+					if(fLevel>1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify / 4)
+					{
+						coarsenMark.mark(old);
+						coarsenCandidate.push_back(map.faceOldestDart(old));
+					}
+
 				}
 			}
 			map.setCurrentLevel(cur) ;
+
+
 		}
 	}
 

src/simulator.cpp

 #include "simulator.h"
 
-Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.2f)
+Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.25f)
 {
 	srand(10);
 	envMap_.init();
 	std::cout << "setup scenario" << std::endl;
 //	importAgents("myAgents.pos");
-	setupScenario(700);
+	setupScenario(10);
+//	setupCircleScenario(1000);
+//	setupCityScenario(-1.0f*(24*(70/2.0f)-10),-1.0f*(24*(70.0f/2.0f)-10),167,5);
 //	addPathsToAgents();
 
 	unsigned nbAgents = agents_.size();
@@ -17,10 +19,12 @@ Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.2f)
 		agents_[i]->updateAgentNeighbors();
 	}
 
-//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
-//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
+//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 2);
+//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents );
 //	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
 //	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
+
+	envMap_.subdivideToProperLevel();
 }
 
 Simulator::~Simulator()
@@ -38,15 +42,17 @@ void Simulator::doStep()
 	for (unsigned int i = 0; i < agents_.size(); ++i)
 	{
 		agents_[i]->updateObstacleNeighbors();
+
+		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
 		agents_[i]->updateAgentNeighbors();
+		envMap_.map.setCurrentLevel(0);
+
 		agents_[i]->computePrefVelocity();
 		agents_[i]->computeNewVelocity();
 	}
 
-//	ThreadUpdateInfo tc1(agents_,0,agents_.size()/2);
-//	ThreadUpdateInfo tc2(agents_,agents_.size()/2,agents_.size());
-//	boost::thread thread1(&ThreadUpdateInfo::run, &tc1);
-//	boost::thread thread2(&ThreadUpdateInfo::run, &tc2);
+//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
+//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
 
 //	std::vector<Dart> oldDarts;
 //	oldDarts.reserve(agents_.size());
@@ -64,11 +70,11 @@ void Simulator::doStep()
 	{
 		Dart oldFace = agents_[i]->part_.d;
 
-		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
-			std::cout << "  AGENT PART INVALID DART before update" << std::endl ;
+//		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
+//			std::cout << "  AGENT PART INVALID DART before update" << std::endl ;
 		agents_[i]->update();
-		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
-			std::cout << "  AGENT PART INVALID DART after update" << std::endl ;
+//		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
+//			std::cout << "  AGENT PART INVALID DART after update" << std::endl ;
 
 //		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
 		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
@@ -91,11 +97,67 @@ void Simulator::doStep()
 
 	globalTime_ += timeStep_;
 
-	if(int(globalTime_ / timeStep_) % 2000 == 0)
+//	if(int(globalTime_ / timeStep_) % 2000 == 0)
+//	{
+//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
+//		swapAgentsGoals();
+//	}
+}
+
+bool Simulator::reachedGoal()
+{
+  /* Check if all agents have reached their goals. */
+	for (unsigned int i = 0; i < agents_.size(); ++i)
+		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2() > agents_[i]->radius_ * agents_[i]->radius_)
+			return false;
+
+  return true;
+}
+
+void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
+{
+	float pi = 3.14159265358979323846f;
+