Commit b3b10bd1 authored by Pierre Kraemer's avatar Pierre Kraemer

subdiv ok.. perf++

parent b8f34fe9
...@@ -14,21 +14,12 @@ class Agent ...@@ -14,21 +14,12 @@ class Agent
public: public:
Agent(Simulator* sim, const VEC3& position, Dart d); 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 updateAgentNeighbors();
void clearAgentNeighbors();
void insertObstacleNeighbor(Dart d);
void removeObstacleNeighbor(Dart d);
void updateObstacleNeighbors(); void updateObstacleNeighbors();
void clearObstacleNeighbors();
void update(); void update();
void computePrefVelocity(); void computePrefVelocity();
void computeNewVelocity(); void computeNewVelocity();
bool linearProgram1( bool linearProgram1(
...@@ -47,14 +38,9 @@ public: ...@@ -47,14 +38,9 @@ public:
const std::vector<Line>& lines, unsigned int numObstLines, const std::vector<Line>& lines, unsigned int numObstLines,
float radius, VEC3& result 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::vector<std::pair<float, Agent*> > agentNeighbors_;
std::vector<std::pair<float, Dart> > obstacleNeighbors_;
std::multimap<float, Dart> obstacleNeighbors_;
std::multimap<float, Dart> newObstacleNeighbors_;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_; CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_;
...@@ -74,8 +60,6 @@ public: ...@@ -74,8 +60,6 @@ public:
VEC3 prefVelocity_; VEC3 prefVelocity_;
Simulator* sim_; Simulator* sim_;
bool treated;
}; };
#endif #endif
......
...@@ -54,18 +54,18 @@ public : ...@@ -54,18 +54,18 @@ public :
void init(); void init();
void foreach_neighborFace(Dart d, FunctorType& f);
void registerObstaclesInFaces(); void registerObstaclesInFaces();
void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor); void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor);
void setAgentNeighborsAndObstacles(Agent* agent); 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 agentChangeFace(Agent* agent, Dart oldFace);
void pushAgentInCell(Agent* agent, Dart d); void pushAgentInCells(Agent* agent, Dart d);
void popAgentInCell(Agent* agent, Dart d); void popAgentInCells(Agent* agent, Dart d);
void addRefineCandidate(Dart d); void addRefineCandidate(Dart d);
void addCoarsenCandidate(Dart d); void addCoarsenCandidate(Dart d);
...@@ -81,13 +81,15 @@ public : ...@@ -81,13 +81,15 @@ public :
PFP::TVEC3 normal; PFP::TVEC3 normal;
PFP::TAB_AGENTVECT agentvect; PFP::TAB_AGENTVECT agentvect;
PFP::TAB_AGENTVECT neighborAgentvect;
PFP::TAB_OBSTACLEVECT obstvect; PFP::TAB_OBSTACLEVECT obstvect;
CellMarker obstacleMark; CellMarker obstacleMark;
CellMarker buildingMark; CellMarker buildingMark;
static const unsigned int nbAgentsToSubdivide = 12; 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> refineCandidate;
std::map<unsigned int, Dart> coarsenCandidate; std::map<unsigned int, Dart> coarsenCandidate;
...@@ -97,31 +99,64 @@ public : ...@@ -97,31 +99,64 @@ public :
* INLINE FUNCTIONS * * INLINE FUNCTIONS *
**************************************/ **************************************/
inline void EnvMap::pushAgentInCell(Agent* agent, Dart d) inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{ {
assert(map.getCurrentLevel() == map.getMaxLevel()); assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end()); assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
agentvect[d].push_back(agent); 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(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end()); assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end());
bool found = false;
PFP::AGENTS::iterator end = agentvect[d].end(); 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) if(*it == agent)
{ {
*it = agentvect[d].back(); *it = agentvect[d].back();
agentvect[d].pop_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) inline void EnvMap::addRefineCandidate(Dart d)
...@@ -141,4 +176,3 @@ inline void EnvMap::clearUpdateCandidates() ...@@ -141,4 +176,3 @@ inline void EnvMap::clearUpdateCandidates()
} }
#endif #endif
...@@ -49,9 +49,9 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD ...@@ -49,9 +49,9 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glLineWidth(1.0f); glLineWidth(1.0f);
glColor3f(1.0f,0.0f,0.0f); glColor3f(1.0f,0.0f,0.0f);
glBegin(GL_LINE_LOOP); glBegin(GL_POLYGON);
for(unsigned int i = 0; i < 5; ++i) 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(); glEnd();
if(showNeighborDist) if(showNeighborDist)
...@@ -60,7 +60,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD ...@@ -60,7 +60,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,1.0f,0.0f); glColor3f(0.0f,1.0f,0.0f);
glBegin(GL_LINE_LOOP); glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i) 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(); glEnd();
} }
...@@ -70,7 +70,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD ...@@ -70,7 +70,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,0.0f,1.0f); glColor3f(0.0f,0.0f,1.0f);
glBegin(GL_LINE_LOOP); glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i) 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(); glEnd();
} }
} }
......
...@@ -13,79 +13,57 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : ...@@ -13,79 +13,57 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
velocity_(0), velocity_(0),
newVelocity_(0), newVelocity_(0),
prefVelocity_(0), prefVelocity_(0),
sim_(sim), sim_(sim)
treated(false)
{
rangeSq_ = (timeHorizonObst_ * maxSpeed_ + radius_) * (timeHorizonObst_ * maxSpeed_ + radius_);
}
void Agent::insertAgentNeighbor(Agent* other)
{ {
if(other != this) rangeSq_ = (timeHorizonObst_ * maxSpeed_ + radius_) * (timeHorizonObst_ * maxSpeed_ + radius_);
{
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));
}
} }
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)); return a1.first < a2.first;
// neighborsIt_[other] = agentNeighbors_.insert(std::make_pair(distSq, other));
} }
void Agent::removeAgentNeighbor(Agent* other) bool obstacleSort(const std::pair<float, Dart>& o1, const std::pair<float, Dart>& o2)
{ {
// agentNeighbors_.erase(neighborsIt_[other]); return o1.first < o2.first;
// neighborsIt_.erase(other);
} }
void Agent::updateAgentNeighbors() 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 - (*it)->part_.m_position;
VEC3 pop = part_.m_position - other->part_.m_position;
float distSq = pop.norm2(); 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() std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
{
agentNeighbors_.clear();
// neighborsIt_.clear();
} }
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); std::vector<Dart>& obst = sim_->envMap_.obstvect[part_.d];
newObstacleNeighbors_.insert(std::make_pair(distSq, d));
}
void Agent::removeObstacleNeighbor(Dart d) obstacleNeighbors_.clear();
{
}
void Agent::updateObstacleNeighbors() for(std::vector<Dart>::iterator it = obst.begin(); it != obst.end(); ++it)
{
for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin(); it != obstacleNeighbors_.end(); ++it)
{ {
Dart d = it->second; float distSq = distSqPointLineSegment(sim_->envMap_.position[*it], sim_->envMap_.position[sim_->envMap_.map.phi1(*it)], part_.m_position);
float distSq = distSqPointLineSegment(sim_->envMap_.position[d], sim_->envMap_.position[sim_->envMap_.map.phi1(d)], part_.m_position); obstacleNeighbors_.push_back(std::make_pair(distSq, *it));
newObstacleNeighbors_.insert(std::make_pair(distSq, d));
} }
newObstacleNeighbors_.swap(obstacleNeighbors_);
newObstacleNeighbors_.clear();
}
void Agent::clearObstacleNeighbors() std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort);
{
obstacleNeighbors_.clear();
} }
void Agent::update() void Agent::update()
...@@ -95,8 +73,6 @@ void Agent::update() ...@@ -95,8 +73,6 @@ void Agent::update()
VEC3 target = part_.m_position + (velocity_ * sim_->timeStep_); VEC3 target = part_.m_position + (velocity_ * sim_->timeStep_);
part_.move(target); part_.move(target);
treated = false;
} }
void Agent::computePrefVelocity() void Agent::computePrefVelocity()
...@@ -129,7 +105,8 @@ void Agent::computeNewVelocity() ...@@ -129,7 +105,8 @@ void Agent::computeNewVelocity()
std::vector<Line> orcaLines_; std::vector<Line> orcaLines_;
/* Create obstacle ORCA lines. */ /* 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 != obstacleNeighbors_.end() && it->first < rangeSq_;
++it) ++it)
{ {
...@@ -353,7 +330,8 @@ void Agent::computeNewVelocity() ...@@ -353,7 +330,8 @@ void Agent::computeNewVelocity()
/* Create agent ORCA lines. */ /* Create agent ORCA lines. */
unsigned int nbA = 0; 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 != agentNeighbors_.end() && it->first < neighborDistSq_ && nbA < maxNeighbors_;
++it, ++nbA) ++it, ++nbA)
{ {
......
#include "env_map.h" #include "env_map.h"
#include "Geometry/inclusion.h"
#include "utils.h" #include "utils.h"
#include "agent.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/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h" #include "Algo/Geometry/normal.h"
...@@ -14,6 +14,7 @@ EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL) ...@@ -14,6 +14,7 @@ EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL)
position = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "position"); position = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "position");
normal = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "normal"); normal = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "normal");
agentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "agents"); agentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "agents");
neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "neighborAgents");
obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE_ORBIT, "obstacles"); obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE_ORBIT, "obstacles");
} }
...@@ -42,11 +43,26 @@ void EnvMap::init() ...@@ -42,11 +43,26 @@ void EnvMap::init()
float sideSize = 70.0f; float sideSize = 70.0f;
unsigned int nbSquares = 25; unsigned int nbSquares = 25;
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares); 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(); map.init();
registerObstaclesInFaces(); 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() void EnvMap::registerObstaclesInFaces()
{ {
CellMarker m(map, FACE_CELL); CellMarker m(map, FACE_CELL);
...@@ -66,16 +82,13 @@ void EnvMap::registerObstaclesInFaces() ...@@ -66,16 +82,13 @@ void EnvMap::registerObstaclesInFaces()
do do
{ {
Dart ddd = map.alpha1(dd); Dart ddd = map.alpha1(map.alpha1(dd));
do while(ddd != dd)
{ {
if(!buildingMark.isMarked(ddd))
addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
ddd = map.alpha1(ddd); 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); dd = map.phi1(dd);
} while(dd != d); } while(dd != d);
} }
...@@ -104,59 +117,88 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo ...@@ -104,59 +117,88 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
} while(dd != d); } while(dd != d);
} }
void EnvMap::setAgentNeighborsAndObstacles(Agent* agent) void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
{ {
agent->clearAgentNeighbors(); Dart oldFace = agent->part_.lastCrossed;
agent->clearObstacleNeighbors(); Dart newFace = agent->part_.d;
Dart d = agent->part_.d; agentvect[newFace].push_back(agent);
PFP::OBSTACLEVECT::iterator end = obstvect[d].end(); PFP::AGENTS::iterator end = agentvect[oldFace].end();
for(PFP::OBSTACLEVECT::iterator it = obstvect[d].begin(); it != end; ++it) for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
agent->insertObstacleNeighbor(*it); {
if(*it == agent)
// add agents of current face {
addNeighborAgents(agent, d); *it = agentvect[oldFace].back();
agentvect[oldFace].pop_back();
// add obstacles and agents of connex faces break;
Dart dd = d; }
}
// 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 do
{ {
Dart ddd = map.alpha1(dd); f.mark(d);
do 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); ddd = map.alpha1(ddd);
if(ddd != dd) }
addNeighborAgents(agent, ddd);
} while(ddd != dd);
dd = map.phi1(dd); dd = map.phi1(dd);