Skip to content

GitLab

Commit de0cd3b6 authored by Thomas's avatar Thomas
Browse files

proprification et correction de la recherche de chemin 

>>>>>> .r274
parent 0966f73e
Hide whitespace changes
Inline Side-by-side
Apps/Examples/SocialAgents/include/agent.h
View file @ de0cd3b6
......@@ -61,7 +61,7 @@ class Simulator;
size_t agentNo_;
size_t goalNo_;
Dart nearestDart;
// Dart nearestDart;
// std::vector<Dart> includingFaces;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> * part;
float rangeSq;
......
Apps/Examples/SocialAgents/include/env_generator.hpp
View file @ de0cd3b6
......@@ -148,7 +148,7 @@ void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& close
// sideSize *= 0.2f;
unsigned int nbBuilding=1000;
float height = sideSize/2.0f;
unsigned int side = 20;
unsigned int side = 3;
generateGrid<PFP,EMBV>(map,position,side,side,sideSize,closeMark);
Dart dEnd = map.end();
......
Apps/Examples/SocialAgents/include/env_render.h
View file @ de0cd3b6
......@@ -5,13 +5,13 @@
#include <GL/glut.h>
static void renderPoint(EnvMap& m, Dart& d)
{
PFP::VEC3 p = m.position[d];
glBegin(GL_POINTS);
glVertex3f(p[0],p[1],p[2]);
glEnd();
}
//static void renderPoint(EnvMap& m, Dart& d)
//{
// PFP::VEC3 p = m.position[d];
// glBegin(GL_POINTS);
// glVertex3f(p[0],p[1],p[2]);
// glEnd();
//}
static void renderDart(EnvMap& m, Dart d)
{
......@@ -33,24 +33,24 @@ static void renderFace(EnvMap& m, Dart& d)
}while(dd!=d);
}
static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
{
switch(dim) {
case 0 : renderPoint(m,d); break;
case 1 : renderDart(m,d); break;
case 2 : renderFace(m,d); break;
}
}
static void renderAllPoints(EnvMap& m)
{
glBegin(GL_POINTS);
for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
PFP::VEC3 p = m.position[d];
glVertex3f(p[0],p[1],p[2]);
}
glEnd();
}
//static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
//{
// switch(dim) {
// case 0 : renderPoint(m,d); break;
// case 1 : renderDart(m,d); break;
// case 2 : renderFace(m,d); break;
// }
//}
//
//static void renderAllPoints(EnvMap& m)
//{
// glBegin(GL_POINTS);
// for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
// PFP::VEC3 p = m.position[d];
// glVertex3f(p[0],p[1],p[2]);
// }
// glEnd();
//}
static void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
{
......
Apps/Examples/SocialAgents/include/path_finder.hpp
View file @ de0cd3b6
......@@ -24,6 +24,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
std::vector<std::pair<float,Dart> > vToDev;
Dart toDev = stopPos;
vToDev.push_back(std::make_pair(0, toDev));
tablePred[toDev] = toDev;
do {
//dev cell
//get all vertex-adjacent cells
......@@ -33,12 +35,12 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
do {
ddd = map.alpha1(ddd);
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
if(tablePred[ddd]==EMBNULL)
tablePred[ddd]=toDev;
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)
++it;
vToDev.insert(it, std::make_pair(costDDD, ddd));
......@@ -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(map.sameOrientedFace(startPos,toDev))
{
std::cout << "found" << std::endl;
Dart toPush=startPos;
std::cout << tablePred[startPos] << std::endl;
do {
......@@ -72,4 +75,4 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
//namespace
}
}
\ No newline at end of file
}
Apps/Examples/SocialAgents/include/simulator.h
View file @ de0cd3b6
......@@ -73,6 +73,7 @@ class Simulator
//scenarii
void setupScenario();
void setupTobogganScenario(float rMax,float rMin);
void setupHelicoidScenario(float rMax,float rMin);
};
......
Apps/Examples/SocialAgents/src/agent.cpp
View file @ de0cd3b6
......@@ -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)
{
nearestDart = 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);
Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
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)
{
nearestDart = d;
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_);
}
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));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
}
......@@ -355,7 +354,6 @@ void Agent::update()
position_ += velocity_ * sim_->timeStep_;
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)
......
Apps/Examples/SocialAgents/src/env_map.cpp
View file @ de0cd3b6
......@@ -218,6 +218,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
{
map.setCurrentLevel(map.getMinLevel()) ;
Dart dd =d;
do {
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)
}
dd = map.phi1(dd);
} while (dd!=d);
map.setCurrentLevel(map.getMaxLevel()) ;
}
void EnvMap::getAllFacesOfAgents(Dart d)
......@@ -431,13 +434,15 @@ void EnvMap::subdivideFaces()
for(std::vector<Dart>::iterator it = marked.begin(); it != marked.end(); ++it)
closeMark.mark(map.phi2(*it)) ;
map.setCurrentLevel(cur) ;
for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
{
resetAgentInFace(*it) ;
agentvect[(*it)->part->d].push_back(*it) ;
}
map.setCurrentLevel(cur) ;
}
}
}
......@@ -684,7 +689,7 @@ VEC3 EnvMap::faceCenter(Dart d)
void EnvMap::resetAgentInFace(Agent * agent)
{
// 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 = Algo::Geometry::faceCentroid<PFP>(map,agent->part->d,position);
......
Apps/Examples/SocialAgents/src/viewer.cpp
View file @ de0cd3b6
......@@ -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));
// glCircle3i(v[i][0],v[i][1],1.5f);
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 posR = pBase;
// pl.normal() = -1.0f*pl.normal();
......@@ -340,7 +340,7 @@ void MyGlutWin::myRedraw()
}
else if(visu==3) {
// 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);
// glBegin(GL_POINTS);
// glVertex3f(-494.518,-672,0);
......@@ -474,7 +474,7 @@ bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::s
std::vector<PFP::VEC3> v = sim->getAgentsPosition();
for(unsigned int i = 0; i< sim->agents_.size() ; ++i) {
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 posR;
pl.normal() = -1.0f*pl.normal();
......
include/Algo/MovingObjects/particle_cell_2D.h
View file @ de0cd3b6
......@@ -72,7 +72,6 @@ class ParticleCell2D : public ParticleBase
void move(const VEC3& newCurrent)
{
if(!Geom::arePointsEquals(newCurrent, m_position)) {
// std::cout << "move to : " << newCurrent << std::endl;
prevPos = m_position;
switch(state) {
......@@ -80,6 +79,8 @@ class ParticleCell2D : public ParticleBase
case EDGE_ORBIT : edgeState(newCurrent); break;
case FACE_ORBIT : faceState(newCurrent); break;
}
display();
}
}
......
include/Algo/MovingObjects/particle_cell_2D.hpp
View file @ de0cd3b6
......@@ -200,6 +200,7 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
edgeState(current);
return;
case Geom::RIGHT :
std::cout << std::setprecision(10);
std::cout << "smthg went bad " << m_position << " " << current << std::endl;
std::cout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << std::endl;
m_position = intersectLineEdge(current,m_position,d);
......
include/Geometry/inclusion.hpp
View file @ de0cd3b6
......@@ -163,7 +163,7 @@ bool arePointsEquals(const VEC3& point1, const VEC3& point2)
{
VEC3 v(point1 - point2);
#define PRECISION 1e-20
#define PRECISION 1e-6
return v.norm2() <= 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