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

merge avc arash

parents 6ee29e1a 419ba993
......@@ -55,7 +55,6 @@ public:
VEC3 finalGoal ;
Dart finalDart ;
MovingObstacle **movingObstacles_;
int nb_mos;
......@@ -63,6 +62,7 @@ public:
static unsigned int maxMovingObstacles_;
static float averageMaxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
static float radius_ ;
......
This diff is collapsed.
/*=====================================================================*\
Friday December 7th 2012
Arash HABIBI
agent.cpp
New behaviour algorithm based on a dynamic model
\*=====================================================================*/
#include "agent.h"
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
float Agent::maxSpeed_ = 2.0f ;
float Agent::neighborDist_ = 10.0f ;
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::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
#ifdef SPATIAL_HASHING
Agent::Agent(Simulator* sim, const VEC3& position) :
pos(position),
curGoal_(-1),
velocity_(0),
newVelocity_(0),
prefVelocity_(0),
meanSpeed_(0),
sim_(sim),
alive(true)
{
agentNeighbors_.reserve(maxNeighbors_* 2) ;
obstacleNeighbors_.reserve(maxNeighbors_* 2) ;
agentNo = cptAgent++ ;
}
#else
Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
part_(sim->envMap_.map, d, position, sim->envMap_.position),
curGoal_(-1),
velocity_(0),
newVelocity_(0),
prefVelocity_(0),
meanSpeed_(0),
sim_(sim),
alive(true)
{
color1 = 1.0f ;
color2 = 0 ;
color3 = 0 ;
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
agentNo = cptAgent++ ;
}
#endif
//-----------------------------------------------------------------
VEC3 Agent::getPosition()
{
#ifdef SPATIAL_HASHING
return pos ;
#else
return part_.getPosition() ;
#endif
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
bool agentSort(const std::pair<float, Agent*>& a1, const std::pair<float, Agent*>& a2)
{
return a1.first < a2.first ;
}
//-----------------------------------------------------------------
bool obstacleSort(const std::pair<float, Obstacle*>& o1, const std::pair<float, Obstacle*>& o2)
{
return o1.first < o2.first ;
}
//-----------------------------------------------------------------
void Agent::updateAgentNeighbors()
{
agentNeighbors_.clear() ;
#ifdef SPATIAL_HASHING
const std::vector<Agent*>& agents = sim_->envMap_.getNeighbors(this) ;
const std::vector<Agent*>& neighborAgents ;
sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ;
#else
const std::vector<Agent*>& agents = sim_->envMap_.agentvect[part_.d] ;
const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ;
#endif
float maxDist = 0.0f ;
for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
{
if ((*it)->alive)
{
if (*it != this)
{
float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
&& distSq < neighborDistSq_)
{
if (distSq > maxDist) maxDist = distSq ;
agentNeighbors_.push_back(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 ;
agentNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
sim_->nbUpdates++ ;
sim_->nearNeighbors += agentNeighbors_.size() ;
sim_->totalNeighbors += agents.size() + neighborAgents.size() ;
}
if (agentNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ;
}
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
void Agent::updateObstacleNeighbors()
{
obstacleNeighbors_.clear() ;
movingObstacleNeighbors_.clear() ;
#ifdef SPATIAL_HASHING
Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ;
if(sim_->envMap_.ht_obstacles.hasData(c))
{
const std::vector<Obstacle*>& obst = sim_->envMap_.ht_obstacles[c] ;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ;
if(distSq < rangeSq_)
{
if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
#else
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)) ;
}
}
}
}
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)) ;
}
}
}
#endif
// if (obstacleNeighbors_.size() > maxNeighbors_)
// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
void Agent::update()
{
if (alive)
{
finalGoal = goals_[curGoal_] ;
velocity_[0] = newVelocity_[0] ;
velocity_[1] = newVelocity_[1] ;
#ifdef SPATIAL_HASHING
pos = pos + (velocity_ * sim->timeStep_) ;
#else
VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
#endif
meanSpeed_ *= 3.0f ;
meanSpeed_ += velocity_ ;
meanSpeed_ /= 4.0f ;
// meanPos_ *= 9.0f;
// meanPos_ += part_.m_position;
// meanPos_ /= 10.0f;
#ifndef SPATIAL_HASHING
part_.move(target) ;
#endif
}
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
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///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------
// fonction d'évitement d'obstacles par les agents (calcul effectué
// avant de lancer RVO2 pour garantir la non-collision entre agents)
// En gros, on représente un obstacle :
// soit par un ensemble de segments orientés dans le sens direct (sim_->avoidance==0),
// soit par son centre (sim_->avoidance==1)
// Cette fonction parcourt les obstacles et calcule la norme de la force globale de
// répulsion. Ensuite, elle ajoute cette somme à l'objectif de façon
// à ce que l'agent change sa trajectoire.
void Agent::obstacle_priority(PFP::VEC3 * goalVector)
{
float factor ;
Obstacle* obst ;
PFP::VEC3 x,y,vec ;
PFP::VEC3 norm,sumNorm ;
sumNorm.zero() ;
// On parcourt les obstacles
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
// rangeSq = range * range (portee au carre)
// les elements de l'iteration sont des obstacles.
// chaque obstacle est une paire : float et *obstacle.
// le float est la distance à l'obstacle et *obstacle
// est un pointeur vers le segment.
factor =(1.0f-(it->first/rangeSq_)) ;
// En particulier ici, (*it).first (ou it->first)
// represente la distance à l'agent (?)
// factor est quelque chose qui faut 0
// quand l'agent commence à interagir avec l'obstacle
// et qui faut 1 quand l'agent se trouve colle sur l'obstacle.
// CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
obst=it->second ;
x=obst->p1 ;
y=obst->p2 ;
vec=y-x ;
vec.normalize() ;
norm.zero() ;
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
}
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
// MovingObstacle * mo = obst->mo;
// norm = this->part_.getPosition()-mo->center;
// }
norm.normalize() ;
norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
// On met factor à la puissance 8 pour adoucir l'impact.
sumNorm+=norm ;
}
// if (sumNorm.norm2()>1.0f) sumNorm.normalize();
// if(sumNorm!=0)CGoGNout<<sumNorm<<"goal vector before :" <<(*goalVector)<< " goal vector after : "<<((*goalVector)+sumNorm)<<CGoGNendl;
//(*goalVector)=((*goalVector)*9.0f+11.0f*sumNorm)/20.0f;
(*goalVector)=((*goalVector)+sumNorm) ;
//(*goalVector).normalize();
}
//-----------------------------------------------------------------
// Calcul vitesse optimale pour atteindre l'objectif (sans prendre
// en compte l'environnement).
void Agent::computePrefVelocity()
{
VEC3 goalVector = goals_[curGoal_] - getPosition() ;
float goalDist2 = goalVector.norm2() ;
// Si l'agent arrive à proximité de l'objectif,
// alors il passe à l'objectif suivant
if (goalDist2 < radius_*radius_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - getPosition() ;
goalDist2 = goalVector.norm2() ;
}
// goadDist2 est le vecteur reliant l'agent à l'objectif.
// Tant que ce vecteur a une norme supérieure à maxSpeed_
// (i.e. tant qu'il est encore assez loin de l'objectif)
// la vitesse sera de maxSpeed_. Sinon, s'il est plus
// proche, la vitesse diminue.
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
prefVelocity_ = goalVector ;
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
{
// The objective is to compute the sum of forces exerted on the agent.
double collision_softening_factor;
double ag_mass = 1.0;
float ag_ambient_damping = 1.0;
//-------------
VEC3 p1, p2, vec, forces, previous_pos;
forces.zero();
previous_pos = getPosition() - velocity_*sim_->timeStep_;
//----- force due à l'attraction de l'objectif ----------
float goal_attraction_force = 200.0; // agent-goal interaction stiffness
VEC3 p_goal = goals_[curGoal_];
VEC3 u_goal = p_goal - getPosition();
u_goal.normalize();
forces += goal_attraction_force * u_goal;
//----- forces dues à la répulsion des obstacles ----------
VEC3 norm;
double obst_stiffness = 50.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 4; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
double dist = it->first;
// double effective_range = 50*range_;
double effective_range = 10*range_;
float force_value=0.0;
if(dist < effective_range)
{
collision_softening_factor = pow(1 - dist/effective_range,obst_power);
force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
}
obst=it->second ;
p1=obst->p1 ;
p2=obst->p2 ;
vec=p2-p1;
vec.normalize();
norm.zero();
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
}
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
forces += force_value * norm;
}
//----- forces dues à la répulsion des autres agents -------
double ag_stiffness = 20.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping
int ag_power = 4; // the power to which elevate the agent-agent distance
unsigned int nbA = 0 ;
for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
{
Agent* other = it->second ;
const VEC3 relativePosition(other->getPosition()-getPosition());
const float distSq = relativePosition.norm2() ;
const float dist = sqrt(distSq);
const float combinedRadius = 10*(radius_ + other->radius_) ;
// const float combinedRadiusSq = combinedRadius * combinedRadius ;
VEC3 other_previous_pos = other->getPosition() - (other->velocity_*sim_->timeStep_);
VEC3 previous_relativePosition = other_previous_pos - previous_pos;
float previous_distSq = previous_relativePosition.norm2();
float previous_dist = sqrt(previous_distSq);
// const VEC3 u_other(relativePosition);
VEC3 u_other(relativePosition);
u_other = relativePosition;
u_other.normalize();
// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
if(dist < combinedRadius)
{
collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
- ag_damping * (dist - previous_dist) / sim_->timeStep_;
forces += force_value * u_other;
}
}
//------- calcul de la force de trainee -----------------------------
forces -= ag_ambient_damping * velocity_;
//------- calcul de la nouvelle valeur de la vitesse ----------------
VEC3 velocity_tmp;
velocity_tmp = velocity_ + forces * (sim_->timeStep_ / ag_mass);
if(velocity_tmp.norm2() > maxSpeed_*maxSpeed_)
{
velocity_tmp.normalize();
velocity_tmp *= maxSpeed_;
}
newVelocity_ = velocity_tmp;
}
//-----------------------------------------------------------------
bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
return true ;
}
//-----------------------------------------------------------------
unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
return 1;
}
//-----------------------------------------------------------------
void Agent::linearProgram3(const std::vector<Line>& lines, unsigned int numObstLines, unsigned int beginLine, float radius, VEC3& result)
{
}
This diff is collapsed.
This diff is collapsed.
......@@ -100,7 +100,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
std::string filename2 = "./svg/mapBuild.svg" ;
// Algo::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;
Geom::BoundingBox<typename PFP::VEC3> bb1, bb2 ;
Geom::BoundingBox<PFP::VEC3> bb1, bb2 ;
bb1 = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
// bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
// bb1.addPoint(bb2.min());
......
......@@ -197,8 +197,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
// return position[d]+deformation[d];
return position[d];
return position[d]+deformation[d];
// return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -393,6 +393,8 @@ void MovingObstacle::update()
Dart d;
velocity_[0] = newVelocity_[0] * velocity_factor;
velocity_[1] = newVelocity_[1] * velocity_factor;
// velocity_[0] = 0.0;
// velocity_[1] = 0.0;
// MAJ des particules
float abs_angle= angle > 0 ? 1 : -1;
......
......@@ -18,10 +18,8 @@ Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgen
detect_agent_collision=true;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
this->config=config;
init(2.0f, nbAgent, nbObst) ;