Commit a738b569 authored by Arash HABIBI's avatar Arash HABIBI
Browse files

rayons physiques aleatoires pour les agents

parents bfe587d3 cc85941a
......@@ -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);
......
......@@ -113,7 +113,9 @@ 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) ;
......@@ -178,13 +180,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], 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)
......
......@@ -29,7 +29,7 @@ public:
void draw();
std::vector<VEC3> computeProjectedPointSet(float maxHeight);
std::vector<VEC3> jarvisConvexHull(std::vector<VEC3> projectedPointSet);
std::vector<VEC3> jarvisConvexHull(const std::vector<VEC3>& projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
PFP::MAP& motherMap;
......@@ -38,6 +38,8 @@ public:
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
float scaleValue;
ShapeMatchingQuadratic<PFP> * smg;
std::vector<VEC3> skeleton;
......
......@@ -16,7 +16,7 @@ class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, ArticulatedObstacle * art=NULL, int indParent=-1);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -55,10 +55,8 @@ public:
// float rigidity;
float gravity_dist;
// float gravity_dist;
VEC3 center;
int nb_agents_voisins;
int nb_register_cells;
int index;
int max_x_ind;
int min_x_ind;
......@@ -77,6 +75,7 @@ public:
std::vector<VEC3> goals_ ;
unsigned int curGoal_;
Dart dDir;
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
......@@ -84,7 +83,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;
......
......@@ -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
{
......
......@@ -13,7 +13,8 @@ unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 10;
float Agent::averageMaxSpeed_ = 2.0f ;
// float Agent::averageMaxSpeed_ = 20.0f ;
float Agent::neighborDist_ = 10.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 ;
......@@ -543,12 +544,20 @@ void Agent::computeNewVelocity()
float ag_ambient_damping = 1.0;
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;
......@@ -563,11 +572,11 @@ void Agent::computeNewVelocity()
VEC3 u_goal = p_goal - getPosition();
u_goal.normalize();
cerr << agentNo << "---------------- position = " << getPosition() << endl;
// cerr << agentNo << "---------------- position = " << getPosition() << endl;
forces += goal_attraction_force * u_goal;
cerr << "forces = " << forces << endl;
// cerr << "goal forces = " << forces << endl;
//----- forces dues à la répulsion des obstacles en mouvement ----------
......@@ -596,7 +605,6 @@ void Agent::computeNewVelocity()
{
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)
{
......@@ -652,21 +660,21 @@ void Agent::computeNewVelocity()
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
forces += force_value * norm;
cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
// cerr << "force_value = " << force_value << endl;
// cerr << "norm=" << norm << endl;
// cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
nobst++;
}
//----- forces dues à la répulsion des autres agents -------
// double ag_stiffness = 500.0; // agent-agent interaction stiffness
double ag_stiffness = 500.0; // agent-agent interaction stiffness
double ag_stiffness = 200.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping
double ag_phys_radius_coef = 20.0;
double ag_power = 1; // the power to which elevate the agent-agent distance
rand = 2.0*drand48()-1.0; // compris entre -1 et 1
double radius_var = 0.8;
double ag_phys_radius_coef = 8.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max
unsigned int nbA = 0 ;
for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
......@@ -698,7 +706,7 @@ void Agent::computeNewVelocity()
float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
- ag_damping * (dist - previous_dist) / sim_->timeStep_;
cerr << "autres agents : force_value = " << force_value << endl;
// cerr << "autres agents : force_value = " << force_value << endl;
forces += force_value * u_other;
}
......
......@@ -10,7 +10,7 @@ ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentI
{
std::vector<VEC3> goal;
goal.push_back(members[i-1]->center);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal, true, true, this,i);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal, true, true, NIL, this,i);
members.push_back(mo4);
}
......
......@@ -554,7 +554,7 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
}
}
void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
MovingObstacle * mo = o->mo;
......@@ -593,6 +593,8 @@ void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
pushObstacleInCells(o, n, memo);
}
}
return d1;
}
void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
......
......@@ -15,23 +15,26 @@ constrainedV(map)
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
normal = map.addAttribute<VEC3, VERTEX>("normal");
scale(4.0f);
float area = Algo::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::min(area/1400.0f,100.0f);
std::cout << "scaleVal " << scaleValue << std::endl;
// scaleValue = 4.0f;
scale(scaleValue);
// scale(2.5f);
// scale(0.5f);
VEC3 v = Algo::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition) ;
translate(v);
TraversorV<PFP::MAP> tv(map);
std::vector<Dart> obstDarts;
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
obstDarts.push_back(dd);
}
smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
smg->initialize();
// TraversorV<PFP::MAP> tv(map);
// std::vector<Dart> obstDarts;
// for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
// {
// obstDarts.push_back(dd);
// }
//
// smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
// smg->initialize();
}
void MovingMesh::linkWithObstacle(MovingObstacle * mo)
......@@ -128,10 +131,10 @@ void MovingMesh::animate()
void MovingMesh::draw()
{
glColor3f(0,1,0);
Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
1.0, position,
normal) ;
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
// 1.0, position,
// normal) ;
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
......@@ -158,8 +161,9 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
return res;
}
std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet)
{
std::cout << "pointSet " << pointSet.size() << std::endl;
assert(pointSet.size()>2);
std::vector<unsigned int> convHull;
......@@ -200,6 +204,11 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
curr = i-1;
convHull.push_back(curr);
}
else
{
std::cout << "Pb : !trouve" << std::endl;
exit(0);
}
} while(curr != minPoint);
......
......@@ -71,10 +71,19 @@ 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 rigid, bool spin, ArticulatedObstacle * art, int indParent) :
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()),
center(0),
index(ind),
goals_(goals),
curGoal_(1),
velocity_factor(0.8f),
color1(1.0f),
color2(1.0f),
color3(1.0f),
seen(false),
newVelocity_(0),
sim_(sim),
rigid_(rigid),
......@@ -82,42 +91,38 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
parent(art),
index_parent(indParent)
{
seen = false;
color1=1.0f;
color2=1.0f;
color3=1.0f;
assert(pos.size() > 2);
// movingObstNeighbors_.reserve(maxNeighbors_);
curGoal_ = 1;
nb_agents_voisins = 0;
nb_register_cells = 0;
float pos_max = 0;
if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]);
velocity_factor = 0.8f;
nbVertices = pos.size();
Dart d = sim_->envMap_.getBelongingCell(pos[0]);
registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d, pos[0], sim_->envMap_.position);
registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, dInside, pos[0], sim_->envMap_.position);
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices];
neighbor_cells = new std::vector<Dart>[nbVertices];
// for (unsigned int i = 0; i < nbVertices; ++i)
position = map.addAttribute<VEC3, VERTEX>("position") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ;
deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
if(!rigid_)
{
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
}
groundFace = map.newFace(nbVertices);
d = groundFace;
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] = pos[i];
deformation[d] = VEC3(0);
velocity[d] = VEC3(0);
if(!rigid_)
velocity[d] = VEC3(0);
center += pos[i];
d = map.phi1(d);
......@@ -139,6 +144,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
dDir=dInside;
}
center /= nbVertices;
......@@ -164,6 +171,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
angle=0;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
......@@ -176,21 +184,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
sim_->envMap_.pushObstacleInCells(o);
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
float it = (center - pos[i]).norm();
if (it > pos_max)
pos_max = it;
}
gravity_dist = pos_max + 5.0f;
obstacle_range = 15.0f * 15.0f;
// float pos_max = 0;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
// float it = (center - pos[i]).norm();
// if (it > pos_max)
// pos_max = it;
// }
// gravity_dist = pos_max + 5.0f;
// obstacle_range = 15.0f * 15.0f;
}
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
return position[d]+deformation[d];
// return position[d]+deformation[d];
return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -414,11 +424,11 @@ void MovingObstacle::update()
VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
//inertie
velocity[dd] = velocity[dd]*0.9f;
velocity[dd] = velocity[dd]*0.90f;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 500.0f;
float rigidity = 40.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
VEC3 f = stretch*(v1/norm)*sim_->timeStep_;
......@@ -498,8 +508,8 @@ void MovingObstacle::update()
map.next(d);
}
if(!rigid_)
center = position[groundFace];
// if(!rigid_)
// center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
......@@ -529,7 +539,10 @@ void MovingObstacle::update()
o->p2 = getDilatedPosition((i+1) % nbVertices);
o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
sim_->envMap_.popAndPushObstacleInCells(o,i);
Dart d = sim_->envMap_.popAndPushObstacleInCells(o,i);
if(i==0)
dDir = d;
if(sim_->detect_agent_collision)
{
......@@ -571,6 +584,7 @@ std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest
registering_part->move(pos);
d1=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
return (registering_part->move(dest));
}
......@@ -624,23 +638,64 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
VEC3 goalVector;
if (index_parent<1)
{
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 5.0f)
if(rigid_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2 = goalVector.norm2() ;
}
// goalVector = goals_[curGoal_] - position[groundFace] ;
if (goalDist2 > maxSpeed_)
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 5.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
}
else
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
// if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
// dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
Dart dStart=dDir;
VEC3 dir = center - position[0];
while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
{
// std::cout << "d " << dDir << std::endl;
// std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// std::cout << "goal vec " << goalVector << std::endl;
dDir = sim_->envMap_.map.phi_1(dDir);
mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
if(dDir==dStart)
break;
}
if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
goalVector = VEC3(0);
// goalVector = mid - center ;
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
// float goalDist2 = goalVector.norm2() ;
// if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_*maxSpeed_;
}
}
}
else
......
#include "simulator.h"
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
timeStep_(0.02f),
timeStep_(0.05f),
globalTime_(0.0f),
nbSteps_(0),
nbUpdates(0),
......@@ -36,6 +36,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
{
case 0 :
setupCircleScenario(nbAgent,nbObst) ;
// addPathsToAgents();
break ;
case 1 :
setupCorridorScenario(nbAgent,nbObst) ;
......@@ -57,7 +58,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
addPathsToAgents();
// SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
addMovingObstacles(nbObst);
addPathToObstacles();
// addPathToObstacles();
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
......@@ -174,8 +175,8 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
// if (multires)
// envMap_.updateMap() ;
if (multires)
envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......@@ -422,15 +423,9 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
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" <<