Commit bb6a8e0e authored by pitiot's avatar pitiot

evitement de loin

parent 86512eb7
......@@ -12,7 +12,7 @@ SET (CGoGN_EXT_LIBS ${CGoGN_EXT_LIBS} ${QT_LIBRARIES} ${QGLVIEWER_LIBRARIES})
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0")
include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
add_subdirectory(${CMAKE_SOURCE_DIR}/Release Release)
......
......@@ -389,9 +389,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
obstacleMark.mark(dd) ;
Dart next = map.phi1(dd) ;
Dart previous = map.phi_1(dd) ;
Obstacle* o = new Obstacle(position[dd], position[next], position[previous],
position[map.phi1(next)], NIL, NIL, NULL, 0);
Obstacle* o = new Obstacle(position[dd], position[next], NULL, 0);
#ifdef SPATIAL_HASHING
VEC3 ov = o->p2 - o->p1 ;
......
......@@ -34,7 +34,7 @@ class ArticulatedObstacle;
//#define EXPORTING3
#define TWO_AND_HALF_DIM
//#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
......@@ -377,20 +377,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
memo_mark.mark(*it);
///////TENTATIVE D'AJOUT RATE
// MovingObstacle * mo = o->mo;
// int n = o->index;
// int m = (n+1)%mo->nbVertices;
//#ifdef TWO_AND_HALF_DIM
// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d,o->p1,mo->sim_->envMap_.position);
//#else
// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(mo->sim_->envMap_.map, mo->position[n],o->p1,mo->sim_->envMap_.position);
//#endif
// registering_part->move(mo->velocity_[n]*mo->velocityAvoidanceFactor+mo->position[n],memo_mark_speed);
// registering_part->move(mo->velocity_[m]*mo->velocityAvoidanceFactor+mo->position[m],memo_mark_speed);
// std::vector<Dart> result =();
// d2=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
......
......@@ -6,7 +6,7 @@
#include "utils.h"
#include "env_map.h"
#include <set>
// #define LINEAR
#define LINEAR
// #define SECURED
#ifdef LINEAR
#include "ShapeMatching/shapeMatchingLinear.h"
......@@ -78,6 +78,7 @@ public:
void attachAgent(Agent* ag);
unsigned int nbVertices;
unsigned int nbParticles;
#ifdef SECURED
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP> * * parts_ ;
......@@ -124,7 +125,7 @@ public:
int max_x_ind;
int min_x_ind;
Obstacle* * obstacles_;
std::vector<Obstacle *> obstacles_;
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
......
......@@ -6,10 +6,10 @@
class Obstacle
{
public:
Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint, Dart d_1, Dart d_2,
Obstacle(const VEC3 point1, const VEC3 point2,
MovingObstacle * moving1=NULL, unsigned int ind=0) :
d1(d_1), d2(d_2), p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind),p3(0,0,0)
p1(point1), p2(point2),
mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(20),obst_stiffness_obst(20)
{
// p1[2] = 0 ;
// p2[2] = 0 ;
......@@ -17,16 +17,18 @@ public:
// nextP[2] = 0 ;
}
Dart d1;
Dart d2;
VEC3 p1 ;
VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
MovingObstacle * mo ;
unsigned int index ;
VEC3 p3; //pour les ellipses
double obst_stiffness_agent;
double obst_stiffness_obst;
} ;
#endif
......@@ -15,7 +15,7 @@ VEC3 Agent::xyPlane = VEC3(0,0,1);
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 20;
float Agent::averageMaxSpeed_ = 2.0f ;
float Agent::averageMaxSpeed_ = 0.5f ;
// float Agent::averageMaxSpeed_ = 20.0f ;
float Agent::neighborDist_ = 10.0f ;
//float Agent::neighborDist_ = 20.0f ;
......@@ -25,7 +25,7 @@ float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ;
//float Agent::timeHorizon_ = 100.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::timeHorizonObst_ = 20.0f ;
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ;
......@@ -809,12 +809,12 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3 norm;
double obst_stiffness = 100.0; // agent-obstacle interaction stiffness
int obst_power = 1 ; // the power to which elevate the agent-obstacle distance
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
double obst_radius_infl;
if(sim_->config==0)
obst_radius_infl = 100.; // scenario 0
obst_radius_infl = 1000.; // scenario 0
else
obst_radius_infl = 40.; // scenario 1 et 3
float force_value;
......@@ -842,7 +842,7 @@ void Agent::computeNewVelocity()
if(sum_of_dists < rest_sum_of_dists)
{
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
force_value = obst->obst_stiffness_agent*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
VEC3 normal = normFace ^v_obst;
// Ajouter une composante tangentielle
......
/*=====================================================================*\
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
///*=====================================================================*\
// 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() ;