Commit 59ec48c3 authored by pitiot's avatar pitiot

detection obstacles optimisée

parent 91a2f25b
......@@ -31,7 +31,8 @@ public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal=0, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
void initGL();
void draw();
void addGeneralCell ( Dart d);
bool removeGeneralCell (Dart d);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -91,7 +92,7 @@ public:
Obstacle* * obstacles_;
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
std::vector<std::pair<Dart, int> > general_belonging;
VEC3 front;
VEC3 finalGoal;
......
......@@ -218,7 +218,7 @@ public slots:
void slot_timer(bool b)
{
b ? timer->start() : timer->stop() ;
display_times =b;
// display_times =b;
}
void slot_drawEnvLines(bool b)
{
......
......@@ -867,6 +867,7 @@ void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
// mo->belonging_cells[n].clear();
mo->belonging_cells[n].push_back(d);
mo->addGeneralCell (d);
// mo->neighbor_cells[n].clear();
Dart dd = d;
......@@ -905,6 +906,7 @@ void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& me
// CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
addElementToVector<Obstacle*>(obstvect[*it],o);
mo->addGeneralCell (*it);
}
......@@ -932,6 +934,7 @@ void EnvMap::popObstacleInCells(Obstacle* o, int n)
for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it)
{
removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
mo->removeGeneralCell (*it);
}
for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
......
......@@ -15,6 +15,47 @@ float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;
void MovingObstacle::addGeneralCell ( Dart d)
{
bool added = false;
std::vector< std::pair<Dart,int> >::iterator it =general_belonging.begin();
while (it != general_belonging.end() )
{
if(sim_->envMap_.map.sameFace((*it).first,d))
{
(*it).second ++;
added= true;
break;
}
else ++it;
};
if (!added)
{
general_belonging.push_back(std::make_pair(d,1));
}
}
bool MovingObstacle::removeGeneralCell (Dart d)
{
std::vector< std::pair<Dart,int> >::iterator it =general_belonging.begin();
while (it != general_belonging.end() )
{
if(sim_->envMap_.map.sameFace((*it).first,d))
{
if(--((*it).second)==0)
{
*it = general_belonging.back() ;
general_belonging.pop_back() ;
}
return true;
}
else ++it;
};
return false;
}
bool MovingObstacle::is_inside(VEC3 p)
{
// return false;
......@@ -461,10 +502,10 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
{
obstacleNeighbors_.clear() ;
movingObstacleNeighbors_.clear() ;
for (std::set<Dart>::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
{
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[*it2] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[*it2] ;
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
// float maxDistObst = 0.0f ;
// float maxDistMovingObst = 0.0f ;
float distance_detection=2.5* gravity_dist;
......@@ -551,9 +592,7 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
void MovingObstacle::update()
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
///////remise a zero
if(sim_->detect_agent_collision)
general_belonging.clear();
//pour les tests de détection///////////////
if(!seen)
{
......@@ -744,25 +783,32 @@ void MovingObstacle::update()
sim_->envMap_.popAndPushObstacleInCells(o,i);
}
if(sim_->detect_agent_collision)
{
for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
{
general_belonging.insert(*it);
}
}
/////affichage des belonging cells
// CGoGNout<< CGoGNendl;
// CGoGNout << "Obstacle "<< i << ": ";
// for(std::vector<Dart>::iterator ite = belonging_cells[i].begin();ite!=belonging_cells[i].end(); ++ite)
// {
// CGoGNout<<(*ite).index<<" ; ";
// }
// CGoGNout<< CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
}
/////affichage du general_belonging
// CGoGNout<< CGoGNendl;
// CGoGNout << "General : ";
// for(std::vector<std::pair<Dart , int> >::iterator ite = general_belonging.begin();ite!=general_belonging.end(); ++ite)
// {
// CGoGNout<<"< "<<(*ite).first.index<<" , "<<(*ite).second<<"> ; ";
// }
// CGoGNout<< CGoGNendl;
//
// envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
// CGoGNout << "nouvel update : "<< CGoGNendl;
if(sim_->detect_agent_collision)
{
for (std::set<Dart>::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
{
std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
{
if (this->is_inside((*it)->part_.getPosition()))
......
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