ajout obstacles articulés + evitement arash

Debug/CMakeLists.txt

@@ -25,6 +25,7 @@ add_executable( socialAgentsD
 	../src/env_map.cpp
 	../src/agent.cpp
 	../src/moving_obstacle.cpp
+	../src/articulated_obstacle.cpp
 	../src/simulator.cpp
 	../src/moving_mesh.cpp
 	../src/gl2ps.c

Release/CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable( socialAgents
 	../src/env_map.cpp
 	../src/agent.cpp
 	../src/moving_obstacle.cpp
+	../src/articulated_obstacle.cpp
 	../src/simulator.cpp
 	../src/moving_mesh.cpp
 	../src/gl2ps.c

include/articulated_obstacle.h

+#ifndef M_ARTICULATED_OBSTACLE_H
+#define M_ARTICULATED_OBSTACLE_H
+
+#include "moving_obstacle.h"
+using namespace std;
+
+class ArticulatedObstacle
+{
+public:
+	ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int nbParts, std::vector<VEC3> goals);
+
+	std::vector<MovingObstacle *> members;
+	int index;
+	int nbBodyPart;
+	std::vector<VEC3> goals;
+	VEC3 curGoal;
+};
+
+#endif

include/env_map.h

@@ -28,6 +28,7 @@ using namespace CGoGN ;
 class Agent;
 class Obstacle;
 class MovingObstacle;
+class ArticulatedObstacle;
 
 struct PFP : public PFP_STANDARD
 {

include/moving_obstacle.h

@@ -10,13 +10,13 @@
 using namespace std;
 PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
 float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
-
+PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
 class Simulator ;
 
 class MovingObstacle
 {
 public:
-	MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin);
+	MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin, ArticulatedObstacle * art=NULL, int ind2=-1);
 	bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
 //	void contournerBatiment();
 	void updateAgentNeighbors() ;
@@ -66,6 +66,8 @@ public:
 	VEC3 prefVelocity_;
 	Simulator* sim_;
 	bool spinning;
+	ArticulatedObstacle * parent;
+	int index_parent;
 };
 
 #endif

include/simulator.h

@@ -7,7 +7,7 @@
 #include "env_map.h"
 #include "agent.h"
 #include "obstacle.h"
-#include "moving_obstacle.h"
+#include "articulated_obstacle.h"
 #include "moving_mesh.h"
 #include "path_finder.h"
 
@@ -98,6 +98,7 @@ public:
 
 	void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
 	void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
+	void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
 	void setupCityScenario(int nbLines, int nbRank) ;
 	void setupScenario(unsigned int nbMaxAgent) ;
 

src/agent.cpp

@@ -338,6 +338,132 @@ void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'ob
 
 }
 
+
+//void Agent::computeNewVelocity()  // de Arash
+//{
+//
+//	// The objective is to compute the sum of forces exerted on the agent.
+//	double collision_softening_factor;
+//	double ag_mass = 1.0;
+//	float ag_ambient_damping = 1.0;
+//
+//	//-------------
+//
+//	VEC3 p1, p2, vec, forces, previous_pos;
+//	forces.zero();
+//
+//	previous_pos = getPosition() - velocity_*sim_->timeStep_;
+//
+//	//----- force due à l'attraction de l'objectif ----------
+//
+//	float goal_attraction_force = 200.0;  // agent-goal interaction stiffness
+//	VEC3 p_goal = goals_[curGoal_];
+//	VEC3 u_goal = p_goal - getPosition();
+//	u_goal.normalize();
+//
+//	forces += goal_attraction_force * u_goal;
+//
+//	//----- forces dues à la répulsion des obstacles ----------
+//
+//	VEC3 norm;
+//	double obst_stiffness = 50.0; // agent-obstacle interaction stiffness
+//	// double obst_damping   = 1.0;  // agent-obstacle interaction damping
+//	int obst_power = 4;           // the power to which elevate the agent-obstacle distance
+//	Obstacle* obst ;
+//
+//	for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
+//		it != movingObstacleNeighbors_.end() ;
+//		++it)
+//	{
+//		double dist = it->first;
+//		// double effective_range = 50*range_;
+//		double effective_range = 10*range_;
+//		float force_value=0.0;
+//		if(dist < effective_range)
+//		{
+//			collision_softening_factor = pow(1 - dist/effective_range,obst_power);
+//			force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
+//		}
+//
+//		obst=it->second ;
+//		p1=obst->p1 ;
+//		p2=obst->p2 ;
+//		vec=p2-p1;
+//		vec.normalize();
+//		norm.zero();
+//
+//		if (sim_->avoidance==0)// avoids with normal of obstacle side
+//		{
+//			norm[0]=vec[1] ;
+//			norm[1]=-vec[0] ;
+//		}
+//		else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
+//		{
+//			MovingObstacle * mo = obst->mo;
+//			norm = this->part_.getPosition()-mo->center;
+//		}
+//		forces += force_value * norm;
+//	}
+//
+//	//----- forces dues à la répulsion des autres agents -------
+//
+//	double ag_stiffness = 20.0;    // agent-agent interaction stiffness
+//	double ag_damping   = 1.0;     // agent-agent interaction damping
+//	double ag_phys_radius_coef = 20.0;
+//	int ag_power = 8;        // the power to which elevate the agent-agent distance
+//
+//	unsigned int nbA = 0 ;
+//
+//	for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
+//		it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
+//	{
+//		Agent* other = it->second ;
+//
+//		const VEC3 relativePosition(other->getPosition()-getPosition());
+//		const float distSq = relativePosition.norm2() ;
+//		const float dist   = sqrt(distSq);
+//		const float combinedRadius = ag_phys_radius_coef*(radius_ + other->radius_) ;
+//		// const float combinedRadiusSq = combinedRadius * combinedRadius ;
+//
+//		VEC3 other_previous_pos = other->getPosition() - (other->velocity_*sim_->timeStep_);
+//		VEC3 previous_relativePosition = other_previous_pos - previous_pos;
+//		float previous_distSq = previous_relativePosition.norm2();
+//		float previous_dist = sqrt(previous_distSq);
+//
+//		// const VEC3 u_other(relativePosition);
+//		VEC3 u_other(relativePosition);
+//		u_other = relativePosition;
+//		u_other.normalize();
+//
+//		// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
+//
+//		if(dist < combinedRadius)
+//		{
+//			collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
+//			float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
+//				- ag_damping * (dist - previous_dist) / sim_->timeStep_;
+//
+//			forces += force_value * u_other;
+//		}
+//	}
+//
+//	//------- calcul de la trainee --------------------------------------
+//
+//	forces -= ag_ambient_damping * velocity_;
+//
+//	//------- calcul de la nouvelle valeur de la vitesse ----------------
+//
+//	VEC3 velocity_tmp;
+//
+//	velocity_tmp = velocity_ + forces * (sim_->timeStep_ / ag_mass);
+//	if(velocity_tmp.norm2() > maxSpeed_*maxSpeed_)
+//	{
+//		velocity_tmp.normalize();
+//		velocity_tmp *= maxSpeed_;
+//	}
+//	newVelocity_ = velocity_tmp;
+//}
+
 /* Search for the best new velocity. */
 void Agent::computeNewVelocity()  // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
 {
@@ -779,7 +905,7 @@ bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo,
 unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius,
 	const VEC3& optVelocity, bool directionOpt, VEC3& result)
 {
-	/* 
+	/*
 	 * Optimize direction. Note that the optimization velocity is of unit
 	 * length in this case.
 	 */

src/articulated_obstacle.cpp

+#include "articulated_obstacle.h"
+
+ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int size , std::vector<VEC3> goals)
+{
+	this->index=index;
+	nbBodyPart = size;
+	MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1 ,pos[0],goals,true,this,0);
+	members.push_back(mo4);
+	for(int i =1; i<nbBodyPart; i++)
+	{
+		std::vector<VEC3> goal;
+		goal.push_back(members[i-1]->center);
+		MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal,true, this,i);
+		members.push_back(mo4);
+
+	}
+}
+
+PFP::VEC3 get_center (ArticulatedObstacle * art, int index)
+{
+	return art->members[index]->center + (art->members[index]->center -art->members[index]->front);
+}

src/moving_obstacle.cpp

@@ -70,10 +70,12 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
 	return pos2;
 }
 
-MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin) :
+MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
 		index(ind),
 		newVelocity_(0),
-		sim_(sim)
+		sim_(sim),
+		parent(art),
+		index_parent(ind2)
 {
 	assert(pos.size() > 2);
 
@@ -102,7 +104,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 	}
 	center = sum / nbVertices;
 	front=(vertices[1] + vertices[2]) / 2;
-	if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
+	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
 	{
 		angle = get_angle(goals_[curGoal_] - center,front  - center);
 		for (unsigned int i = 0; i < nbVertices; ++i)
@@ -327,7 +329,7 @@ void MovingObstacle::update()
 
 	// MAJ des particules
 	float abs_angle= angle > 0 ? 1 : -1;
-	float rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
+	float rotor = abs_angle*angle > 0.03f ? 0.03f : abs_angle*angle ;
 //	CGoGNout << "Obstacle "<< index << CGoGNendl;
 //	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
 	//	on fait tourner l'obstacle
@@ -459,20 +461,35 @@ void displayMO(Obstacle * o)
 // TODO Check position
 void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
 {
-	VEC3 goalVector = goals_[curGoal_] - center ;
-	float goalDist2 = goalVector.norm2() ;
-
-	if (goalDist2 < 2.0f)
+	VEC3 goalVector;
+	if (index_parent<1)
 	{
-		curGoal_ = (curGoal_ + 1) % goals_.size() ;
 		goalVector = goals_[curGoal_] - center ;
-		goalDist2 = goalVector.norm2() ;
-	}
 
-	if (goalDist2 > maxSpeed_)
+		float goalDist2 = goalVector.norm2() ;
+
+		if (goalDist2 < 5.0f)
+		{
+			curGoal_ = (curGoal_ + 1) % goals_.size() ;
+			goalVector = goals_[curGoal_] - center ;
+			goalDist2 = goalVector.norm2() ;
+		}
+
+		if (goalDist2 > maxSpeed_)
+		{
+			goalVector.normalize() ;
+			goalVector *= maxSpeed_;
+		}
+	}
+	else
 	{
-		goalVector.normalize() ;
-		goalVector *= maxSpeed_;
+		goalVector = get_center(parent,index_parent-1) -center;
+		float goalDist2 = goalVector.norm2() ;
+		if (goalDist2 > maxSpeed_)
+		{
+			goalVector.normalize() ;
+			goalVector *= maxSpeed_;
+		}
 	}
 	if (spinning) angle =get_angle(goalVector,front-center);
 	prefVelocity_ = goalVector ;

src/simulator.cpp

@@ -18,7 +18,7 @@ Simulator::Simulator(int minS) :
 	detect_agent_collision=false;
 	srand(10) ;
 	nbStepsPerUnit_ = 1 / timeStep_ ;
-	config=0;
+	config=2;
 	init( minSize, 2.0f) ;
 }
 
@@ -45,9 +45,9 @@ void Simulator::init( float dimension, bool enablePathFinding)
 		case 1 :
 			setupCorridorScenario(1000,40) ;
 			break ;
-//		case 2 :
-//			setupScenario(1000) ;
-//			break ;
+		case 2 :
+			setupSnakeCorridorScenario(1000,5,10) ;
+			break ;
 //		case 3 :
 //			setupCityScenario(20, 20) ;
 ////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
@@ -429,6 +429,131 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
 	}
 }
 
+void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
+{
+	if (multires)
+	{
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
+	}
+	else
+	{
+
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
+	}
+
+
+	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;
+
+	// Bordure à éviter autour de la scène (10% de sa taille)
+	int xBorder = envMap_.geometry.size(0) / 5 ;
+	int yBorder = envMap_.geometry.size(1) / 5 ;
+
+	// Les coordonnées sont comprises entre xMin et xMin+xDelta
+
+	// Départ des agents du quart gauche sur toute la hauteur
+	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
+	int xStartDelta = envMap_.geometry.size(0) / 5 ;
+	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
+	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;
+
+	// Arrivée des agents à l'opposée
+	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
+	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
+	int yGoalMin = yStartMin ;
+	int yGoalDelta = yStartDelta ;
+
+	for (unsigned int i = 0 ; i < nbAgents ; ++i)
+	{
+		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
+		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
+
+		// Un agent sur 2 va de droite à gauche
+		VEC3 tmp ;
+		if (i % 2 == 1)
+		{
+			tmp = goal ;
+			goal = start ;
+			start = tmp ;
+		}
+		std::vector<VEC3> goals;
+		goals.push_back(start);
+		goals.push_back(goal);
+		addAgent(start, goals) ;
+	}
+
+	// Départ des obstacles du quart haut sur toute une demi-largeur
+	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
+	xStartDelta = envMap_.geometry.size(0) / 2 ;
+	yStartMin = envMap_.geometry.min()[1] + yBorder ;
+	yStartDelta = envMap_.geometry.size(1) / 20 ;
+
+	// Arrivée des obstacles à l'opposée
+	yGoalDelta = envMap_.geometry.size(1) / 5 ;
+	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;
+
+	VEC3 xSide (5.0f,0.0f,0.0f);
+	VEC3 ySide (0.0f,10.0f,0.0f);
+
+
+	int sumObstacles=0;
+	for(unsigned int j = 0; j<nbSnakes; j++)
+	{
+		std::vector<PFP::VEC3> positions [snakeSize] ;
+		float x = xStartMin + rand() % xStartDelta;
+		VEC3 goal;
+
+		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
+
+
+		std::vector<VEC3> vPos;
+
+
+		vPos.push_back(start+xSide-ySide);
+		vPos.push_back(start+xSide+ySide);
+		vPos.push_back(start-xSide+ySide);
+		vPos.push_back(start-xSide-ySide);
+
+		std::vector<VEC3> goals;
+		goals.push_back(start);
+		int r=0;
+		while (r<40)
+		{
+			x = xStartMin + rand() % xStartDelta;
+			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
+			if ((goal-goals[r]).norm2()>1000){
+				goals.push_back(goal);
+							r++;
+			}
+
+		}
+
+		positions[0]=vPos;
+
+		for (int i = 1 ; i < snakeSize ; i++)
+
+		{
+
+			start=start-ySide-ySide;
+
+			vPos.clear();
+
+
+			vPos.push_back(start+xSide-ySide);
+			vPos.push_back(start+xSide+ySide);
+			vPos.push_back(start-xSide+ySide);
+			vPos.push_back(start-xSide-ySide);
+			positions[i]=vPos;
+		}
+//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
+		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
+		sumObstacles += snakeSize;
+		for (int i = 0 ; i < snakeSize ; i++)
+		{
+			movingObstacles_.push_back(art->members[i]);
+		}
+	}
+}
+
 void Simulator::setupCityScenario(int nbLines, int nbRank)
 {
 	std::cout << " - Setup City Scenario : " << nbLines << " x " << nbRank << std::endl ;

src/viewer.cpp

@@ -249,15 +249,18 @@ void SocialAgents::cb_redraw()
 				glVertex3fv(p.data()) ;
 			}
 			glEnd() ;
-//////Affiche l'objectif actuel
-//		glColor3f(1.0f, 0.0f, 0.0f) ;
-//		glLineWidth(3.0f) ;
-//		glBegin(GL_LINES);
-//		const VEC3& p = (*it)->front ;
-//		glVertex3fv(p.data());
-//		const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
-//		glVertex3fv(p2.data());
-//		glEnd();
+//////Affiche l'objectif actuel des tetes articulées
+//		if((*it)->index_parent==0)
+//		{
+//			glColor3f(1.0f, 0.0f, 0.0f) ;
+//			glLineWidth(3.0f) ;
+//			glBegin(GL_LINES);
+//			const VEC3& p = (*it)->front ;
+//			glVertex3fv(p.data());
+//			const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
+//			glVertex3fv(p2.data());
+//			glEnd();
+//		}
 
 //////Affiche la direction
 //		glColor3f(0.0f, 1.0f, 1.0f) ;