Commit de0cd3b6 authored by Thomas's avatar Thomas
Browse files

proprification et correction de la recherche de chemin

>>>>>> .r274
parent 0966f73e
...@@ -61,7 +61,7 @@ class Simulator; ...@@ -61,7 +61,7 @@ class Simulator;
size_t agentNo_; size_t agentNo_;
size_t goalNo_; size_t goalNo_;
Dart nearestDart; // Dart nearestDart;
// std::vector<Dart> includingFaces; // std::vector<Dart> includingFaces;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> * part; CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> * part;
float rangeSq; float rangeSq;
......
...@@ -148,7 +148,7 @@ void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& close ...@@ -148,7 +148,7 @@ void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& close
// sideSize *= 0.2f; // sideSize *= 0.2f;
unsigned int nbBuilding=1000; unsigned int nbBuilding=1000;
float height = sideSize/2.0f; float height = sideSize/2.0f;
unsigned int side = 20; unsigned int side = 3;
generateGrid<PFP,EMBV>(map,position,side,side,sideSize,closeMark); generateGrid<PFP,EMBV>(map,position,side,side,sideSize,closeMark);
Dart dEnd = map.end(); Dart dEnd = map.end();
......
...@@ -5,13 +5,13 @@ ...@@ -5,13 +5,13 @@
#include <GL/glut.h> #include <GL/glut.h>
static void renderPoint(EnvMap& m, Dart& d) //static void renderPoint(EnvMap& m, Dart& d)
{ //{
PFP::VEC3 p = m.position[d]; // PFP::VEC3 p = m.position[d];
glBegin(GL_POINTS); // glBegin(GL_POINTS);
glVertex3f(p[0],p[1],p[2]); // glVertex3f(p[0],p[1],p[2]);
glEnd(); // glEnd();
} //}
static void renderDart(EnvMap& m, Dart d) static void renderDart(EnvMap& m, Dart d)
{ {
...@@ -33,24 +33,24 @@ static void renderFace(EnvMap& m, Dart& d) ...@@ -33,24 +33,24 @@ static void renderFace(EnvMap& m, Dart& d)
}while(dd!=d); }while(dd!=d);
} }
static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim) //static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
{ //{
switch(dim) { // switch(dim) {
case 0 : renderPoint(m,d); break; // case 0 : renderPoint(m,d); break;
case 1 : renderDart(m,d); break; // case 1 : renderDart(m,d); break;
case 2 : renderFace(m,d); break; // case 2 : renderFace(m,d); break;
} // }
} //}
//
static void renderAllPoints(EnvMap& m) //static void renderAllPoints(EnvMap& m)
{ //{
glBegin(GL_POINTS); // glBegin(GL_POINTS);
for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) { // for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
PFP::VEC3 p = m.position[d]; // PFP::VEC3 p = m.position[d];
glVertex3f(p[0],p[1],p[2]); // glVertex3f(p[0],p[1],p[2]);
} // }
glEnd(); // glEnd();
} //}
static void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p) static void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
{ {
......
...@@ -24,6 +24,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3 ...@@ -24,6 +24,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
std::vector<std::pair<float,Dart> > vToDev; std::vector<std::pair<float,Dart> > vToDev;
Dart toDev = stopPos; Dart toDev = stopPos;
vToDev.push_back(std::make_pair(0, toDev));
tablePred[toDev] = toDev;
do { do {
//dev cell //dev cell
//get all vertex-adjacent cells //get all vertex-adjacent cells
...@@ -33,12 +35,12 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3 ...@@ -33,12 +35,12 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
do { do {
ddd = map.alpha1(ddd); ddd = map.alpha1(ddd);
if(ddd!=dd) { if(ddd!=dd) {
if(!map.foreach_dart_of_face(ddd,bad)) { if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) {
//evaluate their cost and push them in the vector to dev //evaluate their cost and push them in the vector to dev
if(tablePred[ddd]==EMBNULL) if(tablePred[ddd]==EMBNULL)
tablePred[ddd]=toDev; tablePred[ddd]=toDev;
std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin(); std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin();
float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd); float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
while(it!=vToDev.end() && (*it).first<costDDD) while(it!=vToDev.end() && (*it).first<costDDD)
++it; ++it;
vToDev.insert(it, std::make_pair(costDDD, ddd)); vToDev.insert(it, std::make_pair(costDDD, ddd));
...@@ -58,6 +60,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3 ...@@ -58,6 +60,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
//if path found : from start to stop -> push all predecessors //if path found : from start to stop -> push all predecessors
if(map.sameOrientedFace(startPos,toDev)) if(map.sameOrientedFace(startPos,toDev))
{ {
std::cout << "found" << std::endl;
Dart toPush=startPos; Dart toPush=startPos;
std::cout << tablePred[startPos] << std::endl; std::cout << tablePred[startPos] << std::endl;
do { do {
...@@ -72,4 +75,4 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3 ...@@ -72,4 +75,4 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
//namespace //namespace
} }
} }
\ No newline at end of file
...@@ -73,6 +73,7 @@ class Simulator ...@@ -73,6 +73,7 @@ class Simulator
//scenarii //scenarii
void setupScenario(); void setupScenario();
void setupTobogganScenario(float rMax,float rMin);
void setupHelicoidScenario(float rMax,float rMin); void setupHelicoidScenario(float rMax,float rMin);
}; };
......
...@@ -8,22 +8,21 @@ Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_( ...@@ -8,22 +8,21 @@ Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(
Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark); part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = d; part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark); part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
...@@ -355,7 +354,6 @@ void Agent::update() ...@@ -355,7 +354,6 @@ void Agent::update()
position_ += velocity_ * sim_->timeStep_; position_ += velocity_ * sim_->timeStep_;
part->move(VEC3(position_[0],position_[1],0)); part->move(VEC3(position_[0],position_[1],0));
nearestDart = part->getCell();
} }
bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result) bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result)
......
...@@ -218,6 +218,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos) ...@@ -218,6 +218,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d) void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
{ {
map.setCurrentLevel(map.getMinLevel()) ;
Dart dd =d; Dart dd =d;
do { do {
if(closeMark.isMarked(dd) /*&& (position[map.phi2(dd)][2]==0.0 || position[map.phi1(map.phi2(dd))][2] ==0.0f)*/) { if(closeMark.isMarked(dd) /*&& (position[map.phi2(dd)][2]==0.0 || position[map.phi1(map.phi2(dd))][2] ==0.0f)*/) {
...@@ -230,6 +231,8 @@ void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d) ...@@ -230,6 +231,8 @@ void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
} }
dd = map.phi1(dd); dd = map.phi1(dd);
} while (dd!=d); } while (dd!=d);
map.setCurrentLevel(map.getMaxLevel()) ;
} }
void EnvMap::getAllFacesOfAgents(Dart d) void EnvMap::getAllFacesOfAgents(Dart d)
...@@ -431,13 +434,15 @@ void EnvMap::subdivideFaces() ...@@ -431,13 +434,15 @@ void EnvMap::subdivideFaces()
for(std::vector<Dart>::iterator it = marked.begin(); it != marked.end(); ++it) for(std::vector<Dart>::iterator it = marked.begin(); it != marked.end(); ++it)
closeMark.mark(map.phi2(*it)) ; closeMark.mark(map.phi2(*it)) ;
map.setCurrentLevel(cur) ;
for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it) for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
{ {
resetAgentInFace(*it) ; resetAgentInFace(*it) ;
agentvect[(*it)->part->d].push_back(*it) ; agentvect[(*it)->part->d].push_back(*it) ;
} }
map.setCurrentLevel(cur) ;
} }
} }
} }
...@@ -684,7 +689,7 @@ VEC3 EnvMap::faceCenter(Dart d) ...@@ -684,7 +689,7 @@ VEC3 EnvMap::faceCenter(Dart d)
void EnvMap::resetAgentInFace(Agent * agent) void EnvMap::resetAgentInFace(Agent * agent)
{ {
// agent->part->state = VERTEX_ORBIT; // agent->part->state = VERTEX_ORBIT;
// agent->part->m_position = position[d]; // agent->part->m_position = position[agent->part->d];
// agent->part->m_position = agent->part->pointInFace(agent->part->d); // agent->part->m_position = agent->part->pointInFace(agent->part->d);
agent->part->m_position = Algo::Geometry::faceCentroid<PFP>(map,agent->part->d,position); agent->part->m_position = Algo::Geometry::faceCentroid<PFP>(map,agent->part->d,position);
......
...@@ -191,7 +191,7 @@ void MyGlutWin::myRedraw() ...@@ -191,7 +191,7 @@ void MyGlutWin::myRedraw()
// glColor3f(i%int(2.0f/3.0f*v.size())/(v.size()/3.0f),i%int(1.0f/3.0f*v.size())/(v.size()/3.0f),i/(v.size()/3.0f)); // glColor3f(i%int(2.0f/3.0f*v.size())/(v.size()/3.0f),i%int(1.0f/3.0f*v.size())/(v.size()/3.0f),i/(v.size()/3.0f));
// glCircle3i(v[i][0],v[i][1],1.5f); // glCircle3i(v[i][0],v[i][1],1.5f);
glPushMatrix(); glPushMatrix();
// Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->nearestDart,sim->envMap.position); // Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position);
VEC3 pBase(sim->agents_[i]->position_); VEC3 pBase(sim->agents_[i]->position_);
VEC3 posR = pBase; VEC3 posR = pBase;
// pl.normal() = -1.0f*pl.normal(); // pl.normal() = -1.0f*pl.normal();
...@@ -340,7 +340,7 @@ void MyGlutWin::myRedraw() ...@@ -340,7 +340,7 @@ void MyGlutWin::myRedraw()
} }
else if(visu==3) { else if(visu==3) {
// renderDart(sim->envMap,sim->envMap.getDart(33701)); // renderDart(sim->envMap,sim->envMap.getDart(33701));
// renderDart(sim->envMap,sim->envMap.getDart(452)); // renderDart(sim->envMap,sim->envMap.getDart(2));
// renderCellOfDim(sim->envMap,sim->envMap.getDart(1257),2); // renderCellOfDim(sim->envMap,sim->envMap.getDart(1257),2);
// glBegin(GL_POINTS); // glBegin(GL_POINTS);
// glVertex3f(-494.518,-672,0); // glVertex3f(-494.518,-672,0);
...@@ -474,7 +474,7 @@ bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::s ...@@ -474,7 +474,7 @@ bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::s
std::vector<PFP::VEC3> v = sim->getAgentsPosition(); std::vector<PFP::VEC3> v = sim->getAgentsPosition();
for(unsigned int i = 0; i< sim->agents_.size() ; ++i) { for(unsigned int i = 0; i< sim->agents_.size() ; ++i) {
out << "cylinder {" << std::endl; out << "cylinder {" << std::endl;
Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->nearestDart,sim->envMap.position); Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position);
VEC3 pBase(sim->agents_[i]->position_); VEC3 pBase(sim->agents_[i]->position_);
VEC3 posR; VEC3 posR;
pl.normal() = -1.0f*pl.normal(); pl.normal() = -1.0f*pl.normal();
......
...@@ -72,7 +72,6 @@ class ParticleCell2D : public ParticleBase ...@@ -72,7 +72,6 @@ class ParticleCell2D : public ParticleBase
void move(const VEC3& newCurrent) void move(const VEC3& newCurrent)
{ {
if(!Geom::arePointsEquals(newCurrent, m_position)) { if(!Geom::arePointsEquals(newCurrent, m_position)) {
// std::cout << "move to : " << newCurrent << std::endl;
prevPos = m_position; prevPos = m_position;
switch(state) { switch(state) {
...@@ -80,6 +79,8 @@ class ParticleCell2D : public ParticleBase ...@@ -80,6 +79,8 @@ class ParticleCell2D : public ParticleBase
case EDGE_ORBIT : edgeState(newCurrent); break; case EDGE_ORBIT : edgeState(newCurrent); break;
case FACE_ORBIT : faceState(newCurrent); break; case FACE_ORBIT : faceState(newCurrent); break;
} }
display();
} }
} }
......
...@@ -200,6 +200,7 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current) ...@@ -200,6 +200,7 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
edgeState(current); edgeState(current);
return; return;
case Geom::RIGHT : case Geom::RIGHT :
std::cout << std::setprecision(10);
std::cout << "smthg went bad " << m_position << " " << current << std::endl; std::cout << "smthg went bad " << m_position << " " << current << std::endl;
std::cout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << std::endl; std::cout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << std::endl;
m_position = intersectLineEdge(current,m_position,d); m_position = intersectLineEdge(current,m_position,d);
......
...@@ -163,7 +163,7 @@ bool arePointsEquals(const VEC3& point1, const VEC3& point2) ...@@ -163,7 +163,7 @@ bool arePointsEquals(const VEC3& point1, const VEC3& point2)
{ {
VEC3 v(point1 - point2); VEC3 v(point1 - point2);
#define PRECISION 1e-20 #define PRECISION 1e-6
return v.norm2() <= PRECISION ; return v.norm2() <= PRECISION ;
#undef PRECISION #undef PRECISION
} }
......
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