Commit 38eb3098 authored by pitiot's avatar pitiot
Browse files

merging

parent 3797df7a
......@@ -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
......
......@@ -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
......
......@@ -55,7 +55,12 @@ public:
VEC3 finalGoal ;
Dart finalDart ;
MovingObstacle **movingObstacles_;
int nb_mos;
static unsigned int maxNeighbors_ ;
static unsigned int maxMovingObstacles_;
static float averageMaxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
......
......@@ -15,7 +15,7 @@ bool notDiagonalAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, CellMarker<
template <typename PFP> Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap, unsigned int nbBuildings) ;
template <typename PFP>
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
......
......@@ -63,9 +63,9 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
}
template <typename PFP>
void generateCity(EnvMap& envMap)
void generateCity(EnvMap& envMap, unsigned int nbBuildings)
{
unsigned int nbBuilding = 1000 ;
unsigned int nbBuilding = nbBuildings ;
std::cout << " - Generate City : " << nbBuilding << " buildings" << std::endl ;
generateGrid<PFP>(envMap) ;
......@@ -76,7 +76,7 @@ void generateCity(EnvMap& envMap)
if (!envMap.buildingMark.isMarked(d) && (rand() % 12 == 0)
&& notDiagonalAdjacentToAnObstacle<PFP>(envMap.map, d, envMap.buildingMark))
{
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 2.0f, rand() % 4) ;
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
--nbBuilding ;
}
}
......
......@@ -28,6 +28,7 @@ using namespace CGoGN ;
class Agent;
class Obstacle;
class MovingObstacle;
class ArticulatedObstacle;
#include "pfp.h"
......@@ -402,6 +403,7 @@ inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
inline void EnvMap::pushObstNeighborInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) == neighborObstvect[d].end());
addElementToVector<Obstacle*>(neighborObstvect[d],o);
}
......
......@@ -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() ;
......@@ -44,6 +44,8 @@ public:
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
VEC3 front;
VEC3 focus1, focus2;
double length, width, sum_dist_foci, sum_dist_foci_rest;
VEC3 finalGoal;
float angle;
......@@ -61,11 +63,17 @@ public:
float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
float color1;
float color2;
float color3;
bool seen;
VEC3 velocity_;
VEC3 newVelocity_;
VEC3 prefVelocity_;
Simulator* sim_;
bool spinning;
ArticulatedObstacle * parent;
int index_parent;
};
#endif
......@@ -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"
......@@ -84,7 +84,7 @@ public:
~Simulator() ;
void init(unsigned int config, float dimension, bool enablePathFinding = false) ;
void init(float dimension, bool enablePathFinding = false) ;
void doStep() ;
......@@ -92,7 +92,7 @@ public:
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridor2Scenario(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) ;
......@@ -121,6 +121,7 @@ public:
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
int minSize;
unsigned int config;
float timeStep_ ;
float globalTime_ ;
unsigned int nbSteps_ ;
......
......@@ -129,6 +129,13 @@
<item>
<widget class="QSpinBox" name="check_slide"/>
</item>
<item>
<widget class="QCheckBox" name="check_elipse">
<property name="text">
<string>draw elipse</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
......
......@@ -103,6 +103,7 @@ public:
bool drawObstPredictionTri ;
bool drawObstPath ;
bool draw_dart;
bool draw_elipse;
unsigned int dartSlider;
public slots:
......@@ -182,4 +183,9 @@ public slots:
dartSlider = i;
updateGL();
}
void slot_elipse(bool b)
{
draw_elipse = b;
updateGL();
}
} ;
/*=====================================================================*\
Friday December 7th 2012
Arash HABIBI
agent.cpp
New behaviour algorithm based on a dynamic model
\*=====================================================================*/
#include "agent.h"
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 5;
float Agent::averageMaxSpeed_ = 2.0f ;
float Agent::neighborDist_ = 10.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = (timeHorizonObst_ * averageMaxSpeed_ + radius_) ;
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
......@@ -64,9 +73,14 @@ void Agent::init(const VEC3& start, const VEC3& goal)
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacles_ = new MovingObstacle* [maxMovingObstacles_];
nb_mos=0;
agentNo = cptAgent++ ;
}
//-----------------------------------------------------------------
VEC3 Agent::getPosition()
{
#ifdef SPATIAL_HASHING
......@@ -76,16 +90,23 @@ VEC3 Agent::getPosition()
#endif
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
bool agentSort(const std::pair<float, Agent*>& a1, const std::pair<float, Agent*>& a2)
{
return a1.first < a2.first ;
}
//-----------------------------------------------------------------
bool obstacleSort(const std::pair<float, Obstacle*>& o1, const std::pair<float, Obstacle*>& o2)
{
return o1.first < o2.first ;
}
//-----------------------------------------------------------------
void Agent::updateAgentNeighbors()
{
agentNeighbors_.clear() ;
......@@ -144,6 +165,9 @@ void Agent::updateAgentNeighbors()
}
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
void Agent::updateObstacleNeighbors()
{
obstacleNeighbors_.clear() ;
......@@ -167,43 +191,119 @@ void Agent::updateObstacleNeighbors()
#else
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
float maxDistObst = 0.0f ;
float maxDistMovingObst = 0.0f ;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if (distSq < rangeSq_)
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if ((*it)->mo==NULL)
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
}
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if(distSq < rangeSq_)
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if ((*it)->mo==NULL)
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
if (obstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
}
if (movingObstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(movingObstacleNeighbors_.begin(), movingObstacleNeighbors_.end(), obstacleSort) ;
}
#endif
// if (obstacleNeighbors_.size() > maxNeighbors_)
// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
// CGoGNout<<"obstacles perçus : "<<CGoGNendl;
// for (std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
// it != movingObstacleNeighbors_.end() ;
// ++it)
// {
// (*it).second->mo->color1=0.0f;
// (*it).second->mo->color2=0.0f;
// (*it).second->mo->seen=true;
// CGoGNout<<"obstacle : "<< (*it).second->mo->index<<CGoGNendl;
//
// }
// for (int it= 0; it < nb_mos; it ++)
// {
// movingObstacles_[it]->color1=0.0f;
// movingObstacles_[it]->color2=0.0f;
// movingObstacles_[it]->seen =true;
// }
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
void Agent::update()
{
if (alive)
......@@ -227,6 +327,9 @@ void Agent::update()
}
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
void Agent::computeNewVelocity2()
{
VEC3 centroid ;
......@@ -281,7 +384,18 @@ void Agent::computeNewVelocity2()
//////comportement à modifier///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'obstacles par les agents (calcul effectué avant de lancer RVO2 pour garantir la non-collision entre agents)
//-----------------------------------------------------------------
// fonction d'évitement d'obstacles par les agents (calcul effectué
// avant de lancer RVO2 pour garantir la non-collision entre agents)
// En gros, on représente un obstacle :
// soit par un ensemble de segments orientés dans le sens direct (sim_->avoidance==0),
// soit par son centre (sim_->avoidance==1)
// Cette fonction parcourt les obstacles et calcule la norme de la force globale de
// répulsion. Ensuite, elle ajoute cette somme à l'objectif de façon
// à ce que l'agent change sa trajectoire.
void Agent::obstacle_priority(PFP::VEC3 * goalVector)
{
float factor ;
Obstacle* obst ;
......@@ -289,18 +403,32 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
PFP::VEC3 norm,sumNorm ;
sumNorm.zero() ;
// On parcourt les obstacles
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
factor =(1.0f-((*it).first/rangeSq_)) ;
// rangeSq = range * range (portee au carre)
// les elements de l'iteration sont des obstacles.
// chaque obstacle est une paire : float et *obstacle.
// le float est la distance à l'obstacle et *obstacle
// est un pointeur vers le segment.
factor =(1.0f-(it->first/rangeSq_)) ;
// En particulier ici, (*it).first (ou it->first)
// represente la distance à l'agent (?)
// factor est quelque chose qui faut 0
// quand l'agent commence à interagir avec l'obstacle
// et qui faut 1 quand l'agent se trouve colle sur l'obstacle.
// CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
obst=(*it).second ;
obst=it->second ;
x=obst->p1 ;
y=obst->p2 ;
vec=y-x ;
vec.normalize() ;
norm.zero() ;
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
norm[0]=vec[1] ;
......@@ -309,7 +437,7 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->getPosition()-mo->center;
norm = this->part_.getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
......@@ -318,6 +446,7 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
// }
norm.normalize() ;
norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
// On met factor à la puissance 8 pour adoucir l'impact.
sumNorm+=norm ;
}
......@@ -329,548 +458,754 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
//(*goalVector).normalize();
}
void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'objectif (sans prendre en compte l'environnement)
//-----------------------------------------------------------------
// Calcul vitesse optimale pour atteindre l'objectif (sans prendre
// en compte l'environnement).
void Agent::computePrefVelocity()
{
prefVelocity_ = goals_[curGoal_] - getPosition() ;
float goalDist2 = prefVelocity_.norm2() ;
VEC3 goalVector = goals_[curGoal_] - getPosition() ;
float goalDist2 = goalVector.norm2() ;
// Si l'agent arrive à proximité de l'objectif,
// alors il passe à l'objectif suivant
if (goalDist2 < radius_*radius_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
prefVelocity_ = goals_[curGoal_] - getPosition() ;
goalDist2 = prefVelocity_.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
prefVelocity_.normalize() ;
prefVelocity_ *= maxSpeed_;
goalVector = goals_[curGoal_] - getPosition() ;
goalDist2 = goalVector.norm2() ;
}
}
/* Search for the best new velocity. */
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
{
obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles
std::vector<Line> orcaLines_ ;
orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ;
float invTimeHorizonObst = 1.0f / timeHorizonObst_ ;
/* Create obstacle ORCA lines. */
for (std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ; ++it)
{
const Obstacle* obst = it->second ;
const VEC3 relativePosition1(obst->p1 - getPosition()) ;
const VEC3 relativePosition2(obst->p2 - getPosition()) ;
/*
* Check if velocity obstacle of obstacle is already taken care of by
* previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false ;
// goadDist2 est le vecteur reliant l'agent à l'objectif.
// Tant que ce vecteur a une norme supérieure à maxSpeed_
// (i.e. tant qu'il est encore assez loin de l'objectif)
// la vitesse sera de maxSpeed_. Sinon, s'il est plus
// proche, la vitesse diminue.
for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j)
{
if (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point,
orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON
&& det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point,
orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON)
if (goalDist2 > maxSpeed_)
{
alreadyCovered = true ;
break ;
}
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
if (alreadyCovered)
continue ;
/* Not yet covered. Check for collisions. */
const float distSq1 = relativePosition1.norm2() ;
const float distSq2 = relativePosition2.norm2() ;
prefVelocity_ = goalVector ;
const float radiusSq = radius_ * radius_ ;
const VEC3 obstacleVector(obst->p2 - obst->p1) ;
const VEC3 nextObstacleVector(obst->nextP - obst->p2) ;
const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ;
}
const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ;
const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ;
//-----------------------------------------------------------------
Line line ;
static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, MovingObstacle *mo)
{
unsigned char already_processed=false;
int mo_count;
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
// if (obstacle1->isConvex_)
// {
line.point = VEC3(0) ;
line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
// }
continue ;
}
else if (s > 1 && distSq2 <= radiusSq)
if(nb_mos>=max)
{
/* Collision with right vertex. Ignore if non-convex
* or if it will be taken care of by neighboring obstacle */
// VEC3 nextObstacleVectorN(nextObstacleVector);
// nextObstacleVectorN.normalize();
// if (/*obstacle2->isConvex_ && */det2D(relativePosition2, nextObstacleVectorN/*obstacle2->unitDir_*/) >= 0)
// {
line.point = VEC3(0) ;
line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
// }
continue ;
// cerr << "Agent::moPush : the maximum number of moving obstacles is assumed to be " << max << endl;
// cerr << "There is visibly a need for more." << endl;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = VEC3(0) ;
line.direction = VEC3(-1.0f * obstacleVector) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
continue ;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
VEC3 leftLegDirection, rightLegDirection ;
bool obst1EQobst2 = false ;