Commit 6eea98b7 authored by pitiot's avatar pitiot
Browse files

merge génie logiciel

parent c8fc0e29
......@@ -336,9 +336,12 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
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)));
addElementToVector<Obstacle*>(envMap.obstvect[d],o);
if(!map.isBoundaryMarked(map.phi2(map.phi1(x))))
addElementToVector<Obstacle*>(envMap.obstvect[map.phi2(map.phi1(x))],o);
if(!map.isBoundaryMarked(map.phi2(map.phi_1(x))))
addElementToVector<Obstacle*>(envMap.obstvect[map.phi2(map.phi_1(x))],o);
}
#endif
......
......@@ -100,9 +100,13 @@ public:
void popAgentInCells(Agent* agent, Dart d) ;
void agentChangeFace(Agent* agent, Dart oldFace) ;
void pushObstacleInCells(Obstacle* o, Dart d) ;
void 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);
void popObstacleInCells(Obstacle* o, int n);
void popObstacleInCells(Obstacle* o, Dart d) ;
void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) ;
void refine() ;
void coarse() ;
void updateMap() ;
......@@ -144,8 +148,8 @@ public:
#ifndef SPATIAL_HASHING
static const unsigned int nbAgentsToSubdivide = 10 ;
static const unsigned int nbAgentsToSimplify = 4 ;
static const unsigned int nbAgentsToSubdivide = 5 ;
static const unsigned int nbAgentsToSimplify = 1 ;
CellMarker<FACE> refineMark ;
CellMarker<FACE> coarsenMark ;
......@@ -205,9 +209,18 @@ void displayMO(Obstacle * o);
* INLINE FUNCTIONS *
**************************************/
template <typename T>
inline void addElementToVector(std::vector<T>& a, T ag)
{
assert(std::find(a.begin(), a.end(), ag) == a.end());
a.push_back(ag) ;
}
template <typename T>
inline bool removeElementFromVector(std::vector<T>& a, T ag)
{
assert(std::find(a.begin(), a.end(), ag) != a.end());
typename std::vector<T>::iterator end = a.template end() ;
for (typename std::vector<T>::iterator it = a.begin(); it != end; ++it)
{
......@@ -254,7 +267,8 @@ 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) ;
addElementToVector<Agent*>(agentvect[d],agent);
// agentvect[d].push_back(agent) ;
// nbAgentsIncrease(d);
Dart dd = d ;
......@@ -263,7 +277,8 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Dart ddd = map.alpha1(map.alpha1(dd)) ;
while (ddd != dd)
{
neighborAgentvect[ddd].push_back(agent) ;
addElementToVector<Agent*>(neighborAgentvect[ddd],agent);
// neighborAgentvect[ddd].push_back(agent) ;
// nbAgentsIncrease(ddd);
ddd = map.alpha1(ddd) ;
}
......@@ -295,6 +310,7 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////
inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& belonging_cells, std::vector<Dart> *neighbor_cells)
{
assert(map.getCurrentLevel() == map.getMaxLevel());
......@@ -410,66 +426,16 @@ 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);
addElementToVector<Obstacle*>(neighborObstvect[d],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) ;
// obstvect[map.phi<12>(d)].push_back(o);
// obstvect[map.phi2(map.phi_1(d))].push_back(o);
// Dart dd = d;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// obstvect[ddd].push_back(o);
// ddd = map.alpha1(ddd);
// }
//inline void EnvMap::pushObstacleInCell(Obstacle* o, Dart d)
//{
// assert(map.getCurrentLevel() == map.getMaxLevel()) ;
// assert(!buildingMark.isMarked(d)) ;
//
// dd = map.phi1(dd);
// } while(dd != d);
}
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) ;
// removeElementFromVector<Obstacle* >(obstvect[map.phi<12>(d)],o);
// removeElementFromVector<Obstacle* >(obstvect[map.phi2(map.phi_1(d))],o);
// Dart dd = d;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// removeElementFromVector<Obstacle* >(obstvect[ddd], o);
// ddd = map.alpha1(ddd);
// }
// dd = map.phi1(dd);
// } while(dd != d);
}
// addElementToVector<Obstacle*>(obstvect[d],o);
//}
inline void EnvMap::clearUpdateCandidates()
{
......
......@@ -10,7 +10,6 @@
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
void register_add(Obstacle* o, int n, const std::vector<Dart>& memo);
class Simulator ;
......@@ -29,11 +28,6 @@ public:
void update();
void register_oneRingAdd(Obstacle * o, Dart d, int n);
void register_update(Obstacle* o, int n);
// void difference_list (std::list<Dart> old_, std::list<Dart> new_);
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> *registering_part;
std::vector<PFP::VEC3> vertices;
......@@ -50,11 +44,6 @@ public:
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
VEC3 front;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
// std::set<std::pair<float, Agent*> > agentNeighbors_ ;
// std::set<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
// std::set<std::pair<float, Obstacle*> > movingObstNeighbors_;
VEC3 finalGoal;
float angle;
......
......@@ -15,6 +15,14 @@ template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
Dart startPos, Dart stopPos, CellMarker<FACE>& bad) ;
//computation of a "Rapidly exploring random tree"
//requires the definition of states for the moving objects
template <typename STATE>
std::vector<VEC3> pathRRT(STATE startState, VEC3 stopPos,
float sampleSize, float epsilon,
STATE (*fctStep)(STATE start, VEC3 goal, float sampleSize)
);
}
}
......
......@@ -109,6 +109,78 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC
return path ;
}
template <typename STATE>
std::vector<VEC3> pathRRT(STATE startState, VEC3 stopPos,
float sampleSize, float epsilon,
STATE (*fctStep)(STATE start, VEC3 goal, float sampleSize)
)
{
//NOT TESTED
std::cout << __FILE__ << " " << __LINE__ << " HAS NEVER BEEN TESTED" << std::endl;
std::vector<VEC3> res;
typedef struct Node Node;
struct Node { STATE node; Node& prev; };
std::list<Node> graph;
//init graph with startState
Node n;
n.node = startState;
graph.push_back(n);
Node last=n;
//repeat until path found
while(VEC3(last.node.getPosition()-stopPos).norm2()>epsilon)
{
//pick random position rPos in space (have to be in valid parameter space)
//(add bias to choose goal as random pos occasionally for faster convergence)
// could be biased using voronoi regions size of previous nodes
VEC3 rPos = rand()%2=0 ? stopPos : VEC3(rand()*100.0f,rand()*100.0f,rand()*100.0f);
//get nearest node in graph
std::cout << __FILE__ << " " << __LINE__ << " to improve : make sorted insert !" << std::endl;
float distMin= std::numeric_limits<float>::max();
Node& nProx = *graph.begin();
for(typename std::list<Node>::iterator it = graph.begin() ; it != graph.end() ; ++it)
{
float dist = VEC3(rPos-it->node.getPosition()).norm2();
if(dist<distMin)
{
distMin = dist;
nProx = *it;
}
}
//compute displacement from this node toward rPos
STATE nextStep = fctStep(nProx.node, rPos, sampleSize);
//if trajectory ok : add rPos to graph
if(nextStep.isValid())
{
Node nNode;
nNode.node = nextStep;
nNode.prev = nProx;
graph.push_back(nNode);
last = nNode;
}
}
//get all position from stop to start and store them
while(last!=n)
{
res.push_back(last.node.getPosition());
last = *(last.prev);
}
std::reverse(res.begin(),res.end());
return res;
}
//namespace
}
......
......@@ -309,11 +309,11 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'
}
// if (sumNorm.norm2()>1.0f) sumNorm.normalize();
// if(sumNorm!=0)CGoGNout<<sumNorm<<"goal vector before :" <<(*goalVector)<< " goal vector after : "<<((*goalVector)+sumNorm)<<CGoGNendl;
//(*goalVector)=((*goalVector)*9.0f+11.0f*sumNorm)/20.0f;
(*goalVector)=((*goalVector)+sumNorm) ;
//(*goalVector).normalize();
// if (sumNorm.norm2()>1.0f) sumNorm.normalize();
// if(sumNorm!=0)CGoGNout<<sumNorm<<"goal vector before :" <<(*goalVector)<< " goal vector after : "<<((*goalVector)+sumNorm)<<CGoGNendl;
//(*goalVector)=((*goalVector)*9.0f+11.0f*sumNorm)/20.0f;
(*goalVector)=((*goalVector)+sumNorm) ;
//(*goalVector).normalize();
}
void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'objectif (sans prendre en compte l'environnement)
......
This diff is collapsed.
......@@ -5,7 +5,7 @@
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 1.0f;
float MovingObstacle::maxSpeed_ = 2.0f;
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float MovingObstacle::timeHorizonObst_ = 10.0f;
......@@ -122,7 +122,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
vertices[(i + 2) % nbVertices], this, i);
obstacles_[i] = o;
update_registration(o);
sim_->envMap_.pushObstacleInCells(o);
}
......@@ -363,8 +363,9 @@ void MovingObstacle::update()
// o->p2 = parts_[(i + 1) % nbVertices]->getPosition();
// o->prevP = parts_[(i - 1 + nbVertices) % nbVertices]->getPosition();
// o->nextP = parts_[(i + 2) % nbVertices]->getPosition();
register_update(o,i);
//
sim_->envMap_.popAndPushObstacleInCells(o,i);
if(sim_->detect_agent_collision)
{
for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
......@@ -400,153 +401,13 @@ void MovingObstacle::update()
}
void register_pop(Obstacle* o, int n)
{
MovingObstacle * mo = o->mo;
if (mo != NULL)
{
for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it)
{
mo->sim_->envMap_.popObstacleInCells(o, *it);
}
for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
{
mo->sim_->envMap_.popObstNeighborInCells(o, *it);
}
}
}
void MovingObstacle::register_oneRingAdd(Obstacle * o, Dart d, int n)
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel());
assert(std::find(sim_->envMap_.obstvect[d].begin(), sim_->envMap_.obstvect[d].end(), o) == sim_->envMap_.obstvect[d].end());
sim_->envMap_.pushObstacleInCells(o, d);
belonging_cells[n].clear();
belonging_cells[n].push_back(d);
neighbor_cells[n].clear();
Dart dd = d;
do
{
Dart ddd = sim_->envMap_.map.alpha1(sim_->envMap_.map.alpha1(dd));
while(ddd != dd)
{
sim_->envMap_.pushObstNeighborInCells(o, ddd);
this->neighbor_cells[n].push_back(ddd);
ddd = sim_->envMap_.map.alpha1(ddd);
}
dd = sim_->envMap_.map.phi1(dd);
} while(dd != d);
}
void register_add(Obstacle* o, int n, const std::vector<Dart>& memo_cross)
{
if(memo_cross.empty())
{
displayMO(o);
}
assert(!memo_cross.empty());
MovingObstacle * mo = o->mo;
mo->belonging_cells[n].clear();
mo->belonging_cells[n] = memo_cross;
for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it)
{
// CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
mo->sim_->envMap_.pushObstacleInCells(o, *it);
}
mo->sim_->envMap_.addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));
for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
{
// CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
mo->sim_->envMap_.pushObstNeighborInCells(o, *it);
}
}
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
{
registering_part->get_memo(pos);
registering_part->move(pos);
d1=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
return (registering_part->get_memo(dest));
}
void MovingObstacle::register_update(Obstacle* o, int n)// maj de l'enregistrement
{
VEC3 p1 = vertices[n];
VEC3 p2 = vertices[(n+1)%nbVertices];
Dart d1=NIL;
Dart d2=NIL;
std::vector<Dart> memo;
// bool modif=false;
// if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
// || p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
// {
memo = getMemoCross(p1,p2,d1);
d2=registering_part->d;
// memo.sort();
// modif=true;
// }
// else if(!sim_->envMap_.map.sameFace(p1->d,p2->d))
// {
// memo = getMemoCross(p1,p2->getPosition());
// memo.sort();
// if(belonging_cells[n] != memo)
// modif=true;
// }
//
// if(modif)
{
register_pop(o, n);
if(sim_->envMap_.map.sameFace(d1,d2))
{
register_oneRingAdd(o , d1 , n);
}
else
{
register_add(o, n, memo);
}
}
}
void update_registration(Obstacle * o)// réenregistre l'obstacle en question
{
MovingObstacle * mo = o->mo;
if (mo != NULL)
{
int n = o->index;
VEC3 p1 = mo->vertices[n];
VEC3 p2 = mo->vertices[(n+1)%(mo->nbVertices)];
Dart d1=NIL;
Dart d2=NIL;
std::vector<Dart> memo;
memo = mo->getMemoCross(p1,p2,d1);
d2=mo->registering_part->d;
if(mo->sim_->envMap_.map.sameFace(d1,d2))
{
mo->register_oneRingAdd(o , d1 , n);
}
else
{
register_add(o, n, memo);
}
}
return (registering_part->move(dest));
}
void resetPartSubdiv(Obstacle* o)
......@@ -558,7 +419,7 @@ void resetPartSubdiv(Obstacle* o)
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->setState(FACE) ;
mo->registering_part->get_memo(pos) ;
mo->registering_part->move(pos) ;
}
}
......@@ -577,11 +438,8 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
}
}
void resetPart(MovingObstacle * mo, Dart d1)
{
if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
mo->registering_part->d = d1;
......@@ -603,12 +461,12 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
{
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
......
......@@ -32,6 +32,11 @@ void Simulator::init( float dimension, bool enablePathFinding)
{
std::cout << "Setup scenario" << std::endl ;
#ifndef SPATIAL_HASHING
// envMap_.subdivideToProperLevel() ;
// envMap_.subdivideAllToMaxLevel();
#endif
switch (config)
{
case 0 :
......@@ -73,12 +78,15 @@ void Simulator::init( float dimension, bool enablePathFinding)
agents_[i]->updateAgentNeighbors() ;
}
#ifndef SPATIAL_HASHING
if (multires) envMap_.subdivideToProperLevel() ;
if (multires)
envMap_.subdivideToProperLevel() ;
// envMap_.subdivideAllToMaxLevel();
#endif
// setupMovingObstacle(40);
}
void Simulator::doStep()
......@@ -97,9 +105,7 @@ void Simulator::doStep()
#endif
movingObstacles_[i]->update() ;
}
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(0) ;
#endif
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
{
if (agents_[i]->alive)
......@@ -160,7 +166,9 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
if (multires) envMap_.updateMap() ;
if (multires)
envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......@@ -294,6 +302,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
movingObstacles_.push_back(mo4);
}
#ifndef SPATIAL_HASHING
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
......@@ -404,7 +413,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
//for generating a random path
// unsigned int dartDistForPath = 50 ;
// mo4->goals_.clear() ;
// Dart dStart = mo4->parts_[0]->d;
// 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)
// {
......
......@@ -283,6 +283,7 @@ void SocialAgents::cb_redraw()
// glLineWidth(1.0f);
}
// // affiche les cases de la map qui ont des obstacles
// glColor3f(1.0f, 0.5f, 0.5f);
// glLineWidth(5.0f);
......@@ -702,7 +703,7 @@ void SocialAgents::animate()
std::ostringstream oss ;
std::ostringstream tmpNb ;
tmpNb << std::setfill('0') << std::setw(4) << nbIterations ;
oss << "./corridor/corrida" << tmpNb.str() << ".pov" ;
oss << "./corridorTake2/takeTwo" << tmpNb.str() << ".pov" ;
std::string chaine = oss.str() ;
// VEC3 agPos = sim.agents_[0]->meanPos_ ;
// agPos[2] = agPos[1] ;
......@@ -758,8 +759,16 @@ void SocialAgents::animate()
// VEC3 camLook(200, 0, 0) ;
//corridor side