Commit 60ec330d authored by David Cazier's avatar David Cazier
Browse files

Nettoyage de path finding

parent ee1b831c
......@@ -13,11 +13,7 @@ class Simulator ;
class Agent
{
public:
#ifdef SPATIAL_HASHING
Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) ;
#else
Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) ;
#endif
Agent(Simulator* sim, const VEC3& position, const VEC3& goals) ;
VEC3 getPosition() ;
......
......@@ -8,20 +8,17 @@ namespace PathFinder
{
template <typename PFP>
float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos,
Dart stopPos, CellMarker<FACE>& bad) ;
float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart start, Dart goal) ;
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
Dart startPos, Dart stopPos, CellMarker<FACE>& bad) ;
Dart start, Dart goal, 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)
);
std::vector<VEC3> pathRRT(STATE startState, VEC3 stopPos, float sampleSize, float epsilon,
STATE (*fctStep)(STATE start, VEC3 goal, float sampleSize)) ;
}
......
......@@ -9,176 +9,156 @@ namespace PathFinder
{
template <typename PFP>
float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos,
Dart stopPos, CellMarker<FACE>& bad)
float pathCost(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart start, Dart goal)
{
// float penalty=100000000.0f;
// Dart dd = startPos;
// do
// {
// if(bad.isMarked(map.phi2(dd)))
// {
// penalty*=0.0000001f;
// }
// dd = map.phi1(dd);
// } while(dd!=startPos);
return VEC3(
Algo::Geometry::faceCentroid<PFP>(map, stopPos, position)
- Algo::Geometry::faceCentroid<PFP>(map, startPos, position)).norm2() ;
// +penalty
// +(position[stopPos][2]-position[startPos][2])*1000.0f;
Algo::Geometry::faceCentroid<PFP>(map, goal, position)
- Algo::Geometry::faceCentroid<PFP>(map, start, position)).norm() ;
}
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position,
Dart startPos, Dart stopPos, CellMarker<FACE>& bad)
Dart start, Dart goal, CellMarker<FACE>& isObstacle)
{
std::vector<Dart> path ;
FaceAutoAttribute< float > tablePathCost(map);
FaceAutoAttribute< Dart > tablePred(map);
std::multimap<float, Dart> vToDev ;
CellMarker<FACE> m(map) ;
FaceAutoAttribute<float> facePathCostToGoal(map) ;
FaceAutoAttribute<Dart> faceNextFaceToGoal(map) ;
vToDev.insert(std::make_pair(0, stopPos)) ;
std::multimap<float, Dart> facesToTry ;
m.mark(stopPos) ;
CellMarker<FACE> faceIsTraversed(map) ;
tablePred[stopPos] = stopPos ;
tablePathCost[stopPos] = 0 ;
facePathCostToGoal[goal] = 0 ;
faceNextFaceToGoal[goal] = goal ;
facesToTry.insert(std::make_pair(0, goal)) ;
faceIsTraversed.mark(goal) ;
Dart toDev ;
Dart currentFace ;
do
{
//get all vertex-adjacent cells
toDev = (vToDev.begin())->second ;
vToDev.erase(vToDev.begin()) ;
currentFace = (facesToTry.begin())->second ;
facesToTry.erase(facesToTry.begin()) ;
Dart dd = toDev ;
// Try to traverse adajacent faces of currentFace
Dart inCurrentFace = currentFace ;
do
{
// Dart ddd = map.phi2(dd);
Dart ddd = map.alpha1(map.alpha1(dd)) ;
while (ddd != dd)
Dart nextFace = map.alpha1(map.alpha1(inCurrentFace)) ;
while (nextFace != inCurrentFace)
{
//evaluate their cost and push them in the vector to dev
if (!m.isMarked(ddd))
if (!faceIsTraversed.isMarked(nextFace))
{
m.mark(ddd) ;
if (!bad.isMarked(ddd))
faceIsTraversed.mark(nextFace) ;
if (!isObstacle.isMarked(nextFace))
{
if (tablePred[ddd] == EMBNULL)
{
tablePred[ddd] = dd ;
// tablePred[ddd]=toDev;
tablePathCost[ddd] = tablePathCost[toDev]
+ pathCostSqr<PFP>(map, position, toDev, ddd, bad) ;
float costDDD = pathCostSqr<PFP>(map, position, startPos, ddd, bad)
+ tablePathCost[toDev] ;
vToDev.insert(std::make_pair(costDDD, ddd)) ;
}
else
std::cout << "path_planning : missing case" << std::endl ;
float additionalCost = pathCost<PFP>(map, position, currentFace, nextFace) ;
facePathCostToGoal[nextFace] = facePathCostToGoal[currentFace] + additionalCost ;
faceNextFaceToGoal[nextFace] = currentFace ;
float estimatedCostToGoal = pathCost<PFP>(map, position, nextFace, goal) ;
float estimatedGlobalCost = facePathCostToGoal[nextFace] + estimatedCostToGoal ;
facesToTry.insert(std::make_pair(estimatedGlobalCost, nextFace)) ;
}
}
ddd = map.alpha1(ddd) ;
nextFace = map.alpha1(nextFace) ;
}
inCurrentFace = map.phi1(inCurrentFace) ;
} while (inCurrentFace != currentFace) ;
dd = map.phi1(dd) ;
} while (dd != toDev) ;
} while (vToDev.size() > 0 && !map.sameFace(startPos, toDev)) ;
} while (facesToTry.size() > 0 && !map.sameFace(start, currentFace)) ;
//if path found : from start to stop -> push all predecessors
if (vToDev.size() > 0 && map.sameFace(startPos, toDev))
// If a path has been found, push all predecessors from start to stop
if (map.sameFace(start, currentFace))
{
Dart toPush = startPos ;
Dart toPush = start ;
do
{
path.push_back(toPush) ;
toPush = tablePred[toPush] ;
} while (!map.sameFace(toPush, stopPos)) ;
toPush = faceNextFaceToGoal[toPush] ;
} while (toPush != goal) ;
}
else
std::cout << "fichtre" << std::endl ;
{
std::cout << "pathFindAStar: no path found !" << std::endl ;
exit(1) ;
}
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)
)
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::cout << __FILE__ << " " << __LINE__ << " HAS NEVER BEEN TESTED" << std::endl ;
std::vector<VEC3> res;
std::vector<VEC3> res ;
typedef struct Node Node;
struct Node { STATE node; Node& prev; };
typedef struct Node Node ;
struct Node
{
STATE node ;
Node& prev ;
} ;
std::list<Node> graph;
std::list<Node> graph ;
//init graph with startState
Node n;
n.node = startState;
graph.push_back(n);
Node n ;
n.node = startState ;
graph.push_back(n) ;
Node last=n;
Node last = n ;
//repeat until path found
while(VEC3(last.node.getPosition()-stopPos).norm2()>epsilon)
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);
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;
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 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)
float dist = VEC3(rPos - it->node.getPosition()).norm2() ;
if (dist < distMin)
{
distMin = dist;
nProx = *it;
distMin = dist ;
nProx = *it ;
}
}
//compute displacement from this node toward rPos
STATE nextStep = fctStep(nProx.node, rPos, sampleSize);
STATE nextStep = fctStep(nProx.node, rPos, sampleSize) ;
//if trajectory ok : add rPos to graph
if(nextStep.isValid())
if (nextStep.isValid())
{
Node nNode;
nNode.node = nextStep;
nNode.prev = nProx;
graph.push_back(nNode);
last = nNode;
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)
while (last != n)
{
res.push_back(last.node.getPosition());
last = *(last.prev);
res.push_back(last.node.getPosition()) ;
last = *(last.prev) ;
}
std::reverse(res.begin(),res.end());
std::reverse(res.begin(), res.end()) ;
return res;
return res ;
}
//namespace
......
......@@ -96,7 +96,7 @@ public:
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
void addAgent(const VEC3& start,const std::vector<VEC3>& goals);
void addAgent(const VEC3& start,const VEC3& goals);
#ifndef SPATIAL_HASHING
void addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal);
......
......@@ -13,15 +13,13 @@ float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal) :
#ifdef SPATIAL_HASHING
Agent::Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) :
pos(position),
#else
Agent::Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goals) :
part_(sim->envMap_.map, sim->envMap_.getBelongingCell(position), position, sim->envMap_.position),
part_(sim->envMap_.map, sim->envMap_.getBelongingCell(start), start, sim->envMap_.position),
#endif
curGoal_(0),
goals_(goals),
velocity_(0),
newVelocity_(0),
prefVelocity_(0),
......@@ -32,6 +30,9 @@ Agent::Agent(Simulator* sim, const VEC3& position, const std::vector<VEC3>& goal
#ifdef SPATIAL_HASHING
sim->envMap_.addAgentInGrid(this) ;
#endif
goals_.clear() ;
goals_.push_back(goal) ;
goals_.push_back(start) ;
float ratio = (80.0f + rand() % 20) / 100.0f ;
maxSpeed_ = averageMaxSpeed_ * ratio ; // from 80% to 100% of averageMaxSpeed_
color1 = 1.0f * ratio ; // red = high speed agents
......
......@@ -185,9 +185,9 @@ bool Simulator::reachedGoal()
return true ;
}
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goals)
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
{
agents_.push_back(new Agent(this, start, goals)) ;
agents_.push_back(new Agent(this, start, goal)) ;
}
void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents , unsigned int nbObstacles)
......@@ -216,10 +216,7 @@ void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents ,
VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
VEC3 start = center + v ;
VEC3 goal = center - v ;
std::vector<VEC3> goals;
goals.push_back(start);
goals.push_back(goal);
addAgent(start, goals) ;
addAgent(start, goal) ;
}
VEC3 xSide (5.0f,0.0f,0.0f);
VEC3 ySide (0.0f,10.0f,0.0f);
......@@ -321,17 +318,10 @@ void Simulator::setupCorridorScenario(unsigned int config, unsigned int nbAgents
VEC3 goal(xGoalMin + rand() % xGoalDelta, yGoalMin + rand() % yGoalDelta, 0) ;
// Un agent sur 2 va de droite à gauche
VEC3 tmp ;
if (i % 2 == 1)
{
tmp = goal ;
goal = start ;
start = tmp ;
}
std::vector<VEC3> goals;
goals.push_back(start);
goals.push_back(goal);
addAgent(start, goals) ;
addAgent(start, goal) ;
else
addAgent(goal, start) ;
}
// Départ des obstacles du quart haut sur toute une demi-largeur
......@@ -408,20 +398,12 @@ void Simulator::setupCityScenario(int nbLines, int nbRank)
int xCornerDown = envMap_.geometry.min()[0] + xBorder ;
int yCornerDown = envMap_.geometry.min()[1] + yBorder ;
VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
std::vector<VEC3> goals ;
goals.push_back(-1.0f * posagent) ;
agents_.push_back(new Agent(this, posagent, goals)) ;
for (int i = 0 ; i < nbLines ; ++i)
{
for (int j = 0 ; j < nbRank ; ++j)
{
VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
goals.clear() ;
goals.push_back(-1.0f * posagent) ;
agents_.push_back(new Agent(this, posagent, goals)) ;
addAgent(posagent, -1.0f * posagent) ;
}
}
......@@ -431,7 +413,7 @@ void Simulator::setupCityScenario(int nbLines, int nbRank)
void Simulator::setupScenario(unsigned int nbMaxAgent)
{
/*
* Add agents, specifying their start position, and store their goals on the
* Add agents, specifying their start position, and store their goal on the
* opposite side of the environment.
*/
Dart d = envMap_.map.begin() ;
......@@ -471,10 +453,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
for (unsigned int cury = 0 ; cury < nby ; ++cury)
{
VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
std::vector<VEC3> goals ;
goals.push_back(-1.0f * posagent) ;
goals.push_back(1.0f * posagent) ;
agents_.push_back(new Agent(this, posagent, goals)) ;
addAgent(posagent, -1.0f * posagent) ;
}
}
}
......@@ -690,11 +669,8 @@ bool Simulator::importAgents(std::string filename)
iss >> y ;
iss >> z ;
VEC3 pos(x, y, z) ;
goals.clear() ;
goals.push_back(-1.0f * pos) ;
goals.push_back(pos) ;
agents_.push_back(new Agent(this, pos, goals)) ;
VEC3 posagent(x, y, z) ;
addAgent(posagent, -1.0f * posagent) ;
}
}
......
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