limace improved

include/env_map.h

@@ -79,7 +79,7 @@ public:
 	void popAgentInCells(Agent* agent, Dart d) ;
 	void agentChangeFace(Agent* agent, Dart oldFace) ;
 
-	void popAndPushObstacleInCells(Obstacle* o, int n);
+	Dart popAndPushObstacleInCells(Obstacle* o, int n);
 	void pushObstacleInCells(Obstacle * mo);
 	void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n);
 	void pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross);

include/moving_obstacle.h

@@ -75,6 +75,7 @@ public:
 	std::vector<VEC3> goals_ ;
 
 	unsigned int curGoal_;
+	Dart dDir;
 
 //	static float neighborDistSq_;
 	static unsigned int maxNeighbors_;

src/env_map.cpp

@@ -554,7 +554,7 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
 	}
 }
 
-void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
+Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
 {
 	MovingObstacle * mo = o->mo;
 
@@ -593,6 +593,8 @@ void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
 			pushObstacleInCells(o, n, memo);
 		}
 	}
+
+	return d1;
 }
 
 void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)

src/moving_obstacle.cpp

@@ -144,6 +144,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 		{
 			edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
 		}
+
+		dDir=dInside;
 	}
 
 	center /= nbVertices;
@@ -197,8 +199,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 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)
@@ -422,11 +424,11 @@ void MovingObstacle::update()
 					VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
 
 					//inertie
-					velocity[dd] = velocity[dd]*0.9f;
+					velocity[dd] = velocity[dd]*0.90f;
 
 					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
 					float norm = v1.norm();
-					float rigidity = 500.0f;
+					float rigidity = 40.0f;
 					float stretch = rigidity*(edgeLength[dd]-v1.norm());
 					VEC3 f = stretch*(v1/norm)*sim_->timeStep_;
 
@@ -495,8 +497,8 @@ void MovingObstacle::update()
 		map.next(d);
 	}
 
-	if(!rigid_)
-		center = position[groundFace];
+//	if(!rigid_)
+//		center = position[groundFace];
 
 	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
 
@@ -526,7 +528,10 @@ void MovingObstacle::update()
 		o->p2 = getDilatedPosition((i+1) % nbVertices);
 		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
 		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
-		sim_->envMap_.popAndPushObstacleInCells(o,i);
+		Dart d = sim_->envMap_.popAndPushObstacleInCells(o,i);
+
+		if(i==0)
+			dDir = d;
 
 		if(sim_->detect_agent_collision)
 		{
@@ -568,6 +573,7 @@ std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest
 	registering_part->move(pos);
 
 	d1=registering_part->d;
+
 //	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
 	return (registering_part->move(dest));
 }
@@ -621,23 +627,64 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
 	VEC3 goalVector;
 	if (index_parent<1)
 	{
-		goalVector = goals_[curGoal_] - center ;
-//		goalVector = goals_[curGoal_] - position[groundFace] ;
-
-		float goalDist2 = goalVector.norm2() ;
-
-		if (goalDist2 < 5.0f)
+		if(rigid_)
 		{
-			curGoal_ = (curGoal_ + 1) % goals_.size() ;
 			goalVector = goals_[curGoal_] - center ;
-//			goalVector = goals_[curGoal_] - position[groundFace] ;
-			goalDist2 = goalVector.norm2() ;
-		}
+	//		goalVector = goals_[curGoal_] - position[groundFace] ;
 
-		if (goalDist2 > maxSpeed_)
+			float goalDist2 = goalVector.norm2() ;
+
+			if (goalDist2 < 5.0f)
+			{
+				curGoal_ = (curGoal_ + 1) % goals_.size() ;
+				goalVector = goals_[curGoal_] - center ;
+	//			goalVector = goals_[curGoal_] - position[groundFace] ;
+				goalDist2 = goalVector.norm2() ;
+			}
+
+			if (goalDist2 > maxSpeed_)
+			{
+				goalVector.normalize() ;
+				goalVector *= maxSpeed_;
+			}
+		}
+		else
 		{
-			goalVector.normalize() ;
-			goalVector *= maxSpeed_;
+//			if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
+//				dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
+
+			VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
+			goalVector = mid - position[0] ;
+
+			Dart dStart=dDir;
+			VEC3 dir = center - position[0];
+			while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
+			{
+//				std::cout << "d " << dDir << std::endl;
+//				std::cout << "dir*goalVector " << dir*goalVector << std::endl;
+//				std::cout << "goal vec " << goalVector << std::endl;
+
+				dDir = sim_->envMap_.map.phi_1(dDir);
+				mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
+				goalVector = mid - position[0] ;
+
+				if(dDir==dStart)
+					break;
+			}
+
+			if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
+				goalVector = VEC3(0);
+
+//			goalVector = mid - center ;
+
+//			std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
+
+//			float goalDist2 = goalVector.norm2() ;
+//			if (goalDist2 > maxSpeed_)
+			{
+				goalVector.normalize() ;
+				goalVector *= maxSpeed_*maxSpeed_;
+			}
 		}
 	}
 	else

src/simulator.cpp

 #include "simulator.h"
 
 Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
-	timeStep_(0.02f),
+	timeStep_(0.05f),
 	globalTime_(0.0f),
 	nbSteps_(0),
 	nbUpdates(0),
@@ -36,7 +36,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
 	{
 		case 0 :
 			setupCircleScenario(nbAgent,nbObst) ;
-			addPathsToAgents();
+//			addPathsToAgents();
 			break ;
 		case 1 :
 			setupCorridorScenario(nbAgent,nbObst) ;
@@ -58,7 +58,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
 			addPathsToAgents();
 //			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
 			addMovingObstacles(nbObst);
-			addPathToObstacles();
+//			addPathToObstacles();
 			break;
 		default:
 			std::cout << "Unknown scenario !" << std::endl ;
@@ -175,8 +175,8 @@ void Simulator::doStep()
 	nbRefineCandidate += envMap_.refineCandidate.size() ;
 	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
 
-	// if (multires)
-		// envMap_.updateMap() ;
+	 if (multires)
+		 envMap_.updateMap() ;
 #endif
 
 	globalTime_ += timeStep_ ;
@@ -641,6 +641,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 {
 	std::vector<VEC3> vPos;
 	VEC3 start;
+	MovingMesh * mm;
+	float maxHeight=10.0f;
 
 	switch(obstType)
 	{
@@ -658,9 +660,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 			break;
 		case 1 :
 		{
-			MovingMesh * mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
+			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
 			movingMeshes_.push_back(mm);
-			float maxHeight=10.0f;
 			vPos = mm->computeProjectedPointSet(maxHeight);
 		}
 			break;
@@ -669,6 +670,33 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 			return;
 	}
 
+	bool orientAccordingToFace=true;
+	if(orientAccordingToFace)
+	{
+		while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
+		{
+			d = envMap_.map.phi1(d);
+		}
+
+		VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
+		VEC3 front = vPos[0];
+
+		VEC3 bary(0);
+		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
+			bary += *it;
+
+		bary /= vPos.size();
+
+		float angle = get_angle(mid - bary,front  - bary);
+
+		TraversorV<PFP::MAP> tv(mm->map);
+		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
+		{
+			mm->position[d]+=rotate(mm->position[d], bary, angle);
+		}
+		vPos = mm->computeProjectedPointSet(maxHeight);
+	}
+
 	std::vector<VEC3> goals;
 	goals.push_back(start);
 	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
@@ -723,7 +751,6 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
 
 	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
 	{
-//		VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
 		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
 		dest /= 2.0f ;
 
@@ -738,29 +765,29 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
 
 	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
 	{
-//		VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
 		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
 		dest /= 2.0f ;
 
 		mo->goals_.push_back(dest) ;
 	}
 
-	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_+1]-mo->center))<0)
-	{
-		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
-
-		Geom::Matrix44f m;
-		m.identity();
-		rotateZ(180.0f,m);
-
-		TraversorV<PFP::MAP> tv(mo->map);
-		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
-		{
-			mo->position[d] -= mo->center;
-			mo->position[d] = Geom::transform(mo->position[d],m);
-			mo->position[d] += mo->center;
-		}
-	}
+//	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
+//	{
+//		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
+//
+//		Geom::Matrix44f m;
+//		m.identity();
+//		rotateZ(180.0f,m);
+//
+//		TraversorV<PFP::MAP> tv(mo->map);
+//		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
+//		{
+//			mo->position[d] -= mo->center;
+//			mo->position[d] = Geom::transform(mo->position[d],m);
+//			mo->position[d] += mo->center;
+//			mo->position[d][2] += 10.0f;
+//		}
+//	}
 }
 
 #ifndef SPATIAL_HASHING

src/viewer.cpp

@@ -414,8 +414,13 @@ void SocialAgents::cb_redraw()
 
 	if (drawObstPredictionTri)
 	{
-//		for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
-//		{
+		for (std::vector<MovingObstacle*>::iterator it = simulator.movingObstacles_.begin() ; it != simulator.movingObstacles_.end() ; ++it)
+		{
+			renderPredictionTriangle(simulator.envMap_, (*it)->dDir, (*it)->getDilatedPosition(0)) ;
+
+			std::cout << "dDir2 " << (*it)->dDir << std::endl;
+
+
 //			for(unsigned int i = 0 ; i < (*it)->nbVertices ; ++i)
 //			{
 //				CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> p = *(*it)->parts_[i];
@@ -428,7 +433,7 @@ void SocialAgents::cb_redraw()
 //					glVertex3fv(p.getPosition().data());
 //				glEnd();
 //			}
-//		}
+		}
 	}
 #endif