Skip to content

GitLab

Commit b3b10bd1 authored by Pierre Kraemer's avatar Pierre Kraemer
Browse files

subdiv ok.. perf++

parent b8f34fe9
Hide whitespace changes
Inline Side-by-side
Showing with 301 additions and 192 deletions
include/agent.h
View file @ b3b10bd1
......@@ -14,21 +14,12 @@ class Agent
public:
Agent(Simulator* sim, const VEC3& position, Dart d);
void insertAgentNeighbor(Agent* other);
void insertAgentNeighbor(Agent* other, float distSq);
void removeAgentNeighbor(Agent* other);
void updateAgentNeighbors();
void clearAgentNeighbors();
void insertObstacleNeighbor(Dart d);
void removeObstacleNeighbor(Dart d);
void updateObstacleNeighbors();
void clearObstacleNeighbors();
void update();
void computePrefVelocity();
void computeNewVelocity();
bool linearProgram1(
......@@ -47,14 +38,9 @@ public:
const std::vector<Line>& lines, unsigned int numObstLines,
float radius, VEC3& result
);
std::multimap<float, Agent*> agentNeighbors_;
std::multimap<float, Agent*> newAgentNeighbors_;
// std::map<const Agent*, std::multimap<float, Agent*>::iterator> neighborsIt_;
std::multimap<float, Dart> obstacleNeighbors_;
std::multimap<float, Dart> newObstacleNeighbors_;
std::vector<std::pair<float, Agent*> > agentNeighbors_;
std::vector<std::pair<float, Dart> > obstacleNeighbors_;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_;
......@@ -74,8 +60,6 @@ public:
VEC3 prefVelocity_;
Simulator* sim_;
bool treated;
};
#endif
......
include/env_map.h
View file @ b3b10bd1
......@@ -54,18 +54,18 @@ public :
void init();
void foreach_neighborFace(Dart d, FunctorType& f);
void registerObstaclesInFaces();
void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor);
void setAgentNeighborsAndObstacles(Agent* agent);
void addNeighborAgents(Agent* agent, Dart d);
// void agentChangeFaceThroughEdge(Agent* agent, Dart oldFace);
void agentChangeFaceThroughEdge(Agent* agent);
void agentChangeFace(Agent* agent, Dart oldFace);
void pushAgentInCell(Agent* agent, Dart d);
void popAgentInCell(Agent* agent, Dart d);
void pushAgentInCells(Agent* agent, Dart d);
void popAgentInCells(Agent* agent, Dart d);
void addRefineCandidate(Dart d);
void addCoarsenCandidate(Dart d);
......@@ -81,13 +81,15 @@ public :
PFP::TVEC3 normal;
PFP::TAB_AGENTVECT agentvect;
PFP::TAB_AGENTVECT neighborAgentvect;
PFP::TAB_OBSTACLEVECT obstvect;
CellMarker obstacleMark;
CellMarker buildingMark;
static const unsigned int nbAgentsToSubdivide = 12;
static const unsigned int nbAgentsToSimplify = 8;
static const unsigned int nbAgentsToSimplify = 7;
std::map<unsigned int, Dart> refineCandidate;
std::map<unsigned int, Dart> coarsenCandidate;
......@@ -97,31 +99,64 @@ public :
* INLINE FUNCTIONS *
**************************************/
inline void EnvMap::pushAgentInCell(Agent* agent, Dart d)
inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
agentvect[d].push_back(agent);
addRefineCandidate(d);
Dart dd = d;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
neighborAgentvect[ddd].push_back(agent);
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
}
inline void EnvMap::popAgentInCell(Agent* agent, Dart d)
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end());
bool found = false;
PFP::AGENTS::iterator end = agentvect[d].end();
for(PFP::AGENTS::iterator it = agentvect[d].begin(); it != end; ++it)
for(PFP::AGENTS::iterator it = agentvect[d].begin(); it != end && !found; ++it)
{
if(*it == agent)
{
*it = agentvect[d].back();
agentvect[d].pop_back();
break;
found = true;
}
}
addCoarsenCandidate(d);
Dart dd = d;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
bool found = false;
PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
{
if(*it == agent)
{
*it = neighborAgentvect[ddd].back();
neighborAgentvect[ddd].pop_back();
found = true;
}
}
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
}
inline void EnvMap::addRefineCandidate(Dart d)
......@@ -141,4 +176,3 @@ inline void EnvMap::clearUpdateCandidates()
}
#endif
include/env_render.h
View file @ b3b10bd1
......@@ -49,9 +49,9 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glLineWidth(1.0f);
glColor3f(1.0f,0.0f,0.0f);
glBegin(GL_LINE_LOOP);
glBegin(GL_POLYGON);
for(unsigned int i = 0; i < 5; ++i)
glVertex2f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius));
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glEnd();
if(showNeighborDist)
......@@ -60,7 +60,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,1.0f,0.0f);
glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i)
glVertex2f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius));
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glEnd();
}
......@@ -70,7 +70,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,0.0f,1.0f);
glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i)
glVertex2f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius));
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glEnd();
}
}
......
src/agent.cpp
View file @ b3b10bd1
......@@ -13,79 +13,57 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
velocity_(0),
newVelocity_(0),
prefVelocity_(0),
sim_(sim),
treated(false)
{
rangeSq_ = (timeHorizonObst_ * maxSpeed_ + radius_) * (timeHorizonObst_ * maxSpeed_ + radius_);
}
void Agent::insertAgentNeighbor(Agent* other)
sim_(sim)
{
if(other != this)
{
VEC3 pop = part_.m_position - other->part_.m_position;
float distSq = pop.norm2();
newAgentNeighbors_.insert(std::make_pair(distSq, other));
// neighborsIt_[other] = agentNeighbors_.insert(std::make_pair(distSq, other));
}
rangeSq_ = (timeHorizonObst_ * maxSpeed_ + radius_) * (timeHorizonObst_ * maxSpeed_ + radius_);
}
void Agent::insertAgentNeighbor(Agent* other, float distSq)
bool agentSort(const std::pair<float, Agent*>& a1, const std::pair<float, Agent*>& a2)
{
newAgentNeighbors_.insert(std::make_pair(distSq, other));
// neighborsIt_[other] = agentNeighbors_.insert(std::make_pair(distSq, other));
return a1.first < a2.first;
}
void Agent::removeAgentNeighbor(Agent* other)
bool obstacleSort(const std::pair<float, Dart>& o1, const std::pair<float, Dart>& o2)
{
// agentNeighbors_.erase(neighborsIt_[other]);
// neighborsIt_.erase(other);
return o1.first < o2.first;
}
void Agent::updateAgentNeighbors()
{
for(std::multimap<float, Agent*>::iterator it = agentNeighbors_.begin(); it != agentNeighbors_.end(); ++it)
std::vector<Agent*>& agents = sim_->envMap_.agentvect[part_.d];
std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d];
agentNeighbors_.clear();
for(std::vector<Agent*>::iterator it = agents.begin(); it != agents.end(); ++it)
{
Agent* other = it->second;
VEC3 pop = part_.m_position - other->part_.m_position;
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
newAgentNeighbors_.insert(std::make_pair(distSq, other));
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
for(std::vector<Agent*>::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
{
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
newAgentNeighbors_.swap(agentNeighbors_);
newAgentNeighbors_.clear();
}
void Agent::clearAgentNeighbors()
{
agentNeighbors_.clear();
// neighborsIt_.clear();
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
}
void Agent::insertObstacleNeighbor(Dart d)
void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment(sim_->envMap_.position[d], sim_->envMap_.position[sim_->envMap_.map.phi1(d)], part_.m_position);
newObstacleNeighbors_.insert(std::make_pair(distSq, d));
}
std::vector<Dart>& obst = sim_->envMap_.obstvect[part_.d];
void Agent::removeObstacleNeighbor(Dart d)
{
}
obstacleNeighbors_.clear();
void Agent::updateObstacleNeighbors()
{
for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin(); it != obstacleNeighbors_.end(); ++it)
for(std::vector<Dart>::iterator it = obst.begin(); it != obst.end(); ++it)
{
Dart d = it->second;
float distSq = distSqPointLineSegment(sim_->envMap_.position[d], sim_->envMap_.position[sim_->envMap_.map.phi1(d)], part_.m_position);
newObstacleNeighbors_.insert(std::make_pair(distSq, d));
float distSq = distSqPointLineSegment(sim_->envMap_.position[*it], sim_->envMap_.position[sim_->envMap_.map.phi1(*it)], part_.m_position);
obstacleNeighbors_.push_back(std::make_pair(distSq, *it));
}
newObstacleNeighbors_.swap(obstacleNeighbors_);
newObstacleNeighbors_.clear();
}
void Agent::clearObstacleNeighbors()
{
obstacleNeighbors_.clear();
std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort);
}
void Agent::update()
......@@ -95,8 +73,6 @@ void Agent::update()
VEC3 target = part_.m_position + (velocity_ * sim_->timeStep_);
part_.move(target);
treated = false;
}
void Agent::computePrefVelocity()
......@@ -129,7 +105,8 @@ void Agent::computeNewVelocity()
std::vector<Line> orcaLines_;
/* Create obstacle ORCA lines. */
for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin();
// for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin();
for(std::vector<std::pair<float, Dart> >::iterator it = obstacleNeighbors_.begin();
it != obstacleNeighbors_.end() && it->first < rangeSq_;
++it)
{
......@@ -353,7 +330,8 @@ void Agent::computeNewVelocity()
/* Create agent ORCA lines. */
unsigned int nbA = 0;
for(std::multimap<float, Agent*>::iterator it = agentNeighbors_.begin();
// for(std::multimap<float, Agent*>::iterator it = agentNeighbors_.begin();
for(std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin();
it != agentNeighbors_.end() && it->first < neighborDistSq_ && nbA < maxNeighbors_;
++it, ++nbA)
{
......
src/env_map.cpp
View file @ b3b10bd1
#include "env_map.h"
#include "Geometry/inclusion.h"
#include "utils.h"
#include "agent.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"
......@@ -14,6 +14,7 @@ EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL)
position = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "position");
normal = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "normal");
agentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "agents");
neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "neighborAgents");
obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE_ORBIT, "obstacles");
}
......@@ -42,11 +43,26 @@ void EnvMap::init()
float sideSize = 70.0f;
unsigned int nbSquares = 25;
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
// CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map.init();
registerObstaclesInFaces();
}
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
Dart dd = d;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
f(ddd);
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
}
void EnvMap::registerObstaclesInFaces()
{
CellMarker m(map, FACE_CELL);
......@@ -66,16 +82,13 @@ void EnvMap::registerObstaclesInFaces()
do
{
Dart ddd = map.alpha1(dd);
do
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
if(!buildingMark.isMarked(ddd))
addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
ddd = map.alpha1(ddd);
if(ddd != dd)
{
if(!buildingMark.isMarked(ddd))
addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
}
} while(ddd != dd);
}
dd = map.phi1(dd);
} while(dd != d);
}
......@@ -104,59 +117,88 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
} while(dd != d);
}
void EnvMap::setAgentNeighborsAndObstacles(Agent* agent)
void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
{
agent->clearAgentNeighbors();
agent->clearObstacleNeighbors();
Dart d = agent->part_.d;
PFP::OBSTACLEVECT::iterator end = obstvect[d].end();
for(PFP::OBSTACLEVECT::iterator it = obstvect[d].begin(); it != end; ++it)
agent->insertObstacleNeighbor(*it);
// add agents of current face
addNeighborAgents(agent, d);
// add obstacles and agents of connex faces
Dart dd = d;
Dart oldFace = agent->part_.lastCrossed;
Dart newFace = agent->part_.d;
agentvect[newFace].push_back(agent);
PFP::AGENTS::iterator end = agentvect[oldFace].end();
for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
{
if(*it == agent)
{
*it = agentvect[oldFace].back();
agentvect[oldFace].pop_back();
break;
}
}
// mark adjacent cells shared by newFace and oldFace
CellMarkerNoUnmark f(map, FACE_CELL);
Dart d = oldFace;
do
{
f.mark(d);
d = map.alpha1(d);
} while(d != oldFace);
d = map.phi1(oldFace);
do
{
Dart ddd = map.alpha1(dd);
do
f.mark(d);
d = map.alpha1(d);
} while(d != map.phi1(oldFace));
// remove agent from cells adjacent to oldFace but not to newFace
Dart dd = oldFace;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
if(!f.isMarked(ddd))
{
bool found = false;
PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
{
if(*it == agent)
{
*it = neighborAgentvect[ddd].back();
neighborAgentvect[ddd].pop_back();
found = true;
}
}
}
ddd = map.alpha1(ddd);
if(ddd != dd)
addNeighborAgents(agent, ddd);
} while(ddd != dd);
}
dd = map.phi1(dd);
} while(dd != d);
}
} while(dd != oldFace);
void EnvMap::addNeighborAgents(Agent* agent, Dart d)
{
PFP::AGENTS& agents = agentvect[d];
for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
agent->insertAgentNeighbor(*it);
}
/*
void EnvMap::agentChangeFaceThroughEdge(Agent* agent, Dart oldFace)
{
Dart newFace = agent->part_.d;
popAgentInCell(agent, oldFace);
pushAgentInCell(agent, newFace);
CellMarker f(map, FACE_CELL);
// add agent from cells adjacent to oldFace but not to newFace
// and unmark shared cells
dd = newFace;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
if(!f.isMarked(ddd))
neighborAgentvect[ddd].push_back(agent);
else
f.unmark(ddd);
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != newFace);
}
*/
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
Dart newFace = agent->part_.d;
popAgentInCell(agent, oldFace);
pushAgentInCell(agent, newFace);
setAgentNeighborsAndObstacles(agent);
popAgentInCells(agent, oldFace);
pushAgentInCells(agent, agent->part_.d);
}
void EnvMap::updateMap()
......@@ -172,18 +214,62 @@ void EnvMap::updateMap()
if(agentvect[old].size() > nbAgentsToSubdivide)
{
PFP::AGENTS agents ;
agents.swap(agentvect[old]) ;
PFP::AGENTS oldAgents(agentvect[old]);
for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
popAgentInCells(*ait, old);
neighborAgentvect[old].clear() ;
map.setCurrentLevel(fLevel) ;
CellMarkerStore newF(map, FACE_CELL);
newF.mark(old);
Algo::IHM::subdivideFace<PFP>(map, old, position) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::AGENTS::iterator ait = agents.begin(); ait != agents.end(); ++ait)
for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
{
resetAgentInFace(*ait) ;
agentvect[(*ait)->part_.d].push_back(*ait) ;
pushAgentInCells(*ait, (*ait)->part_.d) ;
}
unsigned int degree = 0;
Dart dd = old;
do
{
++degree;
Dart d3 = dd;
do
{
Dart d4 = map.alpha1(map.alpha1(d3));
while(d4 != d3)
{
if(!newF.isMarked(d4))
neighborAgentvect[d3].insert(neighborAgentvect[d3].end(), agentvect[d4].begin(), agentvect[d4].end());
d4 = map.alpha1(d4);
}
d3 = map.phi1(d3);
} while(d3 != dd);
map.setCurrentLevel(fLevel) ;
dd = map.phi1(dd);
map.setCurrentLevel(map.getMaxLevel()) ;
} while(dd != old);
if(degree == 3)
{
Dart centerFace = map.phi2(map.phi1(old));
Dart d3 = centerFace;
do
{
Dart d4 = map.alpha1(map.alpha1(d3));
while(d4 != d3)
{
if(!newF.isMarked(d4))
neighborAgentvect[d3].insert(neighborAgentvect[d3].end(), agentvect[d4].begin(), agentvect[d4].end());
d4 = map.alpha1(d4);
}
d3 = map.phi1(d3);
} while(d3 != centerFace);
}
}
}
......@@ -208,8 +294,7 @@ void EnvMap::updateMap()
Dart fit = old ;
do
{
unsigned nbA = agentvect[fit].size() ;
nbAgents += nbA ;
nbAgents += agentvect[fit].size() ;
++degree ;
fit = map.phi1(fit) ;
} while(fit != old) ;
......@@ -231,9 +316,11 @@ void EnvMap::updateMap()
unsigned int fEmb = map.getEmbedding(fit, FACE_ORBIT) ;
if(fEmb != face)
coarsenCandidate.erase(fEmb) ;
PFP::AGENTS& a = agentvect[fit] ;
if(a.size() > 0)
agents.insert(agents.end(), a.begin(), a.end()) ;
PFP::AGENTS a(agentvect[fit]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::AGENTS::iterator it = a.begin(); it != a.end(); ++it)