Commit 59ec48c3 authored by pitiot's avatar pitiot

detection obstacles optimisée

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