Commit 4590db2d authored by pitiot's avatar pitiot
Browse files

mise en forme

parent a3f5796b
......@@ -19,10 +19,12 @@ class MovingObstacle
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, VEC3 goal,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();
void computeNewVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1);
void update();
......@@ -48,11 +50,11 @@ public:
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
VEC3 front;
CellMarkerMemo<FACE> * memo_mark;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
std::vector<std::pair<float, MovingObstacle*> > movingObstNeighbors_;
std::set<std::pair<float, Agent*> > agentNeighbors_ ;
std::set<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
std::set<std::pair<float, Obstacle*> > movingObstNeighbors_;
float rot;
VEC3 finalGoal;
......@@ -65,6 +67,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_;
......
......@@ -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) ;
}
}
......@@ -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() ;
......@@ -282,56 +338,8 @@ void Agent::computePrefVelocity()
obstacle_priority(&prefVelocity_);
}
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 ;
}
/* Search for the best new velocity. */
void Agent::computeNewVelocity()
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes
{
std::vector<Line> orcaLines_ ;
orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ;
......
......@@ -6,7 +6,8 @@
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 1.0f;
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;
......@@ -132,109 +133,165 @@ bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
}
// TODO Check position
void MovingObstacle::updateFixedObstNeighbors()
{
PFP::VEC3 toto;
PFP::VEC3 toto_norm;
//for particles in the "front" of the object (to modify)
for(int k =0;k<2;k++)
{
registering_part->get_memo(vertices[k]);
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[registering_part->d];
//search all obstacles around
for(std::vector<Obstacle*>::const_iterator it = obst.begin(); it != obst.end(); ++it)
{
//only for fixed obstacles around
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, vertices[k]);
if(distSq < detectionFixedObst*detectionFixedObst)
{
toto= (*it)->p1 -(*it)->p2;
toto_norm=toto;
toto_norm[0]=-toto[1];
toto_norm[1]=toto[0];
if(test_opposition(toto_norm,front,center)) //// à changer ////////////
{
int co = rand() % 2 ;
if (toto[0]==0)
{
finalGoal[0]=front[0];
if (co == 0)
finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
else
finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
}
else
{
finalGoal[1]=front[1];
if (co == 0)
finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
else
finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
}
float angle =get_angle(finalGoal-center,front -center);
make_half_turn=angle*nbVertices;
break;
}
}
}
}
}
}
//void MovingObstacle::contournerBatiment()
//{
// PFP::VEC3 toto;
// PFP::VEC3 toto_norm;
// //for particles in the "front" of the object (to modify)
// for(int k =0;k<2;k++)
// {
//
// registering_part->get_memo(vertices[k]);
// std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[registering_part->d];
//
// //search all obstacles around
// for(std::vector<Obstacle*>::const_iterator it = obst.begin(); it != obst.end(); ++it)
// {
// //only for fixed obstacles around
// if ((*it)->mo==NULL)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, vertices[k]);
// if(distSq < detectionFixedObst*detectionFixedObst)
// {
// toto= (*it)->p1 -(*it)->p2;
// toto_norm=toto;
// toto_norm[0]=-toto[1];
// toto_norm[1]=toto[0];
//
// if(test_opposition(toto_norm,front,center)) //// à changer ////////////
// {
// int co = rand() % 2 ;
// if (toto[0]==0)
// {
// finalGoal[0]=front[0];
// if (co == 0)
// finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
// else
// finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
// }
// else
// {
// finalGoal[1]=front[1];
// if (co == 0)
// finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
// else
// finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
// }
//
// float angle =get_angle(finalGoal-center,front -center);
// make_half_turn=angle*nbVertices;
// break;
// }
// }
// }
// }
// }
//}
// TODO Check position
void MovingObstacle::computePrefVelocity()
void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au bord (distpointlinesq) de l'obstacle // a mettre en place si besoin
{
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
prefVelocity_ = goalVector ;
// VEC3 goalVector = finalGoal - center;
// int co=2;
// float angle;
// float goalDist2 = goalVector.norm2();
// // CGoGNout << "finalGoal "<< finalGoal << CGoGNendl;
// // CGoGNout << "dist 2 "<< goalDist2 << CGoGNendl;
// if(goalDist2 < 2.0f)
// agentNeighbors_.clear() ;
// Dart d;
// float maxDist = 0.0f ;//distance max des agents eregistrés au centre
// std::set::iterator agIt;//emplacement de l'agent en question
// for(int i = 0;i< nbVertices;i++ )
// {
//// co=rand() % 2 + 1;
// registering_part->get_memo(vertices[i]);
// d=registering_part->d;
// const std::vector<Agent*>& agents = sim_->envMap_.agentvect[d] ;
// const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[d] ;
//
//
//
// for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
// {
// if ((*it)->alive)
// {
//
// if(co==1)
// finalGoal[0]*=-1;
// else
// finalGoal[1]*=-1;
// float distSq = (center - (*it)->getPosition()).norm2() ;
// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
// && distSq < neighborDistSq_)
// {
//
// goalVector = finalGoal - center;
// angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
// make_half_turn=angle*nbVertices;
// if (distSq > maxDist)
// {
// maxDist = distSq ;
// agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
// }
// else
// {
// agentNeighbors_.insert(std::make_pair(distSq, *it))
// }
// }
//
// }
// }
//
// for (std::vector<Agent*>::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
// {
// if ((*it)->alive)
// {
// float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
// && distSq < neighborDistSq_)
// {
// if (distSq > maxDist)
// {
// maxDist = distSq ;
// agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
// }
// else
// {
// agentNeighbors_.insert(std::make_pair(distSq, *it))
// }
// }
// }
//
// }
// }
}
void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
// obstacleNeighbors_.clear() ;
// movingObstacleNeighbors_.clear() ;
//
// if (goalDist2 > 1.0f)
// goalVector.normalize();
// std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
// std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
// for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if (distSq < rangeSq_)
// {
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// {
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
//
// prefVelocity_=goalVector;
// }
// }
// for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if(distSq < rangeSq_)
// {
// if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
// }
}
// TODO Check position
void MovingObstacle::update()
{
......@@ -245,8 +302,8 @@ void MovingObstacle::update()
PFP::VEC3 bary = 0;
Dart d;
velocity_[0] = prefVelocity_[0] * velocity_factor;
velocity_[1] = prefVelocity_[1] * velocity_factor;
velocity_[0] = newVelocity_[0] * velocity_factor;
velocity_[1] = newVelocity_[1] * velocity_factor;
// MAJ des particules
make_half_turn = 0;
......@@ -518,3 +575,37 @@ void displayMO(Obstacle * o)
{
CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////comportement à modifier///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
prefVelocity_ = goalVector ;
}
void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenant compte de l'environnement.
{
newVelocity_=prefVelocity_;
}
......@@ -96,6 +96,7 @@ void Simulator::doStep()
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
{
movingObstacles_[i]->computePrefVelocity() ;
movingObstacles_[i]->computeNewVelocity() ;
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
......
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