Skip to content

GitLab

S
SocialAgents3D
Commit cb708f28 authored by pitiot's avatar pitiot
Browse files
Options

correction initialisation

parent 7f90ed0e
Hide whitespace changes
Inline Side-by-side
Showing with 1084 additions and 353 deletions
Release/CMakeLists.txt
View file @ cb708f28
......@@ -16,6 +16,8 @@ include_directories(
${CMAKE_CURRENT_BINARY_DIR}
)
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
QT4_WRAP_UI( socialAgents_ui ../include/socialAgents.ui )
QT4_WRAP_CPP( socialAgents_moc ../include/viewer.h )
add_executable( socialAgents
......
include/agent.h
View file @ cb708f28
......@@ -6,7 +6,14 @@
#include "utils.h"
#include "env_map.h"
#include "spatialHashing.h"
#include "Algo/MovingObjects/particle_cell_2D.h"
//#define SECURED
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
#else
#include "Algo/MovingObjects/particle_cell_2D.h"
#endif
class Simulator ;
......@@ -46,7 +53,11 @@ public:
#ifdef SPATIAL_HASHING
VEC3 pos ;
#else
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_ ;
#ifdef SECURED
CGoGN::Algo::MovingObjects::ParticleCell2DSecured<PFP> part_ ;
#else
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_ ;
#endif
#endif
unsigned int curGoal_ ;
......@@ -63,6 +74,7 @@ public:
static unsigned int maxMovingObstacles_;
static float averageMaxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
static float radius_ ;
......
include/env_map.h
View file @ cb708f28
......@@ -79,7 +79,7 @@ public:
void popAgentInCells(Agent* agent, Dart d) ;
void agentChangeFace(Agent* agent, Dart oldFace) ;
void popAndPushObstacleInCells(Obstacle* o, int n);
Dart popAndPushObstacleInCells(Obstacle* o, int n);
void pushObstacleInCells(Obstacle * mo);
void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n);
void pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross);
......@@ -328,7 +328,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
} while(first==NIL && d!=beg);
++it;
}while(first==NIL && it!=belonging_cells.end());
assert(!buildingMark.isMarked(d)) ;
// assert(!buildingMark.isMarked(d)) ;
assert(first!=NIL) ;
// CGoGNout<<"first : "<<first<<CGoGNendl;
d=first;
......
include/env_render.h
View file @ cb708f28
......@@ -5,6 +5,8 @@
#include "agent.h"
#include "Utils/colorMaps.h"
#include "Algo/Render/GL1/map_glRender.h"
inline void renderDart(EnvMap& m, Dart d)
{
PFP::VEC3 p1 = m.position[d] ;
......@@ -44,17 +46,66 @@ inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
}
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false)
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false, bool showVertices=false)
{
glBegin(GL_POLYGON) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->getPosition(i) ;
glVertex3fv(p.data()) ;
}
glEnd() ;
// glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(obst->map, Algo::Render::GL1::LINE,
// 1.0, obst->position,
// obst->normal) ;
if(showVertices)
{
glPointSize(3.0f) ;
glColor3f(0.0,0.0,1.0);
glBegin(GL_POINTS) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->getPosition(i) ;
glVertex3fv(p.data()) ;
}
glEnd() ;
glPointSize(1.0f) ;
}
glColor3f(1.0,0.0,1.0);
glBegin(GL_LINE_LOOP) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->parts_[i]->getPosition() ;
const VEC3& p = obst->getDilatedPosition(i) ;
glVertex3fv(p.data()) ;
}
glEnd() ;
// glColor3f(0.0,1.0,1.0);
// glBegin(GL_LINES) ;
// for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
// {
// const VEC3& p = obst->getPosition(i) ;
// glVertex3fv(p.data()) ;
// const VEC3& p2 = obst->getDilatedPosition(i) ;
// glVertex3fv(p2.data()) ;
// }
// glEnd() ;
glLineWidth(5.0f) ;
glColor3f(1.0,1.0,0.0);
glBegin(GL_LINES) ;
VEC3 p = obst->getPosition(0) ;
glVertex3fv(p.data()) ;
p += 20.0f*obst->velocity_;
glVertex3fv(p.data()) ;
glEnd() ;
if (renderPath)
{
glLineWidth(3.0f) ;
......@@ -62,10 +113,19 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
for (std::vector<VEC3>::iterator it = ++(obst->goals_.begin()) ; it != obst->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
// glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
glVertex3f((*it)[0], (*it)[1], obst->index) ;
}
glEnd() ;
glLineWidth(1.0f) ;
glBegin(GL_LINES) ;
VEC3 p = obst->getPosition(0) ;
glVertex3fv(p.data()) ;
p = obst->goals_[obst->curGoal_];
glVertex3fv(p.data()) ;
glEnd() ;
}
}
......@@ -127,13 +187,22 @@ 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() ;
for (std::vector<VEC3>::iterator it = (agent->goals_.begin()) ; it != agent->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
glVertex3f((*it)[0], (*it)[1], agent->agentNo) ;
}
glVertex3f((*(agent->goals_.begin()))[0], (*(agent->goals_.begin()))[1], (*(agent->goals_.begin()))[2]) ;
glEnd() ;
glLineWidth(1.0f) ;
glPointSize(5.0f) ;
glBegin(GL_POINTS) ;
VEC3 g = agent->goals_[agent->curGoal_];
glVertex3fv(g.data()) ;
glEnd() ;
glPointSize(1.0f) ;
}
if (showNeighborDist)
......
include/moving_mesh.h
View file @ cb708f28
......@@ -15,23 +15,32 @@ using namespace std;
class MovingMesh
{
public:
MovingMesh(EnvMap * envMap, Dart d, std::string filename);
MovingMesh(EnvMap& envMap, Dart d, std::string filename);
void linkWithObstacle(MovingObstacle * mo);
void transform(Geom::Matrix44f m);
void translate(VEC3 v);
void scale(float val);
void moveInFace(PFP::MAP& motherMap, Dart d, VertexAttribute<VEC3> pos);
void move();
void animate();
std::vector<VEC3> computeProjectedPointSet();
std::vector<VEC3> jarvisConvexHull(std::vector<VEC3> projectedPointSet);
void draw();
std::vector<VEC3> computeProjectedPointSet(float maxHeight);
void simplifyCurve(std::vector<VEC3>& pointSet, std::vector<bool>& active, int start, int end, float epsilon);
std::vector<VEC3> jarvisConvexHull(const std::vector<VEC3>& projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
PFP::MAP& motherMap;
VertexAttribute<VEC3>& motherPosition;
PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
float scaleValue;
ShapeMatchingQuadratic<PFP> * smg;
std::vector<VEC3> skeleton;
......
include/moving_obstacle.h
View file @ cb708f28
......@@ -7,16 +7,18 @@
#include "env_map.h"
#include <set>
#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);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ;
class MovingMesh;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin, ArticulatedObstacle * art=NULL, int ind2=-1);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal=0, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -25,13 +27,40 @@ public:
void computePrefVelocity();
void computeNewVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> * registering_part);
VEC3 getDilatedPosition(unsigned int ind); //vertex position with velocity dilatation
VEC3 getPosition(unsigned int ind); // vertex position
void update();
PFP::REAL computeMVC(PFP::VEC3 p, Dart vertex);
void computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates);
void attachMesh(MovingMesh* mm);
void updateMesh();
unsigned int nbVertices;
std::vector<PFP::VEC3> vertices;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
float gravity_dist;
PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
VertexAttribute<VEC3> deformation;
VertexAttribute<VEC3> velocity;
EdgeAttribute<float> edgeLength;
Dart groundFace;
//vector de détails pour la deformation
// std::vector<PFP::VEC3> verticesDeformation;
//gestion pour du mass-ressort
// std::vector<PFP::VEC3> verticesVelocity;
// std::vector<float> edgeLength;
// std::vector<float> verticesAngle;
// float rigidity;
// float gravity_dist;
VEC3 center;
int nb_agents_voisins;
int nb_register_cells;
......@@ -53,6 +82,7 @@ public:
std::vector<VEC3> goals_ ;
unsigned int curGoal_;
Dart dDir;
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
......@@ -60,7 +90,7 @@ public:
static float neighborDist_;
static float neighborDistSq_;
static float maxSpeed_;
float obstacle_range;
// float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
float color1;
......@@ -71,9 +101,13 @@ public:
VEC3 newVelocity_;
VEC3 prefVelocity_;
Simulator* sim_;
bool rigid_;
bool spinning;
ArticulatedObstacle * parent;
int index_parent;
MovingMesh* mm_;
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
};
#endif
include/obstacle.h
View file @ cb708f28
......@@ -6,7 +6,7 @@
class Obstacle
{
public:
Obstacle(const VEC3& point1, const VEC3& point2, const VEC3& prevPoint, const VEC3& nextPoint,
Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint,
MovingObstacle * moving1, unsigned int ind) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind)
......@@ -17,10 +17,10 @@ public:
// nextP[2] = 0 ;
}
const VEC3& p1 ;
const VEC3& p2 ;
const VEC3& prevP ;
const VEC3& nextP ;
VEC3 p1 ;
VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
MovingObstacle * mo ;
unsigned int index ;
} ;
......
include/path_finder.hpp
View file @ cb708f28
......@@ -83,8 +83,6 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
// inCurrentFace = map.phi1(inCurrentFace) ;
// } while (inCurrentFace != currentFace) ;
} while (facesToTry.size() > 0 && !map.sameFace(start, currentFace)) ;
// If a path has been found, push all predecessors from start to stop
......@@ -96,6 +94,13 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
path.push_back(toPush) ;
toPush = faceNextFaceToGoal[toPush] ;
} while (toPush != goal) ;
// Dart dS = path[0];
// do
// {
// dS = map.phi1(dS);
// } while(dS!=path[0] || !map.sameFace(map.phi2(dS),path[1]));
// path[0] = dS;
}
else
{
......
include/simulator.h
View file @ cb708f28
......@@ -80,11 +80,11 @@ public:
class Simulator
{
public:
Simulator(unsigned int config, unsigned int minSize) ;
Simulator(unsigned int config, unsigned int minSize, unsigned int nbAgent=1000, unsigned int nbObst=20) ;
~Simulator() ;
void init(float dimension, bool enablePathFinding = false) ;
void init(float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding = false) ;
void doStep() ;
......@@ -120,6 +120,7 @@ public:
EnvMap envMap_ ;
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
std::vector<MovingMesh*> movingMeshes_; //for visualization only
int minSize;
unsigned int config;
float timeStep_ ;
......
include/socialAgents.ui
View file @ cb708f28
......@@ -129,6 +129,19 @@
<item>
<widget class="QSpinBox" name="check_slide"/>
</item>
<item>
<widget class="QCheckBox" name="check_pos">
<property name="text">
<string>Draw X line</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="check_x_val"/>
</item>
<item>
<widget class="QSpinBox" name="check_y_val"/>
</item>
<item>
<widget class="QCheckBox" name="check_elipse">
<property name="text">
......
include/viewer.h
View file @ cb708f28
......@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents(unsigned int config, unsigned int minSize, unsigned int iterations) ;
SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent , unsigned int nbObst , unsigned int iterations) ;
void initGUI() ;
......@@ -103,8 +103,11 @@ public:
bool drawObstPredictionTri ;
bool drawObstPath ;
bool draw_dart;
bool draw_posX;
bool draw_elipse;
unsigned int dartSlider;
int posXSlider;
int posYSlider;
public slots:
void animate() ;
......@@ -183,9 +186,24 @@ public slots:
dartSlider = i;
updateGL();
}
void slot_posX(bool b)
{
draw_posX = b ;
updateGL() ;
}
void slot_slide2(int i)
{
posXSlider = i;
updateGL();
}
void slot_slide3(int i)
{
posYSlider = i;
updateGL();
}
void slot_elipse(bool b)
{
draw_elipse = b;
updateGL();
}
{
draw_elipse = b;
updateGL();
}
} ;
src/agent.cpp
View file @ cb708f28
......@@ -10,12 +10,15 @@
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 5;
unsigned int Agent::maxMovingObstacles_ = 10;
float Agent::averageMaxSpeed_ = 2.0f ;
float Agent::neighborDist_ = 10.0f ;
// float Agent::averageMaxSpeed_ = 20.0f ;
// float Agent::neighborDist_ = 10.0f ;
float Agent::neighborDist_ = 20.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ;
//float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizon_ = 100.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
......@@ -37,7 +40,6 @@ Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
sim_(sim),
alive(true)
{
init(start,goal);
}
......@@ -202,34 +204,32 @@ void Agent::updateObstacleNeighbors()
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// CGoGNout<<"obst trouvé : " << distSq;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
&& distSq < rangeSq_)
{
// CGoGNout<<"obst assez pres :";
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
// CGoGNout<<"obst bien orienté";
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
if (distSq > maxDistObst)
maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
// CGoGNout<<CGoGNendl;
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
&& distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
if (distSq > maxDistMovingObst)
maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
......@@ -237,6 +237,7 @@ void Agent::updateObstacleNeighbors()
}
}
}
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
if ((*it)->mo==NULL)
......@@ -323,6 +324,11 @@ void Agent::update()
#else
VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
part_.move(target) ;
#ifdef SECURED
if(target != part_.getPosition())
std::cout << "Problem agent " << agentNo << " (position : " << part_.getPosition() << ") time " << sim_->globalTime_ << std::endl;
#endif
#endif
meanDirection_.set(0) ;
......@@ -530,19 +536,32 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
return(nb_mos);
}
void Agent::computeNewVelocity() // de arash
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
void Agent::computeNewVelocity()
{
// The objective is to compute the sum of forces exerted on the agent.
double collision_softening_factor;
float ag_ambient_damping = 1.0;
double mass_var = 0.9;
double mass_var = 0.95;
double average_mass = 1.0;
// double average_radius = 20.f;
// double radius_var = 0.9;
// range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
// rangeSq_ = range_ * range_ ;
srand48(agentNo);
double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
// double ag_mass = average_mass + rand*rand*rand*mass_var; // valeurs plus denses autour de la moyenne
double ag_mass = average_mass + rand*mass_var; // valeurs uniformement réparties entre min et max
double ag_mass = average_mass*(1 + rand*mass_var); // valeurs uniformement réparties entre min et max
/*
rand = 2.0*drand48()-1.0; // compris entre -1 et 1
radius_ = average_radius + rand*radius_var; // valeurs uniformement réparties entre min et max
range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
rangeSq_ = range_ * range_ ;
*/
//-------------
VEC3 vec, forces, previous_pos;
......@@ -557,14 +576,19 @@ void Agent::computeNewVelocity() // de arash
VEC3 u_goal = p_goal - getPosition();
u_goal.normalize();
// cerr << agentNo << "---------------- position = " << getPosition() << endl;
forces += goal_attraction_force * u_goal;
//----- forces dues à la répulsion des obstacles ----------
// cerr << "goal forces = " << forces << endl;
//----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3 norm;
double obst_stiffness = 5000.0; // agent-obstacle interaction stiffness
// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 1; // the power to which elevate the agent-obstacle distance
int obst_power = 2; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
nb_mos = 0;
......@@ -585,7 +609,6 @@ void Agent::computeNewVelocity() // de arash
{
mo = movingObstacles_[mo_count];
float dist = (getPosition()-mo->focus2).norm() + (getPosition()-mo->focus1).norm();
// double effective_range = 3*mo->sum_dist_foci;
double effective_range = mo->sum_dist_foci;
if(dist <= effective_range)
{
......@@ -606,12 +629,55 @@ void Agent::computeNewVelocity() // de arash
}
}
//----- forces dues à la répulsion des obstacles fixes ----------
// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
// double fixed_obst_damping = 1.0; // agent-obstacle interaction damping
int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance