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

Merge cgogn:~jund/CGoGN_Apps/SA2

Conflicts:
	include/agent.h
	include/env_generator.h
	include/env_generator.hpp
	include/env_map.h
	include/env_render.h
	include/exportObstacles.h
	include/exportObstacles.hpp
	include/obstacle.h
	include/path_finder.h
	include/path_finder.hpp
	include/simulator.h
	include/viewer.h
	src/agent.cpp
	src/env_map.cpp
	src/simulator.cpp
	src/viewer.cpp
parents f74b0ffe 8159c58a
......@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8)
project(SocialAgents)
add_definitions(-DSPATIAL_HASHING)
#add_definitions(-DSPATIAL_HASHING)
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../CGoGN CACHE STRING "CGoGN root dir")
include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
......
......@@ -24,10 +24,10 @@ public:
void updateAgentNeighbors() ;
void updateObstacleNeighbors() ;
void update() ;
void computePrefVelocity() ;
void computeNewVelocity() ;
void update();
void obstacle_priority(PFP::VEC3 * goalVector); //ajout moving obst
void computePrefVelocity();
void computeNewVelocity();
void computeNewVelocity2() ;
......@@ -42,6 +42,7 @@ public:
std::vector<std::pair<float, Agent*> > agentNeighbors_ ;
std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
std::vector<std::pair<float, Obstacle*> > movingObstacleNeighbors_;
#ifdef SPATIAL_HASHING
VEC3 pos ;
......@@ -66,9 +67,14 @@ public:
static float range_ ;
static float rangeSq_ ;
static float timeStep ;
static unsigned int cptAgent ;
float color1;
float color2;
float color3;
unsigned int agentNo ;
VEC3 velocity_ ;
......@@ -78,7 +84,8 @@ public:
VEC3 meanSpeed_ ;
// VEC3 meanPos_;
Simulator* sim_ ;
} ;
Simulator* sim_;
bool alive;
};
#endif
......@@ -33,11 +33,6 @@ template <typename PFP>
bool animateCity(EnvMap* envMap) ;
#endif
//template <typename PFP>
//Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker<EDGE>& obstacleMark, CellMarker<FACE>& buildingMark);
//template <typename PFP>
//void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker<EDGE>& obstacleMark, CellMarker<FACE>& buildingMark, float sideSize, unsigned int nbSquares);
template <typename PFP>
Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildingType) ;
......@@ -75,7 +70,6 @@ void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position,
template <typename PFP>
void installGuardRail(typename PFP::MAP& map, typename PFP::TVEC3& position,
CellMarker<EDGE>& obstacleMark, CellMarker<FACE>& buildingMark, float height) ;
}
}
......
......@@ -279,10 +279,10 @@ bool animateCity(EnvMap* envMap)
#endif
//template <typename PFP>
//Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark)
//Dart generateBuilding(typename PFP::MAP& map, VertexAttribute<typename PFP::VEC3>& position, Dart d, float height, unsigned int buildingType, CellMarker<EDGE>& obstacleMark, CellMarker<FACE>& buildingMark)
//template <typename PFP>
//void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize, unsigned int nbSquares)
//void generateCity(typename PFP::MAP& map, VertexAttribute<typename PFP::VEC3>& position, CellMarker<EDGE>& obstacleMark, CellMarker<FACE>& buildingMark, float sideSize, unsigned int nbSquares)
template <typename PFP>
Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildingType)
......@@ -302,7 +302,7 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
Dart next = map.phi1(dd) ;
Dart previous = map.phi_1(dd) ;
Obstacle* o = new Obstacle(position[dd], position[next], position[previous],
position[map.phi1(next)]) ;
position[map.phi1(next)], NULL, 0);
#ifdef SPATIAL_HASHING
VEC3 ov = o->p2 - o->p1 ;
......@@ -329,7 +329,18 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
pos += step ;
}
#else
envMap.pushObstacleInCells(o, map.phi2(dd)) ;
// David
// envMap.pushObstacleInCells(o, map.phi2(dd)) ;
// Thomas's team
Dart x =map.phi2(dd);
if(!map.isBoundaryMarked(x))
{
envMap.pushObstacleInCells(o, x);
if(!map.isBoundaryMarked(map.phi2(map.phi1(x))))envMap.pushObstacleInCells(o, map.phi2(map.phi1(x)));
if(!map.isBoundaryMarked(map.phi2(map.phi_1(x))))envMap.pushObstacleInCells(o, map.phi2(map.phi_1(x)));
}
#endif
dd = map.phi1(dd) ;
......@@ -699,7 +710,7 @@ 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)
bool isAnEar(typename PFP::MAP& map, VertexAttribute<typename PFP::VEC3>& position, Dart dd, float& area)
{
typedef typename PFP::VEC3 VEC3 ;
......
......@@ -25,8 +25,9 @@
using namespace CGoGN ;
class Agent ;
class Obstacle ;
class Agent;
class Obstacle;
class MovingObstacle;
struct PFP : public PFP_STANDARD
{
......@@ -36,8 +37,12 @@ struct PFP : public PFP_STANDARD
// definition des listes d'agent
typedef std::vector<Agent*> AGENTS ;
typedef std::vector<Obstacle*> OBSTACLES ;
typedef std::vector<MovingObstacle*> MOVINGOBSTACLES;
typedef NoMathIONameAttribute<AGENTS> AGENTVECT ;
typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT ;
typedef NoMathIONameAttribute<MOVINGOBSTACLES> MOVINGOBSTACLEVECT;
typedef VertexAttribute<PFP::VEC3> TVEC3;
typedef FaceAttribute<AGENTVECT> TAB_AGENTVECT ;
typedef FaceAttribute<OBSTACLEVECT> TAB_OBSTACLEVECT ;
......@@ -72,17 +77,20 @@ public:
unsigned int mapMemoryCost() ;
float max_for_obstacles;
unsigned int nbSquares;
void scale(float scaleVal);
#ifndef SPATIAL_HASHING
Dart getBelongingCell(const VEC3& pos) ;
void subdivideAllToMaxLevel() ;
void subdivideToProperLevel() ;
#endif
#ifndef SPATIAL_HASHING
void foreach_neighborFace(Dart d, FunctorType& f) ;
void registerObstaclesInFaces() ;
void registerWallInFaces();
void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ;
void setAgentNeighborsAndObstacles(Agent* agent) ;
......@@ -111,6 +119,17 @@ public:
PFP::TVEC3 normalScenary ;
CellMarker<EDGE> obstacleMarkS ;
CellMarker<FACE> buildingMarkS ;
CellMarker<EDGE> obstacleMark ;
CellMarker<FACE> buildingMark ;
CellMarker<FACE> pedWayMark ;
// ajout moving obst
void addObstAsNeighbor (Obstacle * o, const std::list<Dart>& belonging_cells, std::list<Dart> * nieghbor_cells);
// void addMovingObstAsNeighbor (MovingObstacle * mo,std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells);
void pushObstNeighborInCells(Obstacle* o, Dart d);
void popObstNeighborInCells(Obstacle* o, Dart d);
void find_next(Obstacle* o,Dart * d, CellMarkerStore<FACE>& cms);
std::vector<Dart> newBuildings ;
......@@ -119,13 +138,11 @@ public:
PFP::TAB_AGENTVECT agentvect ;
PFP::TAB_AGENTVECT neighborAgentvect ;
PFP::TAB_OBSTACLEVECT neighborObstvect;
#endif
PFP::TAB_OBSTACLEVECT obstvect ;
CellMarker<EDGE> obstacleMark ;
CellMarker<FACE> buildingMark ;
CellMarker<FACE> pedWayMark ;
#ifndef SPATIAL_HASHING
static const unsigned int nbAgentsToSubdivide = 10 ;
......@@ -178,6 +195,14 @@ public:
#endif
} ;
void update_registration(Obstacle * o);
void register_pop(Obstacle* o, int n);
void register_add(Obstacle* o, int n);
void resetObstInFace(Obstacle* o);
void resetObstPartInFace(Obstacle* o, Dart d);
void resetPart(MovingObstacle * mo, Dart d);
void displayMO(Obstacle * o);
/**************************************
* INLINE FUNCTIONS *
**************************************/
......@@ -229,6 +254,7 @@ inline void EnvMap::nbAgentsDecrease(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) ;
// nbAgentsIncrease(d);
......@@ -268,9 +294,115 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
} while (dd != d) ;
}
//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////
inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::list<Dart>& belonging_cells, std::list<Dart> *neighbor_cells)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
neighbor_cells->clear();
CellMarkerStore<FACE> MovingObstMark(map);
CellMarkerStore<FACE> OneRingMark(map);
for(std::list<Dart>::const_iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
{ assert(!buildingMark.isMarked(d)) ;
MovingObstMark.mark(*it);
}
std::list<Dart>::const_iterator it=belonging_cells.begin();
Dart beg = NIL;
Dart first =NIL;
Dart d=NIL;
Dart dd=NIL;
// CGoGNout << "debut"<<CGoGNendl;
//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
do
{
beg = *it;
first = NIL;
d=beg;
do {
dd=map.alpha1(map.alpha1(d));
do {
if (!MovingObstMark.isMarked(dd))
{
first = dd;
}
dd=map.alpha1(dd);
}while (first==NIL && dd!=d);
d=map.phi1(d);
} while(first==NIL && d!=beg);
++it;
}while(first==NIL && it!=belonging_cells.end());
assert(!buildingMark.isMarked(d)) ;
d=first;
do
{
if (!OneRingMark.isMarked(d))
{
OneRingMark.mark(d);
(*neighbor_cells).push_back(d);
}
find_next(o,&d, MovingObstMark);
}while(!map.sameFace(d,first));
}
// find_next cherche la prochaine case "voisine" d'un obstacle faisant parti d'un movingobstacle (algo de parcours du one-ring )
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<FACE>& cms)
{
Dart d=*ddd;
Dart first = NIL;
Dart dd;
int rd =0;
do{
rd++;
dd=map.alpha1((d));
if (!(cms.isMarked(dd)))
{
first = dd;
}
d=map.phi_1(d);
if (rd>10000) {
displayMO(o);
CGoGNout <<(first==NIL)<< " infini find next : "<< *ddd<<CGoGNendl;
break;
}
}while(first==NIL);
*ddd=first;
}
inline void EnvMap::pushObstNeighborInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) == neighborObstvect[d].end());
neighborObstvect[d].push_back(o);
}
inline void EnvMap::popObstNeighborInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) != neighborObstvect[d].end());
removeElementFromVector<Obstacle* >(neighborObstvect[d], o);
}
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) == obstvect[d].end());
assert(!buildingMark.isMarked(d)) ;
obstvect[d].push_back(o) ;
......@@ -296,6 +428,7 @@ inline void EnvMap::popObstacleInCells(Obstacle* o, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
assert(!buildingMark.isMarked(d)) ;
assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) != obstvect[d].end());
removeElementFromVector<Obstacle*>(obstvect[d], o) ;
......@@ -320,6 +453,8 @@ inline void EnvMap::clearUpdateCandidates()
refineCandidate.clear() ;
coarsenCandidate.clear() ;
}
///addMovingObstAsNeighbor est pour détecter les voisins du movingobstacle global afin de le considerer comme un super agent
#endif
#endif
......@@ -46,7 +46,7 @@ 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 } ;
inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
bool showObstacleDist = false)
bool showObstacleDist = false, float c1=1.0f,float c2=0, float c3=0 )
{
#ifdef SPATIAL_HASHING
const VEC3& pos = agent->pos ;
......@@ -63,16 +63,21 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
// VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
// glColor3fv(col.data());
glColor3f(1.0f, 0.0f, 0.0f) ;
glColor3f(c1,c2,c3) ;
glBegin(GL_POLYGON) ;
for (unsigned int i = 0; i < 5; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2] + 0.01f) ;
for(unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f) ;
glEnd() ;
VEC3 dir = agent->velocity_ ;
// VEC3 posi = agent->finalGoal;
// glColor3f(0.0f, 0.0f, 1.0f);
// glBegin(GL_POLYGON);
// for(unsigned int i = 0; i < 5; ++i)
// glVertex3f(posi[0] + (cosT[i] * 1.5f), posi[1] + (sinT[i] * 1.5f), posi[2]-0.01f);
// glEnd();
VEC3 dir = agent->prefVelocity_ ;
dir.normalize() ;
//draw the speed vector
//draw the speed vector
// VEC3 base(0,0,1);
// VEC3 myAxisRot = base^dir;
// myAxisRot.normalize();
......@@ -95,8 +100,8 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
{
glLineWidth(3.0f) ;
glBegin(GL_LINE_STRIP) ;
for (std::vector<VEC3>::iterator it = ++(agent->goals_.begin()); it != agent->goals_.end();
++it)
for (std::vector<VEC3>::iterator it = ++(agent->goals_.begin()) ; it != agent->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
}
......@@ -109,8 +114,8 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
radius = agent->neighborDist_ ;
glColor3f(0.0f, 1.0f, 0.0f) ;
glBegin(GL_LINE_LOOP) ;
for (unsigned int i = 0; i < 5; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2] + 0.01f) ;
for (unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2] + 0.01f) ;
glEnd() ;
}
......@@ -119,8 +124,8 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
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)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2] + 0.01f) ;
for (unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2] + 0.01f) ;
glEnd() ;
}
}
......
//#ifndef M_MOVING_OBSTACLE_H
//#define M_MOVING_OBSTACLE_H
//
//#include <iostream>
//
//#include "utils.h"
//#include "env_map.h"
//#include "Algo/MovingObjects/particle_cell_2D.h"
//
//class Simulator;
//
//class MovingObstacle
//{
//public:
// MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos);
//
// void updateAgentNeighbors();
//
// void computePrefVelocity();
// void computeNewVelocity();
//
// void update();
//
// unsigned int nbVertices;
// CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
// Obstacle* * obstacles_;
//
// std::vector<std::pair<float, Agent*> > agentNeighbors_;
//
// VEC3 finalGoal;
#ifndef M_MOVING_OBSTACLE_H
#define M_MOVING_OBSTACLE_H
#include <iostream>
#include "utils.h"
#include "env_map.h"
#include <list>
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
class MovingObstacle
{
public:
MovingObstacle(int index,EnvMap* envmap, std::vector<PFP::VEC3> pos, VEC3 goal,float rota);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
void updateFixedObstNeighbors();
bool is_inside (VEC3 p);
void computePrefVelocity();
void update();
void register_ (Obstacle* o,Dart d, int n );
void register_oneRingAdd(Obstacle * o, Dart d, int n);
void register_update(Obstacle* o, Dart d, int n);
// void difference_list (std::list<Dart> old_, std::list<Dart> new_);
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
float gravity_dist;
VEC3 center;
int nb_agents_voisins;
int nb_register_cells;
int index;
int max_x_ind;
int min_x_ind;
Obstacle* * obstacles_;
std::list<Dart> * belonging_cells;
std::list<Dart> * neighbor_cells;
std::list<Dart> general_belonging;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
std::vector<std::pair<float, MovingObstacle*> > movingObstNeighbors_;
float rot;
VEC3 finalGoal;
float make_half_turn;
// Dart finalDart;
//
// unsigned int curGoal_;
//
// static float neighborDistSq_;
// static float timeStep ;
//
// VEC3 velocity_;
// VEC3 newVelocity_;
// VEC3 prefVelocity_;
//
// EnvMap* envMap_;
//};
//
//#endif
static unsigned int maxNeighbors_;
static float detectionFixedObst;
static float maxSpeed_;
static float timeStep ;
float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
VEC3 velocity_;
VEC3 newVelocity_;
VEC3 prefVelocity_;
EnvMap* envMap_;
};
#endif
......@@ -6,8 +6,10 @@
class Obstacle
{
public:
Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint)
Obstacle(VEC3& point1, VEC3& point2, VEC3& prevPoint, VEC3& nextPoint,
MovingObstacle * moving1, int ind) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind)
{
p1[2] = 0 ;
p2[2] = 0 ;
......@@ -19,6 +21,8 @@ public:
VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
MovingObstacle * mo ;
int index ;
} ;
#endif
......@@ -36,8 +36,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC
{
std::vector<Dart> path ;
FaceAutoAttribute<float> tablePathCost(map) ;
FaceAutoAttribute<Dart> tablePred(map) ;
FaceAutoAttribute< float > tablePathCost(map);
FaceAutoAttribute< Dart > tablePred(map);
std::multimap<float, Dart> vToDev ;
......
......@@ -6,7 +6,7 @@
#include "env_map.h"
#include "agent.h"
#include "obstacle.h"
//#include "moving_obstacle.h"
#include "moving_obstacle.h"
#include "path_finder.h"
#include <iostream>
......@@ -112,7 +112,7 @@ public:
EnvMap envMap_ ;
std::vector<Agent*> agents_ ;
// std::vector<MovingObstacle*> movingObstacles_;
std::vector<MovingObstacle*> movingObstacles_;
float timeStep_ ;
float globalTime_ ;
......@@ -126,10 +126,13 @@ public:
unsigned long nearNeighbors ;
unsigned long totalNeighbors ;
int avoidance;
int nb_dead;
ThreadUpdateInfo* tc1 ;
ThreadUpdateInfo* tc2 ;
ThreadUpdateInfo* tc3 ;
ThreadUpdateInfo* tc4 ;
} ;
};
#endif
......@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>208</width>
<width>211</width>
<height>457</height>
</rect>