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); }