Commit 50940569 authored by Jund Thomas's avatar Jund Thomas
Browse files

deplacement des enregistrements dans envMap pour uniformiser

parents 718e49f7 84006d76
......@@ -336,9 +336,12 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
Dart x =map.phi2(dd);
if(!map.isBoundaryMarked(x))
{
envMap.pushObstacleInCells(o, x);
if(!map.isBoundaryMarked(map.phi2(map.phi1(x))))envMap.pushObstacleInCells(o, map.phi2(map.phi1(x)));
if(!map.isBoundaryMarked(map.phi2(map.phi_1(x))))envMap.pushObstacleInCells(o, map.phi2(map.phi_1(x)));
addElementToVector<Obstacle*>(envMap.obstvect[d],o);
if(!map.isBoundaryMarked(map.phi2(map.phi1(x))))
addElementToVector<Obstacle*>(envMap.obstvect[map.phi2(map.phi1(x))],o);
if(!map.isBoundaryMarked(map.phi2(map.phi_1(x))))
addElementToVector<Obstacle*>(envMap.obstvect[map.phi2(map.phi_1(x))],o);
}
#endif
......
......@@ -100,9 +100,15 @@ public:
void popAgentInCells(Agent* agent, Dart d) ;
void agentChangeFace(Agent* agent, Dart oldFace) ;
void pushObstacleInCells(Obstacle* o, Dart d) ;
void popAndPushObstacleInCells(Obstacle* o, int n);
void pushObstacleInCells(Obstacle * mo);
void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n);
void pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross);
void popObstacleInCells(Obstacle* o, int n);
void popObstacleInCells(Obstacle* o, Dart d) ;
void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) ;
void refine() ;
void coarse() ;
void updateMap() ;
void resetAgentInFace(Agent* agent) ;
......@@ -119,13 +125,13 @@ public:
CellMarker<EDGE> obstacleMark ;
CellMarker<FACE> buildingMark ;
CellMarker<FACE> pedWayMark ;
// CellMarkerMemo<FACE> memo_mark;
// ajout moving obst
void addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& belonging_cells, std::vector<Dart> * nieghbor_cells);
// void addMovingObstAsNeighbor (MovingObstacle * mo,std::vector<Dart> belonging_cells, std::vector<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 find_next(Obstacle* o,Dart * d, CellMarkerMemo<FACE>& cms);
std::vector<Dart> newBuildings ;
......@@ -142,8 +148,8 @@ public:
#ifndef SPATIAL_HASHING
static const unsigned int nbAgentsToSubdivide = 10 ;
static const unsigned int nbAgentsToSimplify = 4 ;
static const unsigned int nbAgentsToSubdivide = 5 ;
static const unsigned int nbAgentsToSimplify = 1 ;
CellMarker<FACE> refineMark ;
CellMarker<FACE> coarsenMark ;
......@@ -203,9 +209,18 @@ void displayMO(Obstacle * o);
* INLINE FUNCTIONS *
**************************************/
template <typename T>
inline void addElementToVector(std::vector<T>& a, T ag)
{
assert(std::find(a.begin(), a.end(), ag) == a.end());
a.push_back(ag) ;
}
template <typename T>
inline bool removeElementFromVector(std::vector<T>& a, T ag)
{
assert(std::find(a.begin(), a.end(), ag) != a.end());
typename std::vector<T>::iterator end = a.template end() ;
for (typename std::vector<T>::iterator it = a.begin(); it != end; ++it)
{
......@@ -252,7 +267,8 @@ 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) ;
addElementToVector<Agent*>(agentvect[d],agent);
// agentvect[d].push_back(agent) ;
// nbAgentsIncrease(d);
Dart dd = d ;
......@@ -261,7 +277,8 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Dart ddd = map.alpha1(map.alpha1(dd)) ;
while (ddd != dd)
{
neighborAgentvect[ddd].push_back(agent) ;
addElementToVector<Agent*>(neighborAgentvect[ddd],agent);
// neighborAgentvect[ddd].push_back(agent) ;
// nbAgentsIncrease(ddd);
ddd = map.alpha1(ddd) ;
}
......@@ -293,21 +310,17 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////
inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& belonging_cells, std::vector<Dart> *neighbor_cells)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(!belonging_cells.empty());
neighbor_cells->clear();
CellMarkerMemo<FACE> memo_mark(map);
CellMarkerMemo<FACE> OneRingMark(map);
CellMarkerStore<FACE> MovingObstMark(map);
CellMarkerStore<FACE> OneRingMark(map);
for(std::vector<Dart>::const_iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
{
assert(!buildingMark.isMarked(*it)) ;
MovingObstMark.mark(*it);
}
for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
memo_mark.mark(*it);
std::vector<Dart>::const_iterator it=belonging_cells.begin();
Dart beg = NIL;
......@@ -324,7 +337,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
do {
dd=map.alpha1(map.alpha1(d));
do {
if (!MovingObstMark.isMarked(dd))
if (!memo_mark.isMarked(dd))
{
first = dd;
}
......@@ -348,14 +361,24 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
(*neighbor_cells).push_back(d);
}
find_next(o,&d, MovingObstMark);
find_next(o,&d, memo_mark);
if(d==NIL)
{
CGoGNout<<"cellule de début : "<<first<<CGoGNendl;
CGoGNout<<"cellules markées OneRing : "<<CGoGNendl;
std::vector<Dart> v=OneRingMark.get_markedCells();
for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
{
CGoGNout<<(*it).index<<CGoGNendl;
}
}
// CGoGNout<<"d : "<<d<<CGoGNendl;
}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, CellMarkerStore<FACE>& cms)
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
{
Dart d=*ddd;
Dart first = NIL;
......@@ -370,12 +393,30 @@ inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<FACE>& cms
first = dd;
}
d=map.phi_1(d);
if (rd>10000) {
displayMO(o);
CGoGNout <<(first==NIL)<< " infini find next : "<< *ddd<<CGoGNendl;
break;
if(rd>100)
{
if (rd>10000)
{
displayMO(o);
CGoGNout <<(first==NIL)<< " infini find next d : "<< d<<"phi 2 markée ? : "<<cms.isMarked(map.phi2(d))<<CGoGNendl;
CGoGNout<<"cellules markées : "<<CGoGNendl;
std::vector<Dart> v=cms.get_markedCells();
for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
{
CGoGNout<<(*it).index<<CGoGNendl;
}
break;
}
CGoGNout <<(first==NIL)<< " infini find next d : "<< d<<"phi 2 markée ? : "<<cms.isMarked(map.phi2(d))<<CGoGNendl;
CGoGNout<<"cellules markées : "<<CGoGNendl;
std::vector<Dart> v=cms.get_markedCells();
for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
{
CGoGNout<<(*it).index<<CGoGNendl;
}
}
d=map.phi_1(d);
}while(first==NIL);
*ddd=first;
}
......@@ -383,68 +424,17 @@ inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<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());
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);
addElementToVector<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());
assert(!buildingMark.isMarked(d)) ;
obstvect[d].push_back(o) ;
// obstvect[map.phi<12>(d)].push_back(o);
// obstvect[map.phi2(map.phi_1(d))].push_back(o);
// Dart dd = d;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// obstvect[ddd].push_back(o);
// ddd = map.alpha1(ddd);
// }
//inline void EnvMap::pushObstacleInCell(Obstacle* o, Dart d)
//{
// assert(map.getCurrentLevel() == map.getMaxLevel()) ;
// assert(!buildingMark.isMarked(d)) ;
//
// dd = map.phi1(dd);
// } while(dd != d);
}
inline void EnvMap::popObstacleInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
assert(!buildingMark.isMarked(d)) ;
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);
}
// addElementToVector<Obstacle*>(obstvect[d],o);
//}
inline void EnvMap::clearUpdateCandidates()
{
......
......@@ -121,7 +121,7 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
if (showObstacleDist)
{
radius = (agent->timeHorizonObst_ * agent->maxSpeed_) + agent->radius_ ;
radius = (agent->range_);
glColor3f(0.0f, 0.0f, 1.0f) ;
glBegin(GL_LINE_LOOP) ;
for (unsigned int i = 0 ; i < 5 ; ++i)
......
......@@ -5,33 +5,29 @@
#include "utils.h"
#include "env_map.h"
#include <set>
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
void register_add(Obstacle* o, int n, const std::vector<Dart>& memo);
class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, VEC3 goal,float rota);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,float rota);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
void updateFixedObstNeighbors();
// void contournerBatiment();
void updateAgentNeighbors() ;
void updateObstacleNeighbors() ;
bool is_inside (VEC3 p);
void computePrefVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart * d1);
void computeNewVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1);
void update();
void register_oneRingAdd(Obstacle * o, Dart d, int n);
void register_update(Obstacle* o, int n);
// void difference_list (std::list<Dart> old_, std::list<Dart> new_);
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> *registering_part;
std::vector<PFP::VEC3> vertices;
......@@ -42,15 +38,12 @@ public:
int index;
int max_x_ind;
int min_x_ind;
Obstacle* * obstacles_;
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
std::vector<Dart> general_belonging;
std::set<Dart> general_belonging;
VEC3 front;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
std::vector<std::pair<float, MovingObstacle*> > movingObstNeighbors_;
float rot;
VEC3 finalGoal;
......@@ -63,6 +56,8 @@ public:
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
static float detectionFixedObst;
static float neighborDist_;
static float neighborDistSq_;
static float maxSpeed_;
float obstacle_range;
static float timeHorizonObst_;
......
......@@ -15,6 +15,14 @@ template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
Dart startPos, Dart stopPos, CellMarker<FACE>& bad) ;
//computation of a "Rapidly exploring random tree"
//requires the definition of states for the moving objects
template <typename STATE>
std::vector<VEC3> pathRRT(STATE startState, VEC3 stopPos,
float sampleSize, float epsilon,
STATE (*fctStep)(STATE start, VEC3 goal, float sampleSize)
);
}
}
......
......@@ -109,6 +109,78 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC
return path ;
}
template <typename STATE>
std::vector<VEC3> pathRRT(STATE startState, VEC3 stopPos,
float sampleSize, float epsilon,
STATE (*fctStep)(STATE start, VEC3 goal, float sampleSize)
)
{
//NOT TESTED
std::cout << __FILE__ << " " << __LINE__ << " HAS NEVER BEEN TESTED" << std::endl;
std::vector<VEC3> res;
typedef struct Node Node;
struct Node { STATE node; Node& prev; };
std::list<Node> graph;
//init graph with startState
Node n;
n.node = startState;
graph.push_back(n);
Node last=n;
//repeat until path found
while(VEC3(last.node.getPosition()-stopPos).norm2()>epsilon)
{
//pick random position rPos in space (have to be in valid parameter space)
//(add bias to choose goal as random pos occasionally for faster convergence)
// could be biased using voronoi regions size of previous nodes
VEC3 rPos = rand()%2=0 ? stopPos : VEC3(rand()*100.0f,rand()*100.0f,rand()*100.0f);
//get nearest node in graph
std::cout << __FILE__ << " " << __LINE__ << " to improve : make sorted insert !" << std::endl;
float distMin= std::numeric_limits<float>::max();
Node& nProx = *graph.begin();
for(typename std::list<Node>::iterator it = graph.begin() ; it != graph.end() ; ++it)
{
float dist = VEC3(rPos-it->node.getPosition()).norm2();
if(dist<distMin)
{
distMin = dist;
nProx = *it;
}
}
//compute displacement from this node toward rPos
STATE nextStep = fctStep(nProx.node, rPos, sampleSize);
//if trajectory ok : add rPos to graph
if(nextStep.isValid())
{
Node nNode;
nNode.node = nextStep;
nNode.prev = nProx;
graph.push_back(nNode);
last = nNode;
}
}
//get all position from stop to start and store them
while(last!=n)
{
res.push_back(last.node.getPosition());
last = *(last.prev);
}
std::reverse(res.begin(),res.end());
return res;
}
//namespace
}
......
......@@ -90,18 +90,18 @@ public:
~Simulator() ;
void init(unsigned int config, int minSize, float dimension, bool enablePathFinding = false) ;
void init( float dimension, bool enablePathFinding = false) ;
void doStep() ;
bool reachedGoal() ;
void setupCircleScenario(unsigned int nbAgents) ;
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
void addAgent(const VEC3& start,const VEC3& goal);
void addAgent(const VEC3& start,const std::vector<VEC3>& goal);
void setupMovingObstacle(unsigned int nbObstacles) ;
......@@ -122,7 +122,8 @@ public:
EnvMap envMap_ ;
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
int minSize;
unsigned int config;
float timeStep_ ;
float globalTime_ ;
unsigned int nbSteps_ ;
......@@ -137,7 +138,8 @@ public:
int avoidance;
int nb_dead;
bool multires;
bool detect_agent_collision;
ThreadUpdateInfo* tc1 ;
ThreadUpdateInfo* tc2 ;
ThreadUpdateInfo* tc3 ;
......
......@@ -8,7 +8,7 @@ float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = timeHorizonObst_ * maxSpeed_ + radius_ ;
float Agent::range_ = (timeHorizonObst_ * maxSpeed_ + radius_) ;
float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
......@@ -117,11 +117,12 @@ void Agent::updateAgentNeighbors()
sim_->nearNeighbors += agentNeighbors_.size() ;
sim_->totalNeighbors += agents.size() + neighborAgents.size() ;
if (agentNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ;
}
}
if (agentNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ;
}
}
......@@ -196,7 +197,7 @@ void Agent::update()
pos = pos + (velocity_ * sim->timeStep_) ;
#else
VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
obstacle_priority(&target);
#endif
meanSpeed_ *= 3.0f ;
......@@ -212,7 +213,62 @@ void Agent::update()
#endif
}
}
void Agent::obstacle_priority(PFP::VEC3 * goalVector)
void Agent::computeNewVelocity2()
{
VEC3 centroid ;
VEC3 c ;
VEC3 vel ;
float cohesion = 1.0f ;
centroid.zero() ;
c.zero() ;
vel.zero() ;
for (std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it)
{
}
unsigned int nbA = 0 ;
for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
{
VEC3 mPos = getPosition() ;
VEC3 oPos = (*it).second->getPosition() ;
centroid += oPos ;
VEC3 pOp(oPos - mPos) ;
if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f)
{
c -= pOp ;
// prefVelocity_.zero();
}
vel += (*it).second->prefVelocity_ ;
}
if (nbA > 0)
{
centroid /= nbA ;
centroid -= getPosition() ;
centroid /= 10 ;
vel /= nbA ;
vel -= prefVelocity_ ;
vel /= 8 ;
}
newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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)
{
float factor ;
Obstacle* obst ;
......@@ -260,7 +316,7 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector)
//(*goalVector).normalize();
}
void Agent::computePrefVelocity()
void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'objectif (sans prendre en compte l'environnement)
{
VEC3 goalVector = goals_[curGoal_] - getPosition() ;
float goalDist2 = goalVector.norm2() ;
......@@ -279,59 +335,14 @@ void Agent::computePrefVelocity()
}
prefVelocity_ = goalVector ;
}
void Agent::computeNewVelocity2()
{
VEC3 centroid ;
VEC3 c ;
VEC3 vel ;
float cohesion = 1.0f ;
centroid.zero() ;
c.zero() ;
vel.zero() ;