Skip to content

GitLab

Commit ee1b831c authored by David Cazier's avatar David Cazier
Browse files

Nettoyage scenario

parent d904c0a3
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 210 additions and 491 deletions
include/agent.h
View file @ ee1b831c
......@@ -14,9 +14,9 @@ class Agent
{
public:
#ifdef SPATIAL_HASHING
Agent(Simulator* sim, const VEC3& position) ;
Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) ;
#else
Agent(Simulator* sim, const VEC3& position, Dart d) ;
Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) ;
#endif
VEC3 getPosition() ;
......@@ -50,15 +50,14 @@ public:
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_ ;
#endif
unsigned int curGoal_ ;
std::vector<VEC3> goals_ ;
VEC3 finalGoal ;
Dart finalDart ;
unsigned int curGoal_ ;
static unsigned int maxNeighbors_ ;
static float maxSpeed_ ;
static float averageMaxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
static float radius_ ;
......@@ -74,12 +73,12 @@ public:
float color3;
unsigned int agentNo ;
float maxSpeed_ ;
VEC3 velocity_ ;
VEC3 newVelocity_ ;
VEC3 prefVelocity_ ;
VEC3 meanSpeed_ ;
// VEC3 meanPos_;
VEC3 meanVelocity_[4] ;
VEC3 meanDirection_ ;
Simulator* sim_;
bool alive;
......
include/env_generator.hpp
View file @ ee1b831c
......@@ -49,15 +49,6 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
Dart d = prim.grid_topo(nx, ny) ;
prim.embedGrid(envMap.geometry.size(0), envMap.geometry.size(1), 0.0f) ;
// envMap.agentvect = envMap.map.addAttribute<typename PFP::AGENTVECT, FACE>("agents") ;
// envMap.neighborAgentvect = envMap.map.addAttribute<typename PFP::AGENTVECT, FACE>("neighborAgents") ;
// envMap.obstvect = envMap.map.addAttribute<typename PFP::OBSTACLEVECT, FACE>("obstacles") ;
// envMap.neighborObstvect = envMap.map.addAttribute<typename PFP::OBSTACLEVECT, FACE>("neighborObstacles") ;
// envMap.subdivisableFace = envMap.map.addAttribute<typename PFP::BOOLATTRIB, FACE>("subdivisableFace") ;
//
// envMap.refineCandidate.reserve(100) ;
// envMap.coarsenCandidate.reserve(100) ;
Dart boundary = envMap.map.phi2(d) ;
envMap.buildingMark.mark(boundary) ;
......
include/env_map.h
View file @ ee1b831c
......@@ -184,8 +184,7 @@ public:
return ht_agents[c];
}
const std::vector<Agent*>& getNeighbors(Agent* a) ;
void getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors) ;
const std::vector<Agent*>& getNeighborAgents(Agent* a) ;
void addAgentInGrid(Agent* a) ;
void addAgentInGrid(Agent* a, Geom::Vec2ui c) ;
......@@ -194,6 +193,7 @@ public:
void removeAgentFromGrid(Agent* a, Geom::Vec2ui c) ;
HashTable2D< std::vector<Agent*> > ht_agents ;
HashTable2D< std::vector<Agent*> > ht_neighbor_agents ;
HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
} ;
......
include/simulator.h
View file @ ee1b831c
......@@ -80,31 +80,24 @@ public:
class Simulator
{
public:
Simulator(int minSize) ;
Simulator(
float timeStep, float neighborDist, unsigned int maxNeighbors,
float timeHorizon, float timeHorizonObst, float radius,
float maxSpeed, const VEC3& velocity = VEC3(0)
);
Simulator(unsigned int config, unsigned int minSize) ;
~Simulator() ;
void init( float dimension, bool enablePathFinding = false) ;
void init(unsigned int config, float dimension, bool enablePathFinding = false) ;
void doStep() ;
bool reachedGoal() ;
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCircleScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridor2Scenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
void addAgent(const VEC3& start,const std::vector<VEC3>& goal);
void addAgent(const VEC3& start,const std::vector<VEC3>& goals);
void setupMovingObstacle(unsigned int nbObstacles) ;
#ifndef SPATIAL_HASHING
void addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal);
void addPathToCorner() ;
......@@ -123,7 +116,6 @@ public:
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
int minSize;
unsigned int config;
float timeStep_ ;
float globalTime_ ;
unsigned int nbSteps_ ;
......
include/viewer.h
View file @ ee1b831c
......@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents(int minSize = Agent::neighborDist_, int iterations = 0) ;
SocialAgents(unsigned int config, unsigned int minSize, unsigned int iterations) ;
void initGUI() ;
......@@ -72,15 +72,19 @@ public:
QTimer* timer ;
int nbIterations ;
// Number of iterations before stopping the animation (0 if interactive animation)
int maxIterations ;
/* nbIterations counts the current number of iterations
* When maxIterations is reached the simulation is stopped
* If maxIterations is set to 0 the simulation is never stopped (interactive)
*/
unsigned int nbIterations ;
unsigned int maxIterations ;
// to count fps
int frames ;
unsigned int frames ;
struct timespec startTime ;
time_t nextUpdate ;
Simulator sim ;
Simulator simulator ;
SelectorTrue allDarts ;
bool render_anim ;
......
src/agent.cpp
View file @ ee1b831c
......@@ -2,52 +2,47 @@
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
float Agent::maxSpeed_ = 2.0f ;
float Agent::averageMaxSpeed_ = 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::range_ = (timeHorizonObst_ * averageMaxSpeed_ + radius_) ;
float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
#ifdef SPATIAL_HASHING
Agent::Agent(Simulator* sim, const VEC3& position) :
Agent::Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) :
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),
Agent::Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) :
part_(sim->envMap_.map, sim->envMap_.getBelongingCell(position), position, sim->envMap_.position),
#endif
curGoal_(0),
goals_(goals),
velocity_(0),
newVelocity_(0),
prefVelocity_(0),
meanSpeed_(0),
meanDirection_(0),
sim_(sim),
alive(true)
{
color1 = 1.0f ;
color2 = 0 ;
#ifdef SPATIAL_HASHING
sim->envMap_.addAgentInGrid(this) ;
#endif
float ratio = (80.0f + rand() % 20) / 100.0f ;
maxSpeed_ = averageMaxSpeed_ * ratio ; // from 80% to 100% of averageMaxSpeed_
color1 = 1.0f * ratio ; // red = high speed agents
color2 = 1.0f * 5 * (1 - ratio) ; // green = low speed agents
color3 = 0 ;
for (unsigned int i=0 ; i<4 ; ++i) meanVelocity_[i].set(0) ;
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
agentNo = cptAgent++ ;
}
#endif
VEC3 Agent::getPosition()
{
......@@ -74,8 +69,7 @@ void Agent::updateAgentNeighbors()
#ifdef SPATIAL_HASHING
const std::vector<Agent*>& agents = sim_->envMap_.getNeighbors(this) ;
const std::vector<Agent*>& neighborAgents ;
sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ;
const std::vector<Agent*>& neighborAgents = sim_->envMap_.getNeighborAgents(this);
#else
const std::vector<Agent*>& agents = sim_->envMap_.agentvect[part_.d] ;
const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ;
......@@ -191,26 +185,21 @@ void Agent::update()
if (alive)
{
finalGoal = goals_[curGoal_] ;
velocity_[0] = newVelocity_[0] ;
velocity_[1] = newVelocity_[1] ;
velocity_ = newVelocity_ ;
#ifdef SPATIAL_HASHING
pos = pos + (velocity_ * sim->timeStep_) ;
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
meanDirection_.set(0) ;
for (unsigned int i=0 ; i<4 ; ++i) {
meanDirection_ += meanVelocity_[i] ;
meanVelocity_[i] = meanVelocity_[(i+1)%4] ;
}
meanVelocity_[3] = velocity_ ;
meanDirection_.normalize() ;
}
}
......@@ -296,7 +285,7 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
norm = this->getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
......@@ -318,24 +307,21 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
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() ;
prefVelocity_ = goals_[curGoal_] - getPosition() ;
float goalDist2 = prefVelocity_.norm2() ;
if (goalDist2 < radius_*radius_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - getPosition() ;
goalDist2 = goalVector.norm2() ;
prefVelocity_ = goals_[curGoal_] - getPosition() ;
goalDist2 = prefVelocity_.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
prefVelocity_.normalize() ;
prefVelocity_ *= maxSpeed_;
}
prefVelocity_ = goalVector ;
}
/* Search for the best new velocity. */
......
src/env_map.cpp
View file @ ee1b831c
......@@ -1064,168 +1064,6 @@ const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
return ht_neighbor_agents[c] ;
}
void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
Geom::Vec2ui c = agentPositionCell(a) ;
for(int ii = -1 ; ii <= 1 ; ++ii)
{
for(int jj = -1 ; jj <= 1 ; ++jj)
{
if (ii != 0 || jj != 0)
{
Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
const std::vector<Agent*>& v = ht_agents[cc] ;
neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
}
}
}
}
void EnvMap::addAgentInGrid(Agent* a)
{
Geom::Vec2ui c = agentPositionCell(a) ;
addAgentInGrid(a,c) ;
}
void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
ht_agents[c].push_back(a) ;
PFP::AGENTS agents ;
PFP::OBSTACLEVECT obst ;
PFP::OBSTACLEVECT neighborObst ;
fit = old ;
do
{
PFP::AGENTS a(agentvect[fit]) ;
PFP::OBSTACLEVECT ob(obstvect[fit]) ;
PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
obst.insert(obst.end(), ob.begin(), ob.end()) ;
neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
popAgentInCells(*ait, fit) ;
map.setCurrentLevel(fLevel - 1) ;
Dart nf = map.phi2(fit) ;
if (!map.faceIsSubdivided(nf))
{
map.setCurrentLevel(fLevel) ;
PFP::AGENTS& an = agentvect[nf] ;
PFP::OBSTACLEVECT resetob = obstvect[nf] ;
for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
{
if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
}
for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
{
MovingObstacle* mo = (*ait)->mo ;
if (mo != NULL)
{
resetPart(mo, nf) ;
}
}
map.setCurrentLevel(fLevel - 1) ;
}
fit = map.phi1(fit) ;
} while (fit != old) ;
if (degree == 3)
{
map.setCurrentLevel(fLevel) ;
Dart centerFace = map.phi2(map.phi1(old)) ;
PFP::AGENTS a(agentvect[centerFace]) ;
PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
obst.insert(obst.end(), ob.begin(), ob.end()) ;
neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
popAgentInCells(*ait, centerFace) ;
map.setCurrentLevel(fLevel - 1) ;
}
neighborAgentvect[old].clear() ;
// TODO Check with optimisation
map.setCurrentLevel(map.getMaxLevel()) ;
for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
register_pop(*ait, (*ait)->index) ;
for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
register_pop(*ait, (*ait)->index) ;
map.setCurrentLevel(fLevel - 1) ;
Algo::IHM::coarsenFace<PFP>(map, old, position) ;
std::pair<bool, bool>& sf = subdivisableFace[old] ;
sf.first = true ;
sf.second = true ;
void EnvMap::removeAgentFromGrid(Agent* a)
{
Geom::Vec2ui c = agentPositionCell(a) ;
removeAgentFromGrid(a,c) ;
}
void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
removeElementFromVector<Agent*>(ht_agents[c], a) ;
if (ht_agents[c].empty()) ht_agents.erase(c) ;
for(int ii = -1; ii <= 1; ++ii)
{
for(int jj = -1; jj <= 1; ++jj)
{
if(!(ii == 0 && jj == 0))
{
Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
removeElementFromVector<Agent*>(ht_neighbor_agents[cc], a) ;
if (ht_neighbor_agents[c].empty()) ht_neighbor_agents.erase(c) ;
}
}
}
}
#endif
#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
VEC3 relativePos = a->pos - geometry.min() ;
relativePos /= minCellSize ;
return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
Geom::Vec2ui c = agentPositionCell(a) ;
return ht_agents[c] ;
}
const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
{
Geom::Vec2ui c = agentPositionCell(a) ;
return ht_neighbor_agents[c] ;
}
void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
Geom::Vec2ui c = agentPositionCell(a) ;
for(int ii = -1 ; ii <= 1 ; ++ii)
{
for(int jj = -1 ; jj <= 1 ; ++jj)
{
if (ii != 0 || jj != 0)
{
Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
const std::vector<Agent*>& v = ht_agents[cc] ;
neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
}
}
}
}
void EnvMap::addAgentInGrid(Agent* a)
{
Geom::Vec2ui c = agentPositionCell(a) ;
......
src/moving_obstacle.cpp
View file @ ee1b831c
......@@ -386,7 +386,7 @@ void MovingObstacle::update()
std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
{
if (this->is_inside((*it)->part_.getPosition()))
if (this->is_inside((*it)->getPosition()))
{
(*it)->alive=false;
(*it)->color1=0.0f;
......
src/simulator.cpp
View file @ ee1b831c
#include "simulator.h"
Simulator::Simulator(int minS) :
Simulator::Simulator(unsigned int config, unsigned int minS) :
timeStep_(0.2f),
globalTime_(0.0f),
nbSteps_(0),
......@@ -18,8 +18,7 @@ Simulator::Simulator(int minS) :
detect_agent_collision=false;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
config=5;
init( minSize, 2.0f) ;
init(config, minSize, 2.0f) ;
}
Simulator::~Simulator()
......@@ -28,22 +27,20 @@ Simulator::~Simulator()
delete agents_[i] ;
}
void Simulator::init( float dimension, bool enablePathFinding)
void Simulator::init(unsigned int config, float dimension, bool enablePathFinding)
{
std::cout << "Setup scenario" << std::endl ;
#ifndef SPATIAL_HASHING
// envMap_.subdivideToProperLevel() ;
// envMap_.subdivideAllToMaxLevel();
#endif
switch (config)
{
case 0 :
setupCircleScenario(1000,35) ;
setupCircleScenario(0, 1000, 35) ;
break ;
case 1 :
setupCorridorScenario(1000,40) ;
setupCorridorScenario(1, 1000,40) ;
break ;
case 2 :
// setupCorridor2Scenario(1, 200,4) ;
break ;
// case 2 :
// setupScenario(1000) ;
......@@ -57,8 +54,11 @@ void Simulator::init( float dimension, bool enablePathFinding)
// importAgents("myAgents.pos") ;
// break ;
case 5:
envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
envMap_.init(5, 1600.0f, 1200.0f, minSize, 400.0f) ; //cases fines
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
exit(1) ;
}
#ifndef SPATIAL_HASHING
......@@ -85,10 +85,8 @@ void Simulator::init( float dimension, bool enablePathFinding)
#ifndef SPATIAL_HASHING
if (multires)
envMap_.subdivideToProperLevel() ;
// envMap_.subdivideAllToMaxLevel();
// envMap_.subdivideAllToMaxLevel();
#endif
// setupMovingObstacle(40);
}
void Simulator::doStep()
......@@ -187,33 +185,14 @@ bool Simulator::reachedGoal()
return true ;
}
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goal)
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goals)
{
#ifdef SPATIAL_HASHING
Agent* a = new Agent(this, start) ;
agents_.push_back(a) ;
envMap_.addAgentInGrid(a) ;
#else
Dart dCell = envMap_.getBelongingCell(start) ;
// if (dCell==envMap_.map.begin()) CGoGNout<<"pbagents"<<CGoGNendl;
agents_.push_back(new Agent(this, start, dCell)) ;
#endif
agents_.back()->goals_=goal ;
agents_.back()->curGoal_ = 0 ;
agents_.push_back(new Agent(this, start, goals)) ;
}
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents , unsigned int nbObstacles)
{
if (multires)
{
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
}
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;
......@@ -312,17 +291,9 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
void Simulator::setupCorridorScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles)
{