Commit 9b8d3a29 authored by pitiot's avatar pitiot

ajout obstacles articulés + evitement arash

parent 6eea98b7
...@@ -25,6 +25,7 @@ add_executable( socialAgentsD ...@@ -25,6 +25,7 @@ add_executable( socialAgentsD
../src/env_map.cpp ../src/env_map.cpp
../src/agent.cpp ../src/agent.cpp
../src/moving_obstacle.cpp ../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp ../src/simulator.cpp
../src/moving_mesh.cpp ../src/moving_mesh.cpp
../src/gl2ps.c ../src/gl2ps.c
......
...@@ -23,6 +23,7 @@ add_executable( socialAgents ...@@ -23,6 +23,7 @@ add_executable( socialAgents
../src/env_map.cpp ../src/env_map.cpp
../src/agent.cpp ../src/agent.cpp
../src/moving_obstacle.cpp ../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp ../src/simulator.cpp
../src/moving_mesh.cpp ../src/moving_mesh.cpp
../src/gl2ps.c ../src/gl2ps.c
......
#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
...@@ -28,6 +28,7 @@ using namespace CGoGN ; ...@@ -28,6 +28,7 @@ using namespace CGoGN ;
class Agent; class Agent;
class Obstacle; class Obstacle;
class MovingObstacle; class MovingObstacle;
class ArticulatedObstacle;
struct PFP : public PFP_STANDARD struct PFP : public PFP_STANDARD
{ {
......
...@@ -10,13 +10,13 @@ ...@@ -10,13 +10,13 @@
using namespace std; using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle); PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2); float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ; class Simulator ;
class MovingObstacle class MovingObstacle
{ {
public: 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); bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment(); // void contournerBatiment();
void updateAgentNeighbors() ; void updateAgentNeighbors() ;
...@@ -66,6 +66,8 @@ public: ...@@ -66,6 +66,8 @@ public:
VEC3 prefVelocity_; VEC3 prefVelocity_;
Simulator* sim_; Simulator* sim_;
bool spinning; bool spinning;
ArticulatedObstacle * parent;
int index_parent;
}; };
#endif #endif
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include "env_map.h" #include "env_map.h"
#include "agent.h" #include "agent.h"
#include "obstacle.h" #include "obstacle.h"
#include "moving_obstacle.h" #include "articulated_obstacle.h"
#include "moving_mesh.h" #include "moving_mesh.h"
#include "path_finder.h" #include "path_finder.h"
...@@ -98,6 +98,7 @@ public: ...@@ -98,6 +98,7 @@ public:
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ; void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(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 setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ; void setupScenario(unsigned int nbMaxAgent) ;
......
...@@ -338,6 +338,132 @@ void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'ob ...@@ -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. */ /* Search for the best new velocity. */
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles 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, ...@@ -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, unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius,
const VEC3& optVelocity, bool directionOpt, VEC3& result) const VEC3& optVelocity, bool directionOpt, VEC3& result)
{ {
/* /*
* Optimize direction. Note that the optimization velocity is of unit * Optimize direction. Note that the optimization velocity is of unit
* length in this case. * length in this case.
*/ */
......
#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);
}
...@@ -70,10 +70,12 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece ...@@ -70,10 +70,12 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return pos2; 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), index(ind),
newVelocity_(0), newVelocity_(0),
sim_(sim) sim_(sim),
parent(art),
index_parent(ind2)
{ {
assert(pos.size() > 2); assert(pos.size() > 2);
...@@ -102,7 +104,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -102,7 +104,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
} }
center = sum / nbVertices; center = sum / nbVertices;
front=(vertices[1] + vertices[2]) / 2; 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); angle = get_angle(goals_[curGoal_] - center,front - center);
for (unsigned int i = 0; i < nbVertices; ++i) for (unsigned int i = 0; i < nbVertices; ++i)
...@@ -327,7 +329,7 @@ void MovingObstacle::update() ...@@ -327,7 +329,7 @@ void MovingObstacle::update()
// MAJ des particules // MAJ des particules
float abs_angle= angle > 0 ? 1 : -1; 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 << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl; // CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle // on fait tourner l'obstacle
...@@ -459,20 +461,35 @@ void displayMO(Obstacle * o) ...@@ -459,20 +461,35 @@ void displayMO(Obstacle * o)
// TODO Check position // TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ? void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{ {
VEC3 goalVector = goals_[curGoal_] - center ; VEC3 goalVector;
float goalDist2 = goalVector.norm2() ; if (index_parent<1)
if (goalDist2 < 2.0f)
{ {
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ; 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 = get_center(parent,index_parent-1) -center;
goalVector *= maxSpeed_; float goalDist2 = goalVector.norm2() ;
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
} }
if (spinning) angle =get_angle(goalVector,front-center); if (spinning) angle =get_angle(goalVector,front-center);
prefVelocity_ = goalVector ; prefVelocity_ = goalVector ;
......
...@@ -18,7 +18,7 @@ Simulator::Simulator(int minS) : ...@@ -18,7 +18,7 @@ Simulator::Simulator(int minS) :
detect_agent_collision=false; detect_agent_collision=false;
srand(10) ; srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ; nbStepsPerUnit_ = 1 / timeStep_ ;
config=0; config=2;
init( minSize, 2.0f) ; init( minSize, 2.0f) ;
} }
...@@ -45,9 +45,9 @@ void Simulator::init( float dimension, bool enablePathFinding) ...@@ -45,9 +45,9 @@ void Simulator::init( float dimension, bool enablePathFinding)
case 1 : case 1 :
setupCorridorScenario(1000,40) ; setupCorridorScenario(1000,40) ;
break ; break ;
// case 2 : case 2 :
// setupScenario(1000) ; setupSnakeCorridorScenario(1000,5,10) ;
// break ; break ;
// case 3 : // case 3 :
// setupCityScenario(20, 20) ; // setupCityScenario(20, 20) ;
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10), //// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
...@@ -429,6 +429,131 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst ...@@ -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) void Simulator::setupCityScenario(int nbLines, int nbRank)
{ {
std::cout << " - Setup City Scenario : " << nbLines << " x " << nbRank << std::endl ; std::cout << " - Setup City Scenario : " << nbLines << " x " << nbRank << std::endl ;
......
...@@ -249,15 +249,18 @@ void SocialAgents::cb_redraw() ...@@ -249,15 +249,18 @@ void SocialAgents::cb_redraw()
glVertex3fv(p.data()) ; glVertex3fv(p.data()) ;
} }
glEnd() ; glEnd() ;
//////Affiche l'objectif actuel //////Affiche l'objectif actuel des tetes articulées
// glColor3f(1.0f, 0.0f, 0.0f) ; // if((*it)->index_parent==0)
// glLineWidth(3.0f) ; // {
// glBegin(GL_LINES); // glColor3f(1.0f, 0.0f, 0.0f) ;
// const VEC3& p = (*it)->front ; // glLineWidth(3.0f) ;
// glVertex3fv(p.data()); // glBegin(GL_LINES);
// const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ; // const VEC3& p = (*it)->front ;
// glVertex3fv(p2.data()); // glVertex3fv(p.data());
// glEnd(); // const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
// glVertex3fv(p2.data());
// glEnd();
// }
//////Affiche la direction //////Affiche la direction
// glColor3f(0.0f, 1.0f, 1.0f) ; // glColor3f(0.0f, 1.0f, 1.0f) ;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment