up

include/env_map.h

@@ -66,7 +66,7 @@ public:
 	void draw();
 
 	void scale(float val);
-
+	bool is_insideConvexCell2D(VEC3 p, Dart d);
 	void markPedWay() ;
 
 	unsigned int mapMemoryCost() ;
@@ -221,6 +221,7 @@ public:
 void update_registration(Obstacle * o);
 void register_pop(Obstacle* o, int n);
 void resetPartSubdiv(Obstacle* o);
+//void resetObstPartInFace(Obstacle* o, Dart d, unsigned int fLevel);// empeche de viser une dart ayant disparu
 void resetObstPartInFace(Obstacle* o, Dart d);// empeche de viser une dart ayant disparu
 void resetPart(Obstacle * mo, Dart d); // empeche de viser une dart ayant disparu pour les voisins
 void displayMO(Obstacle * o);

include/viewer.h

@@ -298,7 +298,7 @@ public slots:
 
 					Utils::VBO * vbo = new Utils::VBO();
 					vbo->setDataSize(3);
-					vbo->allocate(3);
+					vbo->allocate(4*simulator.movingObstacles_[i]->nbVertices);
 					shader->setAttributePosition(vbo);
 
 					m_triObst_VBO.push_back(vbo);

src/agent.cpp

@@ -5,7 +5,7 @@
   New behaviour algorithm based on a dynamic model
 \*=====================================================================*/
 
-#define POTENTIAL
+//#define POTENTIAL
 #include "agent.h"
 #include "simulator.h"
 #include "Geometry/frame.h"
@@ -17,14 +17,14 @@ unsigned int Agent::maxNeighbors_ = 10 ;
 unsigned int Agent::maxMovingObstacles_ = 20;
 float Agent::averageMaxSpeed_ = 2.0f ;
 // float Agent::averageMaxSpeed_ = 20.0f ;
-// float Agent::neighborDist_ = 10.0f ;
-float Agent::neighborDist_ = 20.0f ;
+ float Agent::neighborDist_ = 10.0f ;
+//float Agent::neighborDist_ = 20.0f ;
 float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
 //float Agent::radius_ = 8.0f ;
-float Agent::radius_ = 3.0f ;
-//float Agent::radius_ = 1.5f ;
-//float Agent::timeHorizon_ = 10.0f ;
-float Agent::timeHorizon_ = 100.0f ;
+//float Agent::radius_ = 3.0f ;
+float Agent::radius_ = 1.5f ;
+float Agent::timeHorizon_ = 10.0f ;
+//float Agent::timeHorizon_ = 100.0f ;
 float Agent::timeHorizonObst_ = 10.0f ;
 float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
 float Agent::rangeSq_ = range_ * range_ ;

src/env_map.cpp

@@ -404,6 +404,35 @@ void EnvMap::scale(float val)
 
 }
 
+bool EnvMap::is_insideConvexCell2D(VEC3 p, Dart d)
+{
+//	return false;
+	VEC3 vec, norm, p1,p2;
+	Dart dd = d,ddd;
+
+	do
+	{
+		ddd = map.phi1(dd) ;
+		 p1 = position[dd];
+		 p2 = position[ddd];
+
+		vec = VEC3(p2 - p1);
+		norm[0] = vec[1];
+		norm[1] = -vec[0];
+
+		vec = VEC3(p2 -p);
+		if (vec*norm < 0){
+//			CGoGNout<<"not inside !"<<CGoGNendl;
+			return false;
+
+		}
+
+		dd = ddd ;
+	} while (dd != d) ;
+//	CGoGNout<<"c'est dedans !"<<CGoGNendl;
+	return true;
+}
+
 void EnvMap::markPedWay()
 {
 	//split all pedway with 3 roads intersection (to avoid spikes)
@@ -775,7 +804,7 @@ Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
 //	 	if(belonging_cells[n] != memo)
 //			modif=true;
 //	}
-//
+//	if (mo->index==12 && o->index ==1) CGoGNout<<"popAndPush :"<<CGoGNendl;
 //	if(modif)
 	{
 		popObstacleInCells(o, n);
@@ -805,7 +834,7 @@ void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
 	mo->belonging_cells[n].push_back(d);
 	mo->addGeneralCell (d);
 //	mo->neighbor_cells[n].clear();
-//	if(o->index==1 && mo->index==279)	 CGoGNout <<"obstacle"<< o->index << " : "<< d << CGoGNendl;
+//	if(o->index==1 && mo->index==12)	 CGoGNout <<"push : "<< d << CGoGNendl;
 	Dart dd = d;
 	do
 	{
@@ -836,16 +865,16 @@ void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& me
 //	mo->belonging_cells[n].clear();
 //	mo->neighbor_cells[n].clear();
 	mo->belonging_cells[n] = memo_cross;
-//	if(o->index==1 && mo->index==279)	CGoGNout<<"obstacle"<< o->index << " : ";
+//	if (mo->index==12 && o->index ==1) CGoGNout<<"push : "<<CGoGNendl;
 	for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
 	{
-//			if(o->index==1 && mo->index==279)	 CGoGNout << *it << " ; ";
+//			if(o->index==1 && mo->index==12)	 CGoGNout << *it << " ; ";
 
 			addElementToVector<Obstacle*>(obstvect[*it],o);
 			mo->addGeneralCell (*it);
 
 	}
-//	if(o->index==1 && mo->index==279)	CGoGNout <<CGoGNendl;
+//	if(o->index==1 && mo->index==12)	CGoGNout <<CGoGNendl;
 	addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));
 
 	for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
@@ -867,12 +896,14 @@ void EnvMap::popObstacleInCells(Obstacle* o, int n)
 
 	if (mo != NULL)
 	{
+//		if (mo->index==12 && o->index ==1) CGoGNout<<"pop : "<<CGoGNendl;
 		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
 		{
+//			if (mo->index==12 && o->index ==1) CGoGNout<<*it<< " ; ";
 			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
 			mo->removeGeneralCell (*it);
 		}
-
+//		if (mo->index==12 && o->index ==1) CGoGNout<<CGoGNendl<<CGoGNendl;
 		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
 		{
 			if (!map.isBoundaryMarked2(*it))
@@ -912,6 +943,7 @@ void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
 
 void EnvMap::refine()
 {
+//	CGoGNout<<"refine"<<CGoGNendl;
 	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
 	{
 		Dart d = (*it) ;
@@ -1051,7 +1083,7 @@ void EnvMap::refine()
 				//agents contained in the subdivided cell are pushed correctly
 				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
 				{
-
+//					CGoGNout<<"reseting Agents"<<CGoGNendl;
 					resetAgentInFace(*ait) ;
 					pushAgentInCells(*ait, (*ait)->part_.d) ;
 				}
@@ -1059,6 +1091,7 @@ void EnvMap::refine()
 				//same for obstacles contained
 				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
 				{
+//					CGoGNout<<"reseting Obstacles"<<CGoGNendl;
 					resetPartSubdiv(*ait);
 					pushObstacleInCells(*ait);
 
@@ -1083,6 +1116,7 @@ void EnvMap::refine()
 
 void EnvMap::coarse()
 {
+//	CGoGNout<<"coarse"<<CGoGNendl;
 	// On recrée une liste des faces à simplifier en empêchant les doublons
 	// On en profite pour vérifier les conditions de simplifications
 	std::vector<Dart> checkCoarsenCandidate ;
@@ -1218,6 +1252,7 @@ void EnvMap::coarse()
 			map.setCurrentLevel(map.getMaxLevel()) ;
 			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
 			{
+//				resetObstPartInFace(*ait, fit,fLevel);
 				this->popObstacleInCells(*ait, (*ait)->index) ;
 				obst.push_back(*ait);
 			}
@@ -1242,6 +1277,7 @@ void EnvMap::coarse()
 			map.setCurrentLevel(map.getMaxLevel()) ;
 			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
 			{
+//				resetObstPartInFace(*ait, fit,Level);
 				this->popObstacleInCells(*ait, (*ait)->index) ;
 				obst.push_back(*ait);
 			}
@@ -1317,7 +1353,7 @@ void EnvMap::coarse()
 		}
 		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
 		{
-			resetObstPartInFace(*ait, old) ;
+			resetObstPartInFace(*ait, old);
 			pushObstacleInCells(*ait) ;
 		}
 		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)

src/moving_obstacle.cpp

@@ -58,6 +58,8 @@ bool  MovingObstacle::removeGeneralCell (Dart d)
 	return false;
 }
 
+
+
 bool MovingObstacle::is_inside(VEC3 p)
 {
 //	return false;
@@ -316,7 +318,8 @@ void MovingObstacle::draw(bool showPath)
 
 	VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
 
-	if(movingObstacleNeighbors_.size()==0)
+//	if(movingObstacleNeighbors_.size()==0)
+	if(index==12)
 	// if(obstacleNeighbors_.size()==0)
 		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
 	else
@@ -348,8 +351,8 @@ void MovingObstacle::draw(bool showPath)
 VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
 {
 	Dart d(ind); //WARNING : works only for one face created at start !
-//	return position[d]+deformation[d];
-	return position[d];
+	return position[d]+deformation[d];
+//	return position[d];
 }
 
 VEC3 MovingObstacle::getPosition(unsigned int ind)
@@ -1131,18 +1134,25 @@ void resetPartSubdiv(Obstacle* o)
 
 	if (mo != NULL)
 	{
-		int n =o->index;
+		unsigned int n =o->index;
+		unsigned int n2 =(n+1)%(mo->nbVertices);
 
-		VEC3 pos =mo->parts_[n]->getPosition();
+		VEC3 pos = mo->parts_[n]->getPosition();
+		VEC3 pos2 = mo->parts_[n2]->getPosition();
 		mo->parts_[n]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;
 
 		mo->parts_[n]->setState(FACE) ;
 		mo->parts_[n]->move(pos) ;
 
+		mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;
+
+		mo->parts_[n2]->setState(FACE) ;
+		mo->parts_[n2]->move(pos2) ;
+
 		mo->dDir = mo->parts_[0]->d;
 	}
 }
-
+//, unsigned int fLevel
 void resetObstPartInFace(Obstacle* o, Dart d1)
 {
 	MovingObstacle * mo = o->mo;
@@ -1150,14 +1160,42 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
 	if (mo != NULL)
 	{
 		unsigned int n =o->index;
+		unsigned int n2 =(n+1)%(mo->nbVertices);
 		VEC3 pos1 = mo->parts_[n]->getPosition();
-
-		if (Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
+		VEC3 pos2 = mo->parts_[n2]->getPosition();
+//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
+		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos1, d1))
 			mo->parts_[n]->d = d1;
-
+//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos2, true))
+		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos2, d1))
+					mo->parts_[n2]->d = d1;
 		if(n==0)
 			mo->dDir = mo->parts_[0]->d;
 	}
+//
+//	MovingObstacle * mo = o->mo;
+//
+//	if (mo != NULL)
+//	{
+//
+//		unsigned int n =o->index;
+//		unsigned int n2 =(n+1)%mo->nbVertices;
+//		Dart d2 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n]->d);
+//		Dart d3 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n2]->d);
+//		mo->sim_->envMap_.map.setCurrentLevel(fLevel - 1) ;
+//		if ( mo->sim_->envMap_.map.sameFace(d1,d2))
+//		{
+//			CGoGNout<<"particule : "<< n<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n]->d <<" en "<<d2<<CGoGNendl;
+//			mo->parts_[n]->d = d2;
+//
+//		}
+//		if ( mo->sim_->envMap_.map.sameFace(d1,d3))
+//		{
+//			CGoGNout<<"particule : "<< n2<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n2]->d <<" en "<<d3<<CGoGNendl;
+//			mo->parts_[n2]->d = d3;
+//		}
+//		mo->sim_->envMap_.map.setCurrentLevel(mo->sim_->envMap_.map.getMaxLevel()) ;
+//	}
 }
 
 void resetPart(Obstacle * o, Dart d1)
@@ -1170,6 +1208,9 @@ void resetPart(Obstacle * o, Dart d1)
 		if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1))
 			mo->parts_[n]->d = d1;
 
+		if (mo->parts_[(n+1)%mo->nbVertices]->d == mo->sim_->envMap_.map.phi1(d1))
+					mo->parts_[(n+1)%mo->nbVertices]->d = d1;
+
 		if(n==0)
 			mo->dDir = mo->parts_[n]->d;
 	}

src/simulator.cpp

@@ -135,7 +135,9 @@ void Simulator::doStep()
 	struct timespec begTime, endTime ;
 
 	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
+
 	{
+//		unsigned int i =12;
 		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
 		movingObstacles_[i]->computePrefVelocity() ;
 		movingObstacles_[i]->computeNewVelocity() ;
@@ -149,6 +151,7 @@ void Simulator::doStep()
 	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
 	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
 	{
+//		unsigned int i =12;
 		movingObstacles_[i]->updateAgentNeighbors();
 		movingObstacles_[i]->updateObstacleNeighbors();
 		movingObstacles_[i]->updateForces() ;
@@ -156,16 +159,17 @@ void Simulator::doStep()
 	}
 	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
 	time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
-
+//	CGoGNout<<"deplacement obstacles"<<CGoGNendl;
 	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
 	{
+//		unsigned int i =12;
 		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
 		movingObstacles_[i]->applyForces();
 		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
 		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
 		movingObstacles_[i]->updateMesh() ;
 	}
-
+//	CGoGNout<<"deplacement obstacles fin"<<CGoGNendl;
 
 	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
 	{
@@ -397,7 +401,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
 	if (multires)
 	{
 //		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
-		envMap_.init(config, nbObstacles > 10 ? ((nbObstacles*20*xSide[0])/6) : 500.0f , 960.0f, minSize, 320.0f) ; //grosses cases
+		envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f , 960.0f, minSize, 320.0f) ; //grosses cases
 	}
 	else
 	{

src/viewer.cpp

@@ -461,15 +461,18 @@ void SocialAgents::updateObstaclePredTriVBO()
 	{
 		Utils::VBO * vbo = m_triObst_VBO[i];
 		PFP::VEC3* data = static_cast<PFP::VEC3*>(vbo->lockPtr());
+		for(unsigned int j =0; j<simulator.movingObstacles_[i]->nbVertices; j++ )
+		{
+			VEC3 p = simulator.movingObstacles_[i]->getDilatedPosition(j);
+			data[j*4+0] = p;
+			data[j*4+3] = p;
 
-		VEC3 p = simulator.movingObstacles_[i]->getDilatedPosition(0);
-		data[0] = p;
+			p = simulator.envMap_.position[simulator.movingObstacles_[i]->parts_[j]->d];
+			data[j*4+1] = p;
 
-		p = simulator.envMap_.position[simulator.movingObstacles_[i]->parts_[0]->d];
-		data[1] = p;
-
-		p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[0]->d)];
-		data[2] = p;
+			p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[j]->d)];
+			data[j*4+2] = p;
+		}
 
 		vbo->releasePtr();
 	}
@@ -714,7 +717,26 @@ void SocialAgents::cb_redraw()
 			m_ds->endList();
 		}
 	}
+	if (draw_posX)
+	{
+		glDisable(GL_LIGHTING);
+		VEC3 pos =VEC3 (posXSlider,posYSlider,0);
+		m_ds->newList(GL_COMPILE_AND_EXECUTE);
+		m_ds->lineWidth(5.0f);
+		m_ds->pointSize(10.0f);
+		m_ds->begin(GL_POINTS);
+		// fait varier la couleur du plus pres au plus loin
+
+		m_ds->color3f(0.0f , 1.0f, 0.9f);
+
+		m_ds->vertex(pos);
+		m_ds->vertex(pos+VEC3(0,0,50));
+
+		m_ds->vertex(pos+VEC3(0,10,50));
 
+		m_ds->end();
+		m_ds->endList();
+	}
 	if (drawMovingObstacles)
 	{
 #ifdef SHADOWSHELL
@@ -724,7 +746,7 @@ void SocialAgents::cb_redraw()
 		for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i)
 		{
 #ifdef SHADOWSHELL
-			if(simulator.movingObstacles_[i]->index==279){
+			if(simulator.movingObstacles_[i]->index==12){
 				Utils::ShaderFlat* moShader = m_obstShader[i];
 
 //			moShader->setAmbiant(Geom::Vec4f(0.43137254902,0.76862745098,0.8862745098,0.));
@@ -769,9 +791,9 @@ void SocialAgents::cb_redraw()
 		for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i)
 		{
 			Utils::ShaderSimpleColor* moShader = m_triObst_Shader[i];
-
+			MovingObstacle * mo =simulator.movingObstacles_[i];
 			moShader->enableVertexAttribs();
-			glDrawArrays(GL_LINE_LOOP, 0, 3);
+			glDrawArrays(GL_LINE_LOOP, 0, 4*mo->nbVertices);
 			moShader->disableVertexAttribs();
 		}