Skip to content

GitLab

Commit 9b8d3a29 authored by pitiot's avatar pitiot
Browse files

ajout obstacles articulés + evitement arash

parent 6eea98b7
Hide whitespace changes
Inline Side-by-side
Debug/CMakeLists.txt
View file @ 9b8d3a29
......@@ -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
View file @ 9b8d3a29
......@@ -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 0 → 100644
View file @ 9b8d3a29
#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
View file @ 9b8d3a29
......@@ -28,6 +28,7 @@ using namespace CGoGN ;
class Agent;
class Obstacle;
class MovingObstacle;
class ArticulatedObstacle;
struct PFP : public PFP_STANDARD
{
......
include/moving_obstacle.h
View file @ 9b8d3a29
......@@ -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
View file @ 9b8d3a29
......@@ -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
View file @ 9b8d3a29
......@@ -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 0 → 100644
View file @ 9b8d3a29
#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
View file @ 9b8d3a29
......@@ -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
View file @ 9b8d3a29
......@@ -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
View file @ 9b8d3a29
......@@ -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) ;
......
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