subdiv ok.. perf++

include/agent.h

@@ -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

@@ -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

@@ -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

@@ -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

 #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);