proprification et correction de la recherche de chemin

>>>>>> .r274

Apps/Examples/SocialAgents/include/agent.h

@@ -61,7 +61,7 @@ class Simulator;
 
 		size_t agentNo_;
 		size_t goalNo_;
-		Dart nearestDart;
+//		Dart nearestDart;
 // 		std::vector<Dart> includingFaces;
 		CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> * part;
 		float rangeSq;

Apps/Examples/SocialAgents/include/env_generator.hpp

@@ -148,7 +148,7 @@ void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& close
 // 	sideSize *= 0.2f;
 	unsigned int nbBuilding=1000;
 	float height = sideSize/2.0f;
-	unsigned int side = 20;
+	unsigned int side = 3;
 	generateGrid<PFP,EMBV>(map,position,side,side,sideSize,closeMark);
 
 	Dart dEnd = map.end();

Apps/Examples/SocialAgents/include/env_render.h

@@ -5,13 +5,13 @@
 #include <GL/glut.h>
 
 
-static void renderPoint(EnvMap& m, Dart& d)
-{
-	PFP::VEC3 p = m.position[d];
-	glBegin(GL_POINTS);
-		glVertex3f(p[0],p[1],p[2]);
-	glEnd();
-}
+//static void renderPoint(EnvMap& m, Dart& d)
+//{
+//	PFP::VEC3 p = m.position[d];
+//	glBegin(GL_POINTS);
+//		glVertex3f(p[0],p[1],p[2]);
+//	glEnd();
+//}
 
 static void renderDart(EnvMap& m, Dart d)
 {
@@ -33,24 +33,24 @@ static void renderFace(EnvMap& m, Dart& d)
 	}while(dd!=d);
 }
 
-static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
-{
-	switch(dim) {
-		case 0 : renderPoint(m,d); break;
-		case 1 : renderDart(m,d);  break;
-		case 2 : renderFace(m,d);  break;
-	}
-}
-
-static void renderAllPoints(EnvMap& m)
-{
-	glBegin(GL_POINTS);
-	for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
-		PFP::VEC3 p = m.position[d];
-		glVertex3f(p[0],p[1],p[2]);
-	}
-	glEnd();
-}
+//static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
+//{
+//	switch(dim) {
+//		case 0 : renderPoint(m,d); break;
+//		case 1 : renderDart(m,d);  break;
+//		case 2 : renderFace(m,d);  break;
+//	}
+//}
+//
+//static void renderAllPoints(EnvMap& m)
+//{
+//	glBegin(GL_POINTS);
+//	for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
+//		PFP::VEC3 p = m.position[d];
+//		glVertex3f(p[0],p[1],p[2]);
+//	}
+//	glEnd();
+//}
 
 static void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
 {

Apps/Examples/SocialAgents/include/path_finder.hpp

@@ -24,6 +24,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
 	std::vector<std::pair<float,Dart> > vToDev;
 
 	Dart toDev = stopPos;
+	vToDev.push_back(std::make_pair(0, toDev));
+	tablePred[toDev] = toDev;
 	do {
 		//dev cell
 		//get all vertex-adjacent cells 
@@ -33,12 +35,12 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
 			do {
 				ddd = map.alpha1(ddd);
 				if(ddd!=dd) {
-					if(!map.foreach_dart_of_face(ddd,bad)) {
+					if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) {
 						//evaluate their cost and push them in the vector to dev
 						if(tablePred[ddd]==EMBNULL)
 							tablePred[ddd]=toDev;
 						std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin();
-						float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd);
+						float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
 						while(it!=vToDev.end() && (*it).first<costDDD)
 							++it;
 						vToDev.insert(it, std::make_pair(costDDD, ddd));
@@ -58,6 +60,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
 	//if path found : from start to stop -> push all predecessors
 	if(map.sameOrientedFace(startPos,toDev))
 	{
+		std::cout << "found" << std::endl;
 		Dart toPush=startPos;
 		std::cout << tablePred[startPos] << std::endl;
 		do {
@@ -72,4 +75,4 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
 //namespace
 }
 
-}
\ No newline at end of file
+}

Apps/Examples/SocialAgents/include/simulator.h

@@ -73,6 +73,7 @@ class Simulator
 
 		//scenarii
 		void setupScenario();
+		void setupTobogganScenario(float rMax,float rMin);
 		void setupHelicoidScenario(float rMax,float rMin);
 };
 

Apps/Examples/SocialAgents/src/agent.cpp

@@ -8,22 +8,21 @@ Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(
 
 Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
 {
-	nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
-	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
+	Dart d =  sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
+	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
 	rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
 }
 
 Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
 {
-	nearestDart = d;
-	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
+	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
 	rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
 }
 
 Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false)
 {
-	nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
-	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
+	Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
+	part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
 	rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
 }
 
@@ -355,7 +354,6 @@ void Agent::update()
 	position_ += velocity_ * sim_->timeStep_;
 
 	part->move(VEC3(position_[0],position_[1],0));
-	nearestDart = part->getCell();
 }
 
 bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result)

Apps/Examples/SocialAgents/src/env_map.cpp

@@ -218,6 +218,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
 
 void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
 {
+	map.setCurrentLevel(map.getMinLevel()) ;
 	Dart dd =d;
 	do {
 		if(closeMark.isMarked(dd) /*&& (position[map.phi2(dd)][2]==0.0 || position[map.phi1(map.phi2(dd))][2] ==0.0f)*/) {
@@ -230,6 +231,8 @@ void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
 		}
 		dd = map.phi1(dd);
 	} while (dd!=d);
+
+	map.setCurrentLevel(map.getMaxLevel()) ;
 }
 
 void EnvMap::getAllFacesOfAgents(Dart d)
@@ -431,13 +434,15 @@ void EnvMap::subdivideFaces()
 					for(std::vector<Dart>::iterator it = marked.begin(); it != marked.end(); ++it)
 						closeMark.mark(map.phi2(*it)) ;
 
-					map.setCurrentLevel(cur) ;
+
 
 					for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
 					{
 						resetAgentInFace(*it) ;
 						agentvect[(*it)->part->d].push_back(*it) ;
 					}
+
+					map.setCurrentLevel(cur) ;
 				}
 			}
 		}
@@ -684,7 +689,7 @@ VEC3 EnvMap::faceCenter(Dart d)
 void EnvMap::resetAgentInFace(Agent * agent)
 {
 // 	agent->part->state = VERTEX_ORBIT;
-// 	agent->part->m_position = position[d];
+// 	agent->part->m_position = position[agent->part->d];
 
 // 	agent->part->m_position = agent->part->pointInFace(agent->part->d);
 	agent->part->m_position = Algo::Geometry::faceCentroid<PFP>(map,agent->part->d,position);

Apps/Examples/SocialAgents/src/viewer.cpp

@@ -191,7 +191,7 @@ void MyGlutWin::myRedraw()
 // 		glColor3f(i%int(2.0f/3.0f*v.size())/(v.size()/3.0f),i%int(1.0f/3.0f*v.size())/(v.size()/3.0f),i/(v.size()/3.0f));
 //		glCircle3i(v[i][0],v[i][1],1.5f);
 		glPushMatrix();
-//			Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->nearestDart,sim->envMap.position);
+//			Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position);
 			VEC3 pBase(sim->agents_[i]->position_);
 			VEC3 posR = pBase;
 //			pl.normal() = -1.0f*pl.normal();
@@ -340,7 +340,7 @@ void MyGlutWin::myRedraw()
 	}
 	else if(visu==3) {
 // 		renderDart(sim->envMap,sim->envMap.getDart(33701));
-// 		renderDart(sim->envMap,sim->envMap.getDart(452));
+// 		renderDart(sim->envMap,sim->envMap.getDart(2));
 // 		renderCellOfDim(sim->envMap,sim->envMap.getDart(1257),2);
 // 		glBegin(GL_POINTS);
 // 		glVertex3f(-494.518,-672,0);
@@ -474,7 +474,7 @@ bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::s
 	std::vector<PFP::VEC3> v = sim->getAgentsPosition();
 	for(unsigned int i = 0; i< sim->agents_.size() ; ++i) {
 		out << "cylinder {" << std::endl;
-			Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->nearestDart,sim->envMap.position);
+			Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position);
 			VEC3 pBase(sim->agents_[i]->position_);
 			VEC3 posR;
 			pl.normal() = -1.0f*pl.normal();

include/Algo/MovingObjects/particle_cell_2D.h

@@ -72,7 +72,6 @@ class ParticleCell2D : public ParticleBase
 	void move(const VEC3& newCurrent)
 	{
 		if(!Geom::arePointsEquals(newCurrent, m_position)) {
-	// 		std::cout << "move to : " << newCurrent << std::endl;
 			prevPos = m_position;
 
 			switch(state) {
@@ -80,6 +79,8 @@ class ParticleCell2D : public ParticleBase
 			case EDGE_ORBIT : 	edgeState(newCurrent);   break;
 			case FACE_ORBIT : 	faceState(newCurrent);   break;
 			}
+
+			display();
 		}
 	}
 

include/Algo/MovingObjects/particle_cell_2D.hpp

@@ -200,6 +200,7 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
 									edgeState(current);
 									return;
 				case Geom::RIGHT :
+									std::cout << std::setprecision(10);
 									std::cout << "smthg went bad " << m_position << " " << current << std::endl;
 									std::cout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << std::endl;
 									m_position = intersectLineEdge(current,m_position,d);

include/Geometry/inclusion.hpp

@@ -163,7 +163,7 @@ bool arePointsEquals(const VEC3& point1, const VEC3& point2)
 {
 	VEC3 v(point1 - point2);
 
-#define PRECISION 1e-20
+#define PRECISION 1e-6
 	return v.norm2() <= PRECISION ;
 #undef PRECISION
 }