Commit 16f29ebc authored by Jund Thomas's avatar Jund Thomas
Browse files

Merge cgogn.unistra.fr:~cazier/SocialAgents2

parents 0ac354f4 d7f0163a
......@@ -29,31 +29,7 @@ class Agent;
class Obstacle;
class MovingObstacle;
struct PFP : public PFP_STANDARD
{
// definition de la carte
typedef Algo::IHM::ImplicitHierarchicalMap MAP ;
// 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 ;
typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
} ;
typedef PFP::VEC3 VEC3 ;
typedef PFP::REAL REAL ;
#include "pfp.h"
class EnvMap
{
......
#ifndef PATH_FINDER
#define PATH_FINDER
#include "env_map.h"
namespace CGoGN
{
......@@ -8,10 +10,10 @@ namespace PathFinder
{
template <typename PFP>
float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart start, Dart goal) ;
float pathCostSqr(EnvMap& envMap, const typename PFP::TVEC3& position, Dart start, Dart goal) ;
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& position,
Dart start, Dart goal, CellMarker<FACE>& bad) ;
//computation of a "Rapidly exploring random tree"
......
......@@ -9,17 +9,19 @@ namespace PathFinder
{
template <typename PFP>
float pathCost(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart start, Dart goal)
float pathCost(EnvMap& envMap, const typename PFP::TVEC3& position, Dart start, Dart goal)
{
typename PFP::MAP& map = envMap.map ;
return VEC3(
Algo::Geometry::faceCentroid<PFP>(map, goal, position)
- Algo::Geometry::faceCentroid<PFP>(map, start, position)).norm() ;
}
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& position,
Dart start, Dart goal, CellMarker<FACE>& isObstacle)
{
typename PFP::MAP& map = envMap.map ;
std::vector<Dart> path ;
std::multimap<float, Dart> facesToTry ;
......@@ -50,10 +52,10 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC
faceIsTraversed.mark(nextFace) ;
if (!isObstacle.isMarked(nextFace))
{
float additionalCost = pathCost<PFP>(map, position, currentFace, nextFace) ;
float additionalCost = pathCost<PFP>(envMap, position, currentFace, nextFace) ;
facePathCostToGoal[nextFace] = facePathCostToGoal[currentFace] + additionalCost ;
faceNextFaceToGoal[nextFace] = currentFace ;
float estimatedCostToGoal = pathCost<PFP>(map, position, nextFace, goal) ;
float estimatedCostToGoal = pathCost<PFP>(envMap, position, nextFace, goal) ;
float estimatedGlobalCost = facePathCostToGoal[nextFace] + estimatedCostToGoal ;
facesToTry.insert(std::make_pair(estimatedGlobalCost, nextFace)) ;
}
......
#ifndef PFP_H_
#define PFP_H_
struct PFP : public PFP_STANDARD
{
// definition de la carte
typedef Algo::IHM::ImplicitHierarchicalMap MAP ;
// 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 ;
typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
} ;
typedef PFP::VEC3 VEC3 ;
typedef PFP::REAL REAL ;
#endif /* PFP_H_ */
......@@ -90,9 +90,9 @@ public:
bool reachedGoal() ;
void setupCircleScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridor2Scenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
......
......@@ -34,13 +34,16 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin
switch (config)
{
case 0 :
setupCircleScenario(0, 1000, 35) ;
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
setupCircleScenario(1000, 35) ;
break ;
case 1 :
setupCorridorScenario(1, 1000,40) ;
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
setupCorridorScenario(1000, 40) ;
break ;
case 2 :
// setupCorridor2Scenario(1, 200,4) ;
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
setupCorridor2Scenario(200, 4) ;
break ;
// case 2 :
// setupScenario(1000) ;
......@@ -54,7 +57,7 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin
// importAgents("myAgents.pos") ;
// break ;
case 5:
envMap_.init(5, 1600.0f, 1200.0f, minSize, 400.0f) ; //cases fines
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //cases fines
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
......@@ -190,10 +193,8 @@ void Simulator::addAgent(const VEC3& start, const VEC3& goal)
agents_.push_back(new Agent(this, start, goal)) ;
}
void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents , unsigned int nbObstacles)
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
{
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;
// Bordure à éviter autour de la scène (10% de sa taille)
......@@ -284,14 +285,110 @@ void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents ,
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
#endif
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupCorridorScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles)
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
// Bordure à éviter autour de la scène (10% de sa taille)
int xBorder = envMap_.geometry.size(0) / 10 ;
int yBorder = envMap_.geometry.size(1) / 10 ;
// Les coordonnées sont comprises entre xMin et xMin+xDelta
// Départ des agents du quart gauche sur toute la hauteur
int xStartMin = envMap_.geometry.min()[0] + xBorder ;
int xStartDelta = envMap_.geometry.size(0) / 5 ;
int yStartMin = envMap_.geometry.min()[1] + yBorder ;
int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;
// Arrivée des agents à l'opposée
int xGoalDelta = envMap_.geometry.size(0) / 5 ;
int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
int yGoalMin = yStartMin ;
int yGoalDelta = yStartDelta ;
for (unsigned int i = 0 ; i < nbAgents ; ++i)
{
VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
VEC3 goal(xGoalMin + rand() % xGoalDelta, yGoalMin + rand() % yGoalDelta, 0) ;
// Un agent sur 2 va de droite à gauche
if (i % 2 == 1)
addAgent(start, goal) ;
else
addAgent(goal, start) ;
}
// Départ des obstacles du quart haut sur toute une demi-largeur
xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
xStartDelta = envMap_.geometry.size(0) / 2 ;
yStartMin = envMap_.geometry.min()[1] + yBorder ;
yStartDelta = envMap_.geometry.size(1) / 5 ;
// Arrivée des obstacles à l'opposée
yGoalDelta = envMap_.geometry.size(1) / 5 ;
yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;
VEC3 xSide (5.0f,0.0f,0.0f);
VEC3 ySide (0.0f,10.0f,0.0f);
std::vector<VEC3> vPos;
MovingObstacle* mo4;
for (unsigned int i = 0 ; i < nbObstacles ; i++)
{
float x = xStartMin + ((int)i*30) % xStartDelta;
// std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
VEC3 goal(x, yGoalMin + rand() % yGoalDelta, 0) ;
vPos.clear();
// Un obstacle sur deux va vers le haut
VEC3 tmp ;
if (i % 2 == 1)
{
tmp = goal ;
goal = start ;
start = tmp ;
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
}
else {
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
}
std::vector<VEC3> goals;
goals.push_back(start);
goals.push_back(goal);
mo4= new MovingObstacle(this, i,vPos,goals,false);
//for generating a random path
unsigned int dartDistForPath = 50 ;
mo4->goals_.clear() ;
Dart dStart = mo4->registering_part->d;
Dart dStop = dStart ;
for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20 || envMap_.map.sameFace(dStop, dStart) ; ++j)
{
envMap_.map.next(dStop) ;
if (dStop == envMap_.map.end())
dStop = envMap_.map.begin() ;
}
addPathToObstacle(mo4, dStart, dStop);
addPathToObstacle(mo4, dStop, dStart);
movingObstacles_.push_back(mo4);
}
}
void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles)
{
std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
// Bordure à éviter autour de la scène (10% de sa taille)
......@@ -406,8 +503,6 @@ void Simulator::setupCityScenario(int nbLines, int nbRank)
addAgent(posagent, -1.0f * posagent) ;
}
}
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupScenario(unsigned int nbMaxAgent)
......@@ -464,7 +559,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
envMap_.position, dStart,
dGoal,
envMap_.buildingMark) ;
......@@ -488,7 +583,7 @@ void Simulator::addPathToCorner()
Dart dStart = agents_[i]->part_.d ;
Dart dStop = agents_[i]->finalDart ;
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
envMap_.position, dStart,
dStop,
envMap_.buildingMark) ;
......@@ -526,7 +621,7 @@ void Simulator::addPathsToAgents()
// std::cout << "dest1" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop, envMap_.position) << std::endl;
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
envMap_.position, dStart,
dStop,
envMap_.buildingMark) ;
......@@ -553,7 +648,7 @@ void Simulator::addPathsToAgents()
// std::cout << "dest2" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop2, envMap_.position) << std::endl;
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
envMap_.buildingMark) ;
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
......@@ -566,7 +661,7 @@ void Simulator::addPathsToAgents()
agents_[i]->goals_.push_back(dest) ;
}
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
envMap_.buildingMark) ;
// std::cout << "destStart" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStart, envMap_.position) << std::endl;
......@@ -601,7 +696,7 @@ void Simulator::addPathsToAgents_height()
if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
}
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
envMap_.position, dStart,
dStop,
envMap_.buildingMark) ;
......@@ -622,7 +717,7 @@ void Simulator::addPathsToAgents_height()
if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
}
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
envMap_.buildingMark) ;
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
......@@ -631,7 +726,7 @@ void Simulator::addPathsToAgents_height()
agents_[i]->goals_.push_back(dest) ;
}
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
envMap_.buildingMark) ;
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment