Path PathFinder::getPath(const Coord& start, const Coord& end) const
{
Path newPath(end);
Direction allDirections[8] = { 0, 1, 2, 3, 4, 5, 6, 7 };
unsigned int currentGScore = g_score(end);
while( newPath.getFinish() != start )
{
std::random_shuffle( allDirections, allDirections + 8 );
for (unsigned i = 0;; ++i)
{
assert(i < 8);
Direction d = allDirections[i];
Coord next = newPath.getFinish().next(d);
if ( m_closedSet.has(next) || m_openSet.has(next) )
{
unsigned int newG = currentGScore - m_field.passingTime( next, (d + 4) % 8 );
if (newG == g_score(next))
{
newPath.append(d);
currentGScore = newG;
break;
}
}
}
}
newPath.reverse();
return newPath;
}
bool PathFinder::makeMap(const Coord& start, Heuristic heur, EndCondition endCondition, Coord* end)
{
m_openSet.clear();
m_closedSet.clear();
std::priority_queue<DistNode> openQueue;
g_score(start) = 0;
h_score(start) = heur(start);
openQueue.push( DistNode( start, h_score(start) ) );
m_openSet.add(start);
while ( !openQueue.empty() )
{
DistNode dn = openQueue.top();
Coord c = dn.coord;
openQueue.pop();
if (g_score(c) + h_score(c) == dn.f_score)
{
// this entry in the queue is valid
assert(!m_closedSet.has(c));
if (endCondition(c))
{
*end = c;
return true;
}
m_openSet.remove(c);
m_closedSet.add(c);
unsigned int currentGScore = g_score(c);
for (int dir = 0; dir < 8; ++dir)
{
Coord next = c.next(dir);
if (m_closedSet.has(next) || !m_field.isPassable(next))
{
continue;
}
unsigned int newG = currentGScore + m_field.passingTime(c, dir);
bool newNode = false;
if (!m_openSet.has(next))
{
h_score(next) = heur(next);
m_openSet.add(next);
newNode = true;
}
if (newNode || newG < g_score(next))
{
g_score(next) = newG;
openQueue.push(DistNode(next, newG + h_score(next))); // may occur more than once, because we cannot remove the old one
}
}
}
}
return false;
}
int
astar(start, goal)
{
int k, min, ind, m;
Fscore = g_score(start) + Hcost(start, goal) //Estimated total cost from start to goal.
openset[i] = start;
i++;
m=i;
neighbor[4] = {0,0,0,0};
while(m != -1)
{
min = F_score[m-1]
for(k=m-2; k>=0; k--)
{
if(min > F_score[k])
{
min = F_score[k];
ind = k;
}
}
current = openset[ind];
if(current == goal)
return reconstruct_path(goal);
i--;
closedset[l] = current;
l++;
neighbor[0] =
neighbor[1] =
neighbor[2] =
neighbor[3] =
alpha
for(k=0; k<alpha; k++)
{
tentative_g_score = g_score(current) + dist_between(current, neighbor);
tentative_f_score = tentative_g_score + Hcost(neighbor, goal)
neighbor_f_score = g_score(neighbor[i]) + Hcost(neighbor[i], goal);
if((search(closedset, neighbor[i]) == 1) && (tentative_f_score >= neighbor_f_score))
continue;
if((search(openset,neighbor[i],) == 0) || (tentative_f_score < neighbor_f_score))
{
came_from[] = current;
if(search(openset, neighbor[i]) == 0)
openset[] = neighbor[i];
}
}
}
return 0;
}
void NavMap::research(Vector &start, Vector &end, Steering *buffer, float width) {
int startMesh = 0;
int endMesh = 0;
bool check = false;
for(unsigned int i = 0; i < meshes.size(); i++) {
if(meshes[i].isIn(start)) {
startMesh = i;
if(check) break;
check = true;
} if(meshes[i].isIn(end)) {
endMesh = i;
if(check) break;
check = true;
}
}
std::map<Node*, Node*> navigate;
std::vector<float> g_score(meshes.size(), 99999999.f);
std::vector<float> f_score(meshes.size(), 99999999.f);
std::vector<Node*> close;
std::vector<Node*> open;
f_score[startMesh] = getH(graph[startMesh].idMesh, endMesh);
g_score[startMesh] = 0;
open.push_back(&graph[startMesh]);
while(!open.empty()) {
for(int i = 0; i < open.size(); i++) {
auto &A = open[i];
A->f = f_score[A->idMesh];
A->g = g_score[A->idMesh];
}
// Tri la liste ouverte
std::sort(open.begin(), open.end(), compareNodes);
// On prend le chemin le plus court pour le moment
Node *u = open.front();
// Si on est arrivé...
if(u->idMesh == endMesh) {
reconstituate(navigate, startMesh, endMesh);
pathVector.emplace(pathVector.begin(), start);
pathVector.push_back(meshes[endMesh].getGravityCenter());
pathMeshes.push_back(endMesh);
pathVector.push_back(end);
break;
}
//On l'enléve de l'open list (on va le traiter) et on l'ajoute à la closed list
open.erase(open.begin());
close.push_back(u);
// Pour chaques voisins
for(unsigned int i = 0; i < u->connected.size(); i++) {
Node &testingNode = graph[graph[u->connected[i].first].idMesh];
// Si déjà dans closed list...
bool flag = false;
for(unsigned int j = 0; j < close.size(); j++) {
if(close[j] == &testingNode) {
flag = true;
break;
}
}
if(flag) continue;
float currentScore = 0;
if(u->idMesh != startMesh)
currentScore = g_score[u->idMesh] + u->connected[i].second;
else
currentScore = g_score[u->idMesh] + (meshes[startMesh].getGravityCenter() - start).length();
// Si déjà dans open list
for(int j = 0; j < open.size(); j++) {
if(open[j] == &testingNode) {
flag = true;
}
}
if(!flag)
open.push_back(&testingNode);
else if(currentScore >= g_score[testingNode.idMesh])
continue;
navigate[&testingNode] = u;
g_score[testingNode.idMesh] = currentScore;
f_score[testingNode.idMesh] = currentScore + getH(testingNode.idMesh, endMesh);
}
}
funnel(start, end);
buffer->vectors.swap(funnelVector);
}
