Skip to content
GitLab
Commit 38eb3098 authored by pitiot's avatar pitiot
Browse files

merging

parent 3797df7a
Expand all Hide whitespace changes
Inline Side-by-side
Debug/CMakeLists.txt
View file @ 38eb3098
......@@ -25,6 +25,7 @@ add_executable( socialAgentsD
../src/env_map.cpp
../src/agent.cpp
../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
......
Release/CMakeLists.txt
View file @ 38eb3098
......@@ -23,6 +23,7 @@ add_executable( socialAgents
../src/env_map.cpp
../src/agent.cpp
../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
......
include/agent.h
View file @ 38eb3098
......@@ -55,7 +55,12 @@ public:
VEC3 finalGoal ;
Dart finalDart ;
MovingObstacle **movingObstacles_;
int nb_mos;
static unsigned int maxNeighbors_ ;
static unsigned int maxMovingObstacles_;
static float averageMaxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
......
include/env_generator.h
View file @ 38eb3098
......@@ -15,7 +15,7 @@ bool notDiagonalAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, CellMarker<
template <typename PFP> Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap, unsigned int nbBuildings) ;
template <typename PFP>
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
......
include/env_generator.hpp
View file @ 38eb3098
......@@ -63,9 +63,9 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
}
template <typename PFP>
void generateCity(EnvMap& envMap)
void generateCity(EnvMap& envMap, unsigned int nbBuildings)
{
unsigned int nbBuilding = 1000 ;
unsigned int nbBuilding = nbBuildings ;
std::cout << " - Generate City : " << nbBuilding << " buildings" << std::endl ;
generateGrid<PFP>(envMap) ;
......@@ -76,7 +76,7 @@ void generateCity(EnvMap& envMap)
if (!envMap.buildingMark.isMarked(d) && (rand() % 12 == 0)
&& notDiagonalAdjacentToAnObstacle<PFP>(envMap.map, d, envMap.buildingMark))
{
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 2.0f, rand() % 4) ;
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
--nbBuilding ;
}
}
......
include/env_map.h
View file @ 38eb3098
......@@ -28,6 +28,7 @@ using namespace CGoGN ;
class Agent;
class Obstacle;
class MovingObstacle;
class ArticulatedObstacle;
#include "pfp.h"
......@@ -402,6 +403,7 @@ inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
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());
addElementToVector<Obstacle*>(neighborObstvect[d],o);
}
......
include/moving_obstacle.h
View file @ 38eb3098
......@@ -10,13 +10,13 @@
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin, ArticulatedObstacle * art=NULL, int ind2=-1);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -44,6 +44,8 @@ public:
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
VEC3 front;
VEC3 focus1, focus2;
double length, width, sum_dist_foci, sum_dist_foci_rest;
VEC3 finalGoal;
float angle;
......@@ -61,11 +63,17 @@ public:
float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
float color1;
float color2;
float color3;
bool seen;
VEC3 velocity_;
VEC3 newVelocity_;
VEC3 prefVelocity_;
Simulator* sim_;
bool spinning;
ArticulatedObstacle * parent;
int index_parent;
};
#endif
include/simulator.h
View file @ 38eb3098
......@@ -7,7 +7,7 @@
#include "env_map.h"
#include "agent.h"
#include "obstacle.h"
#include "moving_obstacle.h"
#include "articulated_obstacle.h"
#include "moving_mesh.h"
#include "path_finder.h"
......@@ -84,7 +84,7 @@ public:
~Simulator() ;
void init(unsigned int config, float dimension, bool enablePathFinding = false) ;
void init(float dimension, bool enablePathFinding = false) ;
void doStep() ;
......@@ -92,7 +92,7 @@ public:
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 setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
......@@ -121,6 +121,7 @@ public:
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
int minSize;
unsigned int config;
float timeStep_ ;
float globalTime_ ;
unsigned int nbSteps_ ;
......
include/socialAgents.ui
View file @ 38eb3098
......@@ -129,6 +129,13 @@
<item>
<widget class="QSpinBox" name="check_slide"/>
</item>
<item>
<widget class="QCheckBox" name="check_elipse">
<property name="text">
<string>draw elipse</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
......
include/viewer.h
View file @ 38eb3098
......@@ -103,6 +103,7 @@ public:
bool drawObstPredictionTri ;
bool drawObstPath ;
bool draw_dart;
bool draw_elipse;
unsigned int dartSlider;
public slots:
......@@ -182,4 +183,9 @@ public slots:
dartSlider = i;
updateGL();
}
void slot_elipse(bool b)
{
draw_elipse = b;
updateGL();
}
} ;
src/agent.cpp
View file @ 38eb3098
This diff is collapsed.
src/env_map.cpp
View file @ 38eb3098
......@@ -73,17 +73,18 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
switch (config)
{
case 0 :
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,1) ;
break ;
case 1 :
CityGenerator::generateGrid<PFP>(*this) ;
break ;
case 2 :
CityGenerator::generateCity<PFP>(*this) ;
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
break ;
case 3 :
CityGenerator::generateCity<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10) ;
break ;
case 4 :
CityGenerator::generatePlanet<PFP>(*this) ;
......
src/moving_obstacle.cpp
View file @ 38eb3098
......@@ -70,11 +70,17 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return pos2;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin) :
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
index(ind),
newVelocity_(0),
sim_(sim)
sim_(sim),
parent(art),
index_parent(ind2)
{
seen = false;
color1=1.0f;
color2=1.0f;
color3=1.0f;
assert(pos.size() > 2);
// movingObstNeighbors_.reserve(maxNeighbors_);
......@@ -102,7 +108,18 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
center = sum / nbVertices;
front=(vertices[1] + vertices[2]) / 2;
if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
length = (vertices[0]-vertices[1]).norm();
width = (vertices[1]-vertices[2]).norm();
sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle = get_angle(goals_[curGoal_] - center,front - center);
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -319,6 +336,13 @@ void MovingObstacle::update()
if(sim_->detect_agent_collision)
general_belonging.clear();
if(!seen)
{
color1=1.0f;
color2=1.0f;
color3=1.0f;
}
seen=false;
PFP::VEC3 bary = 0;
Dart d;
......@@ -327,7 +351,15 @@ void MovingObstacle::update()
// MAJ des particules
float abs_angle= angle > 0 ? 1 : -1;
float rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
float rotor=0;
if (index_parent==0)
{
rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
}
else
{
rotor = abs_angle*angle ;
}
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
......@@ -354,6 +386,25 @@ void MovingObstacle::update()
center = bary / nbVertices;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3 P0_P1 = vertices[1] - vertices[0];
float velocity_coef = 10.0;
if(P0_P1 * velocity_ > 0) // P0_P1 est dans le sens de la vitesse
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
}
else
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
}
sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
{
......@@ -386,7 +437,7 @@ void MovingObstacle::update()
std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
{
if (this->is_inside((*it)->getPosition()))
if (this->is_inside((*it)->part_.getPosition()))
{
(*it)->alive=false;
(*it)->color1=0.0f;
......@@ -401,6 +452,7 @@ void MovingObstacle::update()
}
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
{
registering_part->move(pos);
......@@ -417,7 +469,8 @@ void resetPartSubdiv(Obstacle* o)
if (mo != NULL)
{
VEC3 pos =mo->registering_part->getPosition();
mo->registering_part->ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
mo->registering_part->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
mo->registering_part->setState(FACE) ;
mo->registering_part->move(pos) ;
}
......@@ -459,22 +512,38 @@ void displayMO(Obstacle * o)
// TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 2.0f)
VEC3 goalVector;
if (index_parent<1)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 5.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
}
else
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
goalVector = get_center(parent,index_parent-1) -center;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
}
if (spinning) angle =get_angle(goalVector,front-center);
prefVelocity_ = goalVector ;
}
......@@ -490,6 +559,7 @@ void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenan
forward.normalize() ;
forward *= objective;
}
if(index_parent==0 && angle>0.01f) forward/=10;
newVelocity_=forward;
}
else
......
src/simulator.cpp
View file @ 38eb3098
......@@ -14,11 +14,12 @@ Simulator::Simulator(unsigned int config, unsigned int minS) :
nb_dead(0)
{
minSize=minS;
multires=false;
detect_agent_collision=false;
multires=true;
detect_agent_collision=true;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
init(config, minSize, 2.0f) ;
this->config=config;
init(2.0f) ;
}
Simulator::~Simulator()
......@@ -27,27 +28,21 @@ Simulator::~Simulator()
delete agents_[i] ;
}
void Simulator::init(unsigned int config, float dimension, bool enablePathFinding)
void Simulator::init( float dimension, bool enablePathFinding)
{
std::cout << "Setup scenario" << std::endl ;
switch (config)
{
case 0 :
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
setupCircleScenario(1000, 35) ;
setupCircleScenario(1000,20) ;
break ;
case 1 :
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
setupCorridorScenario(1000, 40) ;
setupCorridorScenario(1000,40) ;
break ;
case 2 :
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
setupCorridor2Scenario(200, 4) ;
setupSnakeCorridorScenario(1000,5,10) ;
break ;
// case 2 :
// setupScenario(1000) ;
// break ;
// case 3 :
// setupCityScenario(20, 20) ;
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
......@@ -206,6 +201,16 @@ void Simulator::addAgent(const VEC3& start, const VEC3& goal, Dart d)
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
{
if (multires)
{
envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
}
std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;
// Bordure à éviter autour de la scène (10% de sa taille)
......@@ -245,7 +250,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
}
else
{
v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
}
start = center + v ;
positions.push_back(start);
......@@ -296,10 +301,23 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
#endif
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
if (multires)
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
}
std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
// Bordure à éviter autour de la scène (10% de sa taille)
......@@ -398,13 +416,24 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
}
}
void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles)
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
if (multires)
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines
}
std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " 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 ;
int xBorder = envMap_.geometry.size(0) / 5 ;
int yBorder = envMap_.geometry.size(1) / 5 ;
// Les coordonnées sont comprises entre xMin et xMin+xDelta
......@@ -426,75 +455,86 @@ void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObs
VEC3 goal(xGoalMin + rand() % xGoalDelta, yGoalMin + rand() % yGoalDelta, 0) ;
// Un agent sur 2 va de droite à gauche
VEC3 tmp ;
if (i % 2 == 1)
addAgent(start, goal) ;
else
addAgent(goal, start) ;
{
tmp = goal ;
goal = start ;
start = tmp ;
}
addAgent(start, goal) ;
}
// 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 ;
yStartDelta = envMap_.geometry.size(1) / 20 ;
// Arrivée des obstacles à l'opposée
yGoalDelta = envMap_.geometry.size(1) / 5 ;
yGoalDelta = 3* 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++)
int sumObstacles=0;
for(unsigned int j = 0; j<nbSnakes; j++)
{
float x = xStartMin + ((int)i*30) % xStartDelta;
// std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
std::vector<PFP::VEC3> positions [snakeSize] ;
float x = xStartMin + rand() % xStartDelta;
VEC3 goal;
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)
std::vector<VEC3> vPos;
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);
int r=0;
while (r<40)
{
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);
x = xStartMin + rand() % xStartDelta;
goal=VEC3 (x, yGoalMin + rand() % yGoalDelta, 0) ;
if ((goal-goals[r]).norm2()>1000){
goals.push_back(goal);
r++;
}
}
else {
positions[0]=vPos;
for (int i = 1 ; i < snakeSize ; i++)
{
start=start-ySide-ySide;
vPos.clear();
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);