Skip to content

GitLab

Commit 62820091 authored by pitiot's avatar pitiot
Browse files

ajout corridor

parent 9a25743a
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 765 additions and 1479 deletions
include/env_generator.hpp
View file @ 62820091
......@@ -281,7 +281,7 @@ Dart generateBuilding(EnvMap* envMap, Dart d, float height, unsigned int buildin
Dart next = map.phi1(dd);
Dart previous = map.phi_1(dd);
Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]);
Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)], NULL, 0);
#ifdef SPATIAL_HASHING
VEC3 ov = o->p2 - o->p1 ;
......
include/env_map.h
View file @ 62820091
......@@ -32,8 +32,8 @@ class MovingObstacle;
struct PFP: public PFP_STANDARD
{
// definition de la carte
// typedef EmbeddedMap2 MAP;
typedef Algo::IHM::ImplicitHierarchicalMap MAP;
// typedef EmbeddedMap2 MAP;
typedef Algo::IHM::ImplicitHierarchicalMap MAP;
// definition des listes d'agent
typedef std::vector<Agent*> AGENTS;
......@@ -49,13 +49,27 @@ struct PFP: public PFP_STANDARD
};
typedef PFP::VEC3 VEC3;
typedef PFP::REAL REAL ;
class EnvMap
{
public :
EnvMap();
void markPedWay();
/* Geometry of the EnvMap : the bounding box of the scene
* The density of cells in the EnvMap is given by two parameters :
* - minCellSize : the size under which no subdivision occurs
* - maxCellSize : the size of the initial cells (before subdivisions occur)
* - obstacleDistance : the minimal goal distance between agents and obstacles
* The number of cells in the initial EnvMap is about (width*height)/(size*size)
*/
Geom::BoundingBox<VEC3> geometry ;
REAL maxCellSize ;
REAL minCellSize ;
REAL obstacleDistance ;
void markPedWay() ;
float max_for_obstacles;
float sideSize;
unsigned int nbSquares;
......@@ -69,7 +83,7 @@ public :
void subdivideToProperLevel();
#endif
void init(unsigned int config);
void init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize) ;
#ifndef SPATIAL_HASHING
void foreach_neighborFace(Dart d, FunctorType& f);
......@@ -82,7 +96,7 @@ public :
void pushAgentInCells(Agent* agent, Dart d);
void popAgentInCells(Agent* agent, Dart d);
// void agentChangeFaceThroughEdge(Agent* agent);
// void agentChangeFaceThroughEdge(Agent* agent);
void agentChangeFace(Agent* agent, Dart oldFace);
void pushObstacleInCells(Obstacle* o, Dart d);
......@@ -90,15 +104,15 @@ public :
void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace);
//ajout moving obst
void addObstAsNeighbor (Obstacle * o,std::list<Dart> belonging_cells, std::list<Dart> * nieghbor_cells);
// void addMovingObstAsNeighbor (MovingObstacle * mo,std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells);
void pushObstNeighborInCells(Obstacle* o, Dart d);
void popObstNeighborInCells(Obstacle* o, Dart d);
void find_next(Obstacle* o,Dart * d);
void addObstAsNeighbor (Obstacle * o, const std::list<Dart>& belonging_cells, std::list<Dart> * nieghbor_cells);
// void addMovingObstAsNeighbor (MovingObstacle * mo,std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells);
void pushObstNeighborInCells(Obstacle* o, Dart d);
void popObstNeighborInCells(Obstacle* o, Dart d);
void find_next(Obstacle* o,Dart * d, CellMarkerStore<FACE>& cms);
void clearUpdateCandidates();
void clearUpdateCandidates();
void updateMap();
void resetAgentInFace(Agent* agent);
......@@ -130,8 +144,6 @@ public :
CellMarker<EDGE> obstacleMark;
CellMarker<FACE> buildingMark;
CellMarker<FACE> pedWayMark;
CellMarker<FACE> MovingObstMark;
CellMarker<FACE> OneRingMark;
#ifndef SPATIAL_HASHING
static const unsigned int nbAgentsToSubdivide = 5;
......@@ -177,8 +189,8 @@ void resetPart(MovingObstacle * mo, Dart d);
void displayMO(Obstacle * o);
/**************************************
* INLINE FUNCTIONS *
**************************************/
* INLINE FUNCTIONS *
**************************************/
template <typename T>
inline void removeElementFromVector(std::vector<T>& a, T ag)
......@@ -200,7 +212,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
agentvect[d].push_back(agent);
Dart dd = d;
......@@ -238,181 +250,121 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////
// addobstasneighbor
inline void EnvMap::addObstAsNeighbor (Obstacle * o, std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells)
inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::list<Dart>& belonging_cells, std::list<Dart> *neighbor_cells)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
(*neighbor_cells).clear();
MovingObstMark.unmarkAll();
OneRingMark.unmarkAll();
for(std::list<Dart>::iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
neighbor_cells->clear();
CellMarkerStore<FACE> MovingObstMark(map);
CellMarkerStore<FACE> OneRingMark(map);
for(std::list<Dart>::const_iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
{
MovingObstMark.mark(*it);
}
std::list<Dart>::iterator it=belonging_cells.begin();
std::list<Dart>::const_iterator it=belonging_cells.begin();
Dart beg = NIL;
Dart first =NIL;
Dart d=NIL;
Dart dd=NIL;
int rd1=0,rd2=0,rd3=0;
// CGoGNout << "debut"<<CGoGNendl;
//boucle pour trouver la premiere case
do {
rd3++;
if (rd3>10000) {
CGoGNout<< "infini dans addneighbor obst rd3"<<CGoGNendl;break;
}
// CGoGNout << "debut"<<CGoGNendl;
//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
do
{
beg = *it;
first = NIL;
d=beg;
// CGoGNout << "d = "<<d<<CGoGNendl;
do{
// CGoGNout << "first = "<<first<<CGoGNendl;
if (rd2>10000) {
CGoGNout<< "infini dans addneighbor obst rd2"<<CGoGNendl;break;
}
do {
dd=map.alpha1(map.alpha1(d));
do {
if (rd1>10000) {
CGoGNout<< "infini dans addneighbor obst rd1"<<CGoGNendl;break;
}
if (!MovingObstMark.isMarked(dd))
{
first = dd;
// CGoGNout << "dd1 = "<<first<<CGoGNendl;
}
dd=map.alpha1(dd);
rd1++;
}while (dd!=d && first==NIL);
// CGoGNout << "dd = "<<dd<<CGoGNendl;
}while (first==NIL && dd!=d);
d=map.phi1(d);
rd2++;
// CGoGNout << "d2 = "<<d<<" beg = "<< beg<<CGoGNendl;
}while(first==NIL && d!=beg);
} while(first==NIL && d!=beg);
++it;
}while((it!=belonging_cells.end())&&first==NIL);
// CGoGNout << "debut2 : first : "<<first <<CGoGNendl;
// CGoGNout << "coucou je suis la2 et " << "first = "<< first << " et est ce un obstacle ?"<< MovingObstMark.isMarked(first)<< CGoGNendl;
int round = 0;
}while(first==NIL && it!=belonging_cells.end());
d=first;
// int tour =0;
do{
// CGoGNout << "d = "<< d <<CGoGNendl;
do
{
if (!OneRingMark.isMarked(d))
{
{
OneRingMark.mark(d);
(*neighbor_cells).push_back(d);
// CGoGNout << "Dart marquée : "<< d<< CGoGNendl;
}
find_next(o,&d);
round ++;
// CGoGNout << "Dart suivante : "<< d<< CGoGNendl;
// tour++;
if(round>10000) {
CGoGNout << "infini calcul voisins"<<CGoGNendl;
break;
}
}while(!map.sameFace(d,first));
// CGoGNout << "fin"<<CGoGNendl;
// Dart dd = belonging_cells[0];
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// removeElementFromVector<Agent* >(neighborAgentvect[ddd], agent);
// ddd = map.alpha1(ddd);
// }
// dd = map.phi1(dd);
// } while(dd != d);
//
find_next(o,&d, MovingObstMark);
}while(!map.sameFace(d,first));
}
// find_next cherche la prochaine case "voisine" d'un obstacle faisant parti d'un movingobstacle (algo de parcours du one-ring )
inline void EnvMap::find_next(Obstacle* o,Dart * ddd)
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<FACE>& cms)
{
Dart d=*ddd;
Dart first = NIL;
Dart dd;
int rd =0;
do{
rd++;
dd=map.alpha1((d));
if (!(MovingObstMark.isMarked(dd)))
{
first = dd;
}
rd++;
dd=map.alpha1((d));
if (!(cms.isMarked(dd)))
{
first = dd;
}
d=map.phi_1(d);
if (rd>10000) {
displayMO(o);
CGoGNout <<(first==NIL)<< " infini find next : "<< *ddd<<CGoGNendl;
break;
}
}while(first==NIL);
d=map.phi_1(d);
if (rd>10000) {
displayMO(o);
CGoGNout <<(first==NIL)<< " infini find next : "<< *ddd<<CGoGNendl;
break;
}
}while(first==NIL);
*ddd=first;
}
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());
neighborObstvect[d].push_back(o);
neighborObstvect[d].push_back(o);
}
inline void EnvMap::popObstNeighborInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) != neighborObstvect[d].end());
removeElementFromVector<Obstacle* >(neighborObstvect[d], o);
removeElementFromVector<Obstacle* >(neighborObstvect[d], o);
}
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) == obstvect[d].end());
obstvect[d].push_back(o);
}
inline void EnvMap::popObstacleInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) != obstvect[d].end());
removeElementFromVector<Obstacle* >(obstvect[d], o);
// removeElementFromVector<Obstacle* >(obstvect[map.phi<12>(d)],o);
// removeElementFromVector<Obstacle* >(obstvect[map.phi2(map.phi_1(d))],o);
// Dart dd = d;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// removeElementFromVector<Obstacle* >(obstvect[ddd], o);
// ddd = map.alpha1(ddd);
// }
// dd = map.phi1(dd);
// } while(dd != d);
}
inline void EnvMap::clearUpdateCandidates()
......@@ -422,83 +374,6 @@ inline void EnvMap::clearUpdateCandidates()
}
///addMovingObstAsNeighbor est pour détecter les voisins du movingobstacle global afin de le considerer comme un super agent
//inline void EnvMap::addMovingObstAsNeighbor (MovingObstacle * mo,std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells)
//{
// assert(map.getCurrentLevel() == map.getMaxLevel());
// (*neighbor_cells).clear();
// MovingObstMark.unmarkAll();
// OneRingMark.unmarkAll();
//
// for(std::list<Dart>::iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
//
// {
// MovingObstMark.mark(*it);
// }
//
//
// std::list<Dart>::iterator it=belonging_cells.begin();
// Dart beg = NIL;
// Dart first =NIL;
// Dart d=NIL;
// Dart dd=NIL;
// //boucle pour trouver la premiere case
// do {
// beg = *it;
// first = NIL;
// d=beg;
//
// do{
//
// dd=map.alpha1(map.alpha1(d));
// do {
//
// if (!MovingObstMark.isMarked(dd))
// {
// first = dd;
// }
// dd=map.alpha1(dd);
// }while (dd!=d && first==NIL);
// d=map.phi1(d);
// }while(first==NIL);
// ++it;
// }while((it!=belonging_cells.end())&&first==NIL);
//
//// CGoGNout << "coucou je suis la2 et " << "first = "<< first << " et est ce un obstacle ?"<< MovingObstMark.isMarked(first)<< CGoGNendl;
//
// d=first;
//// int tour =0;
// do{
// if (!OneRingMark.isMarked(d))
// {
// OneRingMark.mark(d);
// (*neighbor_cells).push_back(d);
//// CGoGNout << "Dart marquée : "<< d<< CGoGNendl;
//
// }
//
// find_next(mo->obstacles_[0],&d);
//// CGoGNout << "Dart suivante : "<< d<< CGoGNendl;
//// tour++;
//// if(tour==20) break;
// }while(!map.sameFace(d,first));
//
//
//// Dart dd = belonging_cells[0];
//// do
//// {
//// Dart ddd = map.alpha1(map.alpha1(dd));
//// while(ddd != dd)
//// {
//// removeElementFromVector<Agent* >(neighborAgentvect[ddd], agent);
//// ddd = map.alpha1(ddd);
//// }
//// dd = map.phi1(dd);
//// } while(dd != d);
////
//
//}
#endif
#endif
include/moving_obstacle.h
View file @ 62820091
......@@ -10,41 +10,20 @@
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
void unify (std::list<Dart> * list, EnvMap* envmap);
class MovingObstacle
{
public:
MovingObstacle(int index,EnvMap* envmap, std::vector<PFP::VEC3> pos, VEC3 goal,float rota);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
void updateFixedObstNeighbors();
void updateMovingObstNeighbors();
bool is_inside (VEC3 p);
void computePrefVelocity();
void computeNewVelocity();
bool linearProgram1(
const std::vector<Line>& lines, unsigned int lineNo,
float radius, const VEC3& optVelocity,
bool directionOpt, VEC3& result
);
unsigned int linearProgram2(
const std::vector<Line>& lines, float radius,
const VEC3& optVelocity, bool directionOpt,
VEC3& result
);
void linearProgram3(
const std::vector<Line>& lines, unsigned int numObstLines,
unsigned int beginLine, float radius,
VEC3& result
);
void update();
void register_ (Obstacle* o,Dart d, int n );
void register_oneRingAdd(Obstacle * o, Dart d, int n);
void register_update(Obstacle* o, Dart d, int n);
// void difference_list (std::list<Dart> old_, std::list<Dart> new_);
......@@ -61,12 +40,11 @@ public:
std::list<Dart> * belonging_cells;
std::list<Dart> * neighbor_cells;
std::list<Dart> general_belonging;
std::list<Dart> general_neighbors;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
std::vector<std::pair<float, MovingObstacle*> > MovingObstNeighbors_;
std::vector<std::pair<float, MovingObstacle*> > movingObstNeighbors_;
float rot;
VEC3 finalGoal;
......
include/obstacle.h
View file @ 62820091
......@@ -6,23 +6,24 @@
class Obstacle
{
public:
Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), mo(NULL), index(0)
// Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint) :
// p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), mo(NULL), index(0)
// {
// p1[2] = 0 ;
// p2[2] = 0 ;
// prevP[2] = 0 ;
// nextP[2] = 0 ;
//
// }
Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint,MovingObstacle * moving1, int ind) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), mo(moving1),index(ind)
{
p1[2] = 0 ;
p2[2] = 0 ;
prevP[2] = 0 ;
nextP[2] = 0 ;
}
Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint,MovingObstacle * moving1, int ind) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), mo(moving1),index(ind)
{
p1[2] = 0 ;
p2[2] = 0 ;
prevP[2] = 0 ;
nextP[2] = 0 ;
}
VEC3 p1 ;
VEC3 p2 ;
VEC3 prevP ;
......
include/simulator.h
View file @ 62820091
......@@ -16,52 +16,54 @@
class ThreadUpdateInfo
{
public :
std::vector<Agent*> ag_;
unsigned int bMin_;
unsigned int bMax_;
public:
std::vector<Agent*> ag_ ;
unsigned int bMin_ ;
unsigned int bMax_ ;
// Constructor
ThreadUpdateInfo(std::vector<Agent*> agents, unsigned int bMin, unsigned int bMax) :
bMin_(bMin),
bMax_(bMax)
bMin_(bMin), bMax_(bMax)
{
ag_.insert(ag_.end(), agents.begin() + bMin, agents.begin() + bMax);
ag_.insert(ag_.end(), agents.begin() + bMin, agents.begin() + bMax) ;
}
// Destructor
~ThreadUpdateInfo() { }
~ThreadUpdateInfo()
{
}
void run()
{
// Thread execution stuff goes here
// Thread execution stuff goes here
for (unsigned int i = 0; i < ag_.size(); ++i)
{
// ag_[i]->updateObstacleNeighbors();
// ag_[i]->updateAgentNeighbors();
ag_[i]->computePrefVelocity();
ag_[i]->computeNewVelocity();
ag_[i]->computePrefVelocity() ;
ag_[i]->computeNewVelocity() ;
}
}
};
} ;
class ThreadUpdatePos
{
public :
std::vector<Agent*> ag_;
unsigned int bMin_;
unsigned int bMax_;
public:
std::vector<Agent*> ag_ ;
unsigned int bMin_ ;
unsigned int bMax_ ;
// Constructor
ThreadUpdatePos(std::vector<Agent*> agents,unsigned int bMin,unsigned int bMax) :
bMin_(bMin),
bMax_(bMax)
ThreadUpdatePos(std::vector<Agent*> agents, unsigned int bMin, unsigned int bMax) :
bMin_(bMin), bMax_(bMax)
{
ag_.insert(ag_.end(),agents.begin()+bMin,agents.begin()+bMax);
ag_.insert(ag_.end(), agents.begin() + bMin, agents.begin() + bMax) ;
}
// Destructor
~ThreadUpdatePos() { }
~ThreadUpdatePos()
{
}
void run() {
// Thread execution stuff goes here
......@@ -75,7 +77,7 @@ public :
class Simulator
{
public:
Simulator();
Simulator(int minSize);
Simulator(
float timeStep, float neighborDist, unsigned int maxNeighbors,
......@@ -85,13 +87,14 @@ public:
~Simulator();
void init(unsigned int config, float dimension, bool enablePathFinding=false);
void init(unsigned int config, int minSize, float dimension, bool enablePathFinding = false) ;
void doStep();
bool reachedGoal();
void setupCircleScenario(unsigned int nbMaxAgent);
void setupCorridorScenario(unsigned int nbMaxAgent,int nb_obst);
void setupCityScenario(float startX, float startY, int nbLines , int nbRank);
void setupScenario(unsigned int nbMaxAgent);