bla bli blu

CMakeLists.txt

+cmake_minimum_required(VERSION 2.6)
+
+project(SocialAgents)
+
+# a decommenter si on veut voir ce qui se passe a la compilation
+#set( CMAKE_VERBOSE_MAKEFILE 1 )
+
+SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN)
+
+SET(CMAKE_MODULE_PATH " ${CMAKE_MODULE_PATH} ${CGoGN_ROOT_DIR}/cmake_modules/")
+
+find_package(OpenGL)
+find_package(GLUT)
+
+# qq definition specifiques pour mac
+IF(APPLE)
+# attention a changer pour chercher la bonne version automatiquement
+	SET(CMAKE_OSX_SYSROOT "/Developer/SDKs/MacOSX10.6.sdk")
+	SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined -Wl,dynamic_lookup")
+ENDIF(APPLE)
+
+IF(WIN32)
+	SET(GLEW_LIBRARY glew32)
+	SET(DEVIL_LIBRARIES DevIL ILU ILUT)
+	SET(ZLIB_LIBRARIES zlib)
+	SET(LIBXML2_LIBRARIES xml2)
+	INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/dependencies/include/)
+	LINK_DIRECTORIES(${CMAKE_BINARY_DIR}/dependencies/lib/)
+ELSE(WIN32)
+	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
+	find_package(DevIL)
+	find_package(ZLIB)
+	find_package(LibXml2)
+	SET(GLEW_LIBRARY GLEW)
+	IF (IL_LIBRARIES)
+		SET(DEVIL_LIBRARIES ${IL_LIBRARIES} ${ILU_LIBRARIES} ${ILUT_LIBRARIES})
+	ELSE(IL_LIBRARIES)
+		SET(DEVIL_LIBRARIES ${IL_LIBRARY} ${ILU_LIBRARY} ${ILUT_LIBRARY})
+	ENDIF(IL_LIBRARIES)
+ENDIF(WIN32)
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)
+
+SET (COMMON_LIBS ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${GLEW_LIBRARY} ${DEVIL_LIBRARIES} ${ZLIB_LIBRARIES} ${LIBXML2_LIBRARIES} gzstream AntTweakBar )
+
+add_subdirectory(Release)
+add_subdirectory(Debug)
+

Debug/CMakeLists.txt

+cmake_minimum_required(VERSION 2.6)
+
+project(SocialAgents)
+
+SET(CMAKE_BUILD_TYPE Debug)
+
+#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg")
+
+link_directories(
+	${CGoGN_ROOT_DIR}/lib/Debug/
+	${CGoGN_ROOT_DIR}/lib/Release/
+)
+
+# define includes path
+include_directories(
+	/usr/include/libxml2/
+	../include
+	${CGoGN_ROOT_DIR}/include
+	${CGoGN_ROOT_DIR}/ThirdParty/Numerical
+	${CGoGN_ROOT_DIR}/ThirdParty/Numerical/UFconfig
+	${CGoGN_ROOT_DIR}/ThirdParty/AntTweakBar/include
+	${CGoGN_ROOT_DIR}/ThirdParty/gzstream
+	${CGoGN_ROOT_DIR}/ThirdParty/Zinri
+	${CGoGN_ROOT_DIR}/ThirdParty/OpenCTM
+	${CGoGN_ROOT_DIR}/ThirdParty/Assimp/include
+	${CGoGN_ROOT_DIR}/ThirdParty/glm
+)
+
+#define exec to compile
+add_executable( socialAgentsD
+	../src/viewer.cpp
+	../src/env_map.cpp
+	../src/agent.cpp
+	../src/simulator.cpp
+)
+
+target_link_libraries( socialAgentsD
+	containerD topologyD utilsD algoD ${COMMON_LIBS} boost_thread-mt)

Release/CMakeLists.txt

+cmake_minimum_required(VERSION 2.6)
+
+project(SocialAgents)
+
+SET(CMAKE_BUILD_TYPE Release)
+
+link_directories(
+	${CGoGN_ROOT_DIR}/lib/Release/
+)
+
+# define includes path
+include_directories(
+	/usr/include/libxml2/
+	../include
+	${CGoGN_ROOT_DIR}/include
+	${CGoGN_ROOT_DIR}/ThirdParty/Numerical
+	${CGoGN_ROOT_DIR}/ThirdParty/Numerical/UFconfig
+	${CGoGN_ROOT_DIR}/ThirdParty/AntTweakBar/include
+	${CGoGN_ROOT_DIR}/ThirdParty/gzstream
+	${CGoGN_ROOT_DIR}/ThirdParty/Zinri
+	${CGoGN_ROOT_DIR}/ThirdParty/OpenCTM
+	${CGoGN_ROOT_DIR}/ThirdParty/Assimp/include
+	${CGoGN_ROOT_DIR}/ThirdParty/glm
+)
+
+#define exec to compile
+add_executable( socialAgents
+	../src/viewer.cpp
+	../src/env_map.cpp
+	../src/agent.cpp
+	../src/simulator.cpp
+)
+
+target_link_libraries( socialAgents
+	container topology utils algo ${COMMON_LIBS} boost_thread-mt)

include/agent.h

+#ifndef M_AGENT_H
+#define M_AGENT_H
+
+#include <iostream>
+
+#include "utils.h"
+#include "env_map.h"
+#include "Algo/MovingObjects/particle_cell_2D.h"
+
+class Simulator;
+
+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(
+		const std::vector<Line>& lines, unsigned int lineNo,
+		float radius, const VEC3& optVelocity,
+		bool directionOpt, VEC3& result
+	);
+
+	bool linearProgram2(
+		const std::vector<Line>& lines, unsigned int num, float radius,
+		const VEC3& optVelocity, bool directionOpt,
+		VEC3& result
+	);
+
+	void linearProgram3(
+		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_;
+	
+	CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_;
+	
+	std::vector<VEC3> goals_;
+	unsigned int curGoal_;
+	
+	size_t maxNeighbors_;
+	float maxSpeed_;
+	float neighborDistSq_;
+	float radius_;
+	float timeHorizon_;
+	float timeHorizonObst_;
+	float rangeSq_;
+	
+	VEC3 velocity_;
+	VEC3 newVelocity_;
+	VEC3 prefVelocity_;
+	
+	Simulator* sim_;
+	
+	bool treated;
+};
+
+#endif
+

include/env_generator.h

+#ifndef ENV_GENERATOR
+#define ENV_GENERATOR
+
+#include "Container/fakeAttribute.h"
+
+namespace CGoGN
+{
+
+namespace CityGenerator
+{
+
+template <typename PFP>
+bool notDiagonalAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, CellMarker& buildingMark);
+
+template <typename PFP>
+Algo::Modelisation::Polyhedron<PFP> generateGrid(typename PFP::MAP& map, typename PFP::TVEC3& position, unsigned int cX, unsigned int cY, float sideLength, CellMarker& obstacleMark, CellMarker& buildingMark);
+
+template <typename PFP>
+Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& buildingMark, float height);
+
+template <typename PFP>
+void generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark);
+
+template <typename PFP>
+void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize, unsigned int nbSquares);
+
+
+
+
+template <typename PFP>
+void simplifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position);
+
+template <typename PFP>
+bool isAnEar(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart dd, float& area);
+
+template <typename PFP>
+bool canRemoveEdgeConvex(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& obstacleMark);
+
+template <typename PFP>
+void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark);
+
+}
+
+}
+
+#include "env_generator.hpp"
+#endif
+

include/env_map.h

+#ifndef ENVMAP_H
+#define ENVMAP_H
+
+#include <iostream>
+#include <algorithm>
+
+#include "Topology/generic/parameters.h"
+#include "Topology/map/map2.h"
+#include "Topology/generic/embeddedMap2.h"
+
+#include "Algo/ImplicitHierarchicalMesh/ihm.h"
+#include "Algo/ImplicitHierarchicalMesh/subdivision.h"
+
+#include "Algo/Modelisation/polyhedron.h"
+#include "Algo/Modelisation/extrusion.h"
+#include "Algo/Modelisation/subdivision.h"
+#include "Algo/Geometry/centroid.h"
+#include "Algo/Geometry/area.h"
+
+#include "Container/fakeAttribute.h"
+
+#include "env_generator.h"
+
+#include "Algo/Parallel/parallel_foreach.h"
+
+using namespace CGoGN;
+
+class Agent;
+
+struct PFP: public PFP_STANDARD
+{
+	// definition de la carte
+// 	typedef EmbeddedMap2<Map2> MAP;
+ 	typedef Algo::IHM::ImplicitHierarchicalMap MAP;
+
+	// definition des listes d'agent
+	typedef std::vector<Agent*> AGENTS;
+	typedef std::vector<Dart> OBSTACLES;
+	typedef NoMathIONameAttribute<AGENTS> AGENTVECT;
+	typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT;
+	typedef AttributeHandler<AGENTVECT> TAB_AGENTVECT;
+	typedef AttributeHandler<OBSTACLEVECT> TAB_OBSTACLEVECT;
+};
+
+typedef PFP::VEC3 VEC3;
+
+class EnvMap
+{
+public :
+	EnvMap();
+	
+	unsigned int mapMemoryCost();
+	Dart getBelongingCell(const PFP::VEC3& pos);
+	
+	void init();
+	
+	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 agentChangeFace(Agent* agent, Dart oldFace);
+
+	void pushAgentInCell(Agent* agent, Dart d);
+	void popAgentInCell(Agent* agent, Dart d);
+	
+	void addRefineCandidate(Dart d);
+	void addCoarsenCandidate(Dart d);
+	void clearUpdateCandidates();
+	
+	void updateMap();
+
+	void resetAgentInFace(Agent* agent);
+	
+	PFP::MAP map;
+
+	PFP::TVEC3 position;
+	PFP::TVEC3 normal;
+
+	PFP::TAB_AGENTVECT agentvect;
+	PFP::TAB_OBSTACLEVECT obstvect;
+
+	CellMarker obstacleMark;
+	CellMarker buildingMark;
+
+	static const unsigned int nbAgentsToSubdivide = 12;
+	static const unsigned int nbAgentsToSimplify = 8;
+
+	std::map<unsigned int, Dart> refineCandidate;
+	std::map<unsigned int, Dart> coarsenCandidate;
+};
+
+/**************************************
+*           INLINE FUNCTIONS          *
+**************************************/
+
+inline void EnvMap::pushAgentInCell(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);
+}
+
+inline void EnvMap::popAgentInCell(Agent* agent, Dart d)
+{
+	assert(map.getCurrentLevel() == map.getMaxLevel());
+	assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end());
+	
+	PFP::AGENTS::iterator end = agentvect[d].end();
+	for(PFP::AGENTS::iterator it = agentvect[d].begin(); it != end; ++it)
+	{
+		if(*it == agent)
+		{
+			*it = agentvect[d].back();
+			agentvect[d].pop_back();
+			break;
+		}
+	}
+	addCoarsenCandidate(d);
+}
+
+inline void EnvMap::addRefineCandidate(Dart d)
+{
+	refineCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(d, FACE_ORBIT), d)) ;
+}
+
+inline void EnvMap::addCoarsenCandidate(Dart d)
+{
+	coarsenCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(d, FACE_ORBIT), d)) ;
+}
+
+inline void EnvMap::clearUpdateCandidates()
+{
+	refineCandidate.clear();
+	coarsenCandidate.clear();
+}
+
+#endif
+

include/env_render.h

+#ifndef ENVMAP_RENDER
+#define ENVMAP_RENDER
+
+#include "env_map.h"
+#include "agent.h"
+
+#include <GL/glut.h>
+
+void renderDart(EnvMap& m, Dart d)
+{
+	PFP::VEC3 p1 = m.position[d];
+	PFP::VEC3 p2 = m.position[m.map.phi1(d)];
+
+	glBegin(GL_LINES);
+		glVertex3f(p1[0],p1[1],p1[2]);
+		glVertex3f(p2[0],p2[1],p2[2]);
+	glEnd();
+}
+
+void renderFace(EnvMap& m, Dart& d)
+{
+	Dart dd=d;
+	do {
+		renderDart(m,dd);
+		dd = m.map.phi1(dd);
+	}while(dd!=d);
+}
+
+void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
+{
+	VEC3 pos1(m.position[d]);
+	VEC3 pos2(m.position[m.map.phi1(d)]);
+	pos2[2] += 2;
+	glBegin(GL_LINE_LOOP);
+		glVertex3fv(&p[0]);
+		glVertex3fv(& pos1[0]);
+		glVertex3fv(& pos2[0]);
+	glEnd();
+}
+
+static const float cosT[5] = { 1, 0.309017, -0.809017, -0.809017, 0.309017 };
+static const float sinT[5] = { 0, 0.951057, 0.587785, -0.587785, -0.951057 };
+
+void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleDist = false)
+{
+	VEC3& pos = agent->part_.m_position;
+	float radius = agent->radius_;
+	
+	glLineWidth(1.0f);
+	
+	glColor3f(1.0f,0.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));
+	glEnd();
+	
+	if(showNeighborDist)
+	{
+		radius = sqrt(agent->neighborDistSq_);
+		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));
+		glEnd();
+	}
+	
+	if(showObstacleDist)
+	{
+		radius = (agent->timeHorizonObst_ * agent->maxSpeed_) + agent->radius_;
+		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));
+		glEnd();
+	}
+}
+
+#endif
+

include/path_finder.h

+#ifndef PATH_FINDER
+#define PATH_FINDER
+
+namespace CGoGN
+{
+
+namespace PathFinder
+{
+
+	template <typename PFP>
+	float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos, Dart stopPos);
+
+	template <typename PFP>
+	std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad = FunctorType());
+
+}
+
+}
+
+#include "path_finder.hpp"
+#endif

include/path_finder.hpp

+#include "Algo/Geometry/centroid.h"
+#include <limits>
+#include <iostream>
+
+namespace CGoGN
+{
+
+namespace PathFinder
+{
+
+
+template <typename PFP>
+float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos)
+{
+	return VEC3(Algo::Geometry::faceCentroid<PFP>(map,stopPos,position)-Algo::Geometry::faceCentroid<PFP>(map,startPos,position)).norm2();
+}
+
+template <typename PFP>
+std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad)
+{
+	std::vector<Dart> path;
+
+	AutoAttributeHandler< Dart > tablePred(map,FACE_ORBIT);
+	std::vector<std::pair<float,Dart> > vToDev;
+
+	Dart toDev = stopPos;
+	vToDev.push_back(std::make_pair(0, toDev));
+	tablePred[toDev] = toDev;
+	do {
+		//dev cell
+		//get all vertex-adjacent cells 
+		Dart dd = toDev;
+		do {
+			Dart ddd = map.alpha1(dd);
+			do {
+				ddd = map.alpha1(ddd);
+				if(ddd!=dd) {
+					if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) {
+						//evaluate their cost and push them in the vector to dev
+						if(tablePred[ddd]==EMBNULL)
+							tablePred[ddd]=toDev;
+						std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin();
+						float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
+						while(it!=vToDev.end() && (*it).first<costDDD)
+							++it;
+						vToDev.insert(it, std::make_pair(costDDD, ddd));
+					}
+				}
+			} while(ddd!=dd);
+			dd = map.phi1(dd);
+		} while(dd!=toDev);
+
+		//remove developped cell and get new cell to dev
+		if(vToDev.size()>0) {
+			toDev = (vToDev.begin())->second;
+			vToDev.erase(vToDev.begin());
+		}
+	} while(vToDev.size()>0 && !map.sameOrientedFace(startPos,toDev));
+
+	//if path found : from start to stop -> push all predecessors
+	if(map.sameOrientedFace(startPos,toDev))
+	{
+		std::cout << "found" << std::endl;
+		Dart toPush=startPos;
+		std::cout << tablePred[startPos] << std::endl;
+		do {
+			path.push_back(toPush);
+			toPush = tablePred[toPush];
+		} while(!map.sameOrientedFace(toPush,stopPos));
+	}
+
+	return path;
+}
+
+//namespace
+}
+
+}

include/simulator.h

+#ifndef SIMULATOR_H
+#define SIMULATOR_H
+
+#include <limits>
+
+#include "env_map.h"
+#include "agent.h"
+#include "path_finder.h"
+
+#include <iostream>
+#include <vector>
+#include <fstream>
+#include <sstream>
+
+class Simulator
+{
+public:
+	Simulator();
+	
+	Simulator(
+		float timeStep, float neighborDist, unsigned int maxNeighbors,
+		float timeHorizon, float timeHorizonObst, float radius,
+		float maxSpeed, const VEC3& velocity = VEC3(0)
+	);
+	
+	~Simulator();
+	
+	void doStep();
+	
+	void setupScenario();
+//	void addPathsToAgents();
+	
+	bool importAgents(std::string filename);
+	bool exportAgents(std::string filename);
+	
+	void swapAgentsGoals();
+	
+	EnvMap envMap_;
+	std::vector<Agent*> agents_;
+	
+	float globalTime_;
+	float timeStep_;
+};
+
+#endif
+

include/utils.h

+#ifndef UTILS_H
+#define UTILS_H
+
+#include <cmath>
+#include <limits>
+#include <vector>
+
+#include "Geometry/vector_gen.h"
+
+typedef CGoGN::Geom::Vec3f VEC3;
+
+static const float RVO_EPSILON = 0.00001f;
+
+struct Line {
+	VEC3 point;
+	VEC3 direction;
+};
+
+inline float det2D(const VEC3& vector1, const VEC3& vector2)
+{
+	return vector1[0] * vector2[1] - vector1[1] * vector2[0];
+}
+
+inline float distSqPointLineSegment(const VEC3& a1, const VEC3& b1,	const VEC3& c1)
+{
+	VEC3 a1b1 = b1 - a1;
+	VEC3 a1c1 = c1 - a1;
+	const float r = (a1c1 * a1b1) / a1b1.norm2();
+
+	if (r < 0.0f)
+	{
+		return a1c1.norm2();
+	}
+	else if (r > 1.0f)
+	{
+		VEC3 b1c1 = c1 - b1;
+		return b1c1.norm2();
+	}
+	else
+	{
+		VEC3 v = c1 - (a1 + r * a1b1);
+		return v.norm2();
+	}
+}
+
+inline void projectPointOnLine(const VEC3& p, const VEC3& p1, const VEC3& p2, VEC3& proj)
+{
+	VEC3 v = p - p1;
+	VEC3 seg = p2 - p1;
+
+	seg.normalize();
+
+	proj = p1 + seg * (v * seg);
+}
+
+inline float leftOf(const VEC3& a, const VEC3& b, const VEC3& c)
+{
+	return det2D