void AStar::reconstructPath(int long long id,int steps){
long long int came_from = came_from_map[id];
if(came_from == -1){
cout<<"Reached Destination"<<endl;
cout<<"Took "<<steps<<" steps to complete"<<endl;
cout<<"Expanded "<<closedSet.size()<<" nodes in the Graph"<<endl;
return;
}
else{
printNode(came_from);
reconstructPath(came_from,steps+1);
return;
}
}
void driverProgram(char *inputFile, char *outputFile)
{
FILE *fin = fopen(inputFile, "r");
FILE *fout = fopen(outputFile, "w");
int source;
graph *G = loadGraph(fin);
int **cost = alocateMemoryForCost(G->noOfVertices);
int *predecesors = allocateMemoryForPredecesors(G->noOfVertices);
printf_s("Give the source vertex:");
scanf_s("%d", &source);
BellmanFord(G, source, cost, predecesors);
for (int i = 0; i < G->noOfVertices; i++)
{
fprintf_s(fout, "%d\n", cost[1][i]);
}
while (true)
{
int destination;
printf_s("Give the vertex to which you want to reconstruct the path:");
scanf_s("%d", &destination);
if (cost[1][destination] == infinite)
{
printf_s("there is no path to %d from %d\n", destination, source);
}
else if (cost[1][destination] == minfinite)
{
printf_s("it is a negative cycle cannot display path\n");
}
else
{
reconstructPath(source, destination, G, predecesors);
}
printf_s("Continue? \n1.Yes\n0.No\n");
int dummy;
scanf_s("%d",&dummy);
if (dummy == 0)
{
break;
}
}
fclose(fin);
fclose(fout);
}
std::vector<Eigen::Vector2i> PathPlanner::reconstructPath(std::map<
std::pair<int, int>, std::pair<int, int> > & cameFrom, std::pair<
int, int> currentNode)
{
std::vector<Eigen::Vector2i> path;
if(cameFrom.find(currentNode) != cameFrom.end())
{
path = reconstructPath(cameFrom, cameFrom[currentNode]);
path.push_back(Eigen::Vector2i(currentNode.first, currentNode.second));
return path;
}
else
{
path.push_back(Eigen::Vector2i(currentNode.first, currentNode.second));
return path;
}
}
/// \brief Analyze the entire solution space starting from \p InitialState.
///
/// This implements a variant of Dijkstra's algorithm on the graph that spans
/// the solution space (\c LineStates are the nodes). The algorithm tries to
/// find the shortest path (the one with lowest penalty) from \p InitialState
/// to a state where all tokens are placed.
void analyzeSolutionSpace(LineState &InitialState) {
std::set<LineState> Seen;
// Insert start element into queue.
StateNode *Node =
new (Allocator.Allocate()) StateNode(InitialState, false, NULL);
Queue.push(QueueItem(OrderedPenalty(0, Count), Node));
++Count;
// While not empty, take first element and follow edges.
while (!Queue.empty()) {
unsigned Penalty = Queue.top().first.first;
StateNode *Node = Queue.top().second;
if (Node->State.NextToken == NULL) {
DEBUG(llvm::dbgs() << "\n---\nPenalty for line: " << Penalty << "\n");
break;
}
Queue.pop();
// Cut off the analysis of certain solutions if the analysis gets too
// complex. See description of IgnoreStackForComparison.
if (Count > 10000)
Node->State.IgnoreStackForComparison = true;
if (!Seen.insert(Node->State).second)
// State already examined with lower penalty.
continue;
addNextStateToQueue(Penalty, Node, /*NewLine=*/false);
addNextStateToQueue(Penalty, Node, /*NewLine=*/true);
}
if (Queue.empty())
// We were unable to find a solution, do nothing.
// FIXME: Add diagnostic?
return;
// Reconstruct the solution.
reconstructPath(InitialState, Queue.top().second);
DEBUG(llvm::dbgs() << "Total number of analyzed states: " << Count << "\n");
DEBUG(llvm::dbgs() << "---\n");
}
std::list<Node *> PathFinder::findPathFromTo(Node * from, Node * to)
{
Score * current,
neighbour;
//Nettoyage des exécutions précédantes
processMemoryCleaning();
m_bGoalReached = false;
this->setGoal(to);
//Initialisation du noeud de départ
this->setStartingNode(from);
m_StartingNode.m_dGScore = 0;
m_StartingNode.m_dFScore = m_StartingNode.m_dGScore + this->computeHScore(m_StartingNode);
m_OpenSet.push_back(m_ScorePool->getAvailableScore(m_StartingNode));
//Tant que le vecteur de noeuds à explorer n'est pas vide et que l'on n'a pas attent l'objectif
while(!m_OpenSet.empty() && !m_bGoalReached)
{
//On récupère le premier élément dans la liste des noeuds dont le parcours a été planifié
current = m_OpenSet.front();
//L'objectif est atteint
if(current->m_N == m_Goal.m_N)
{
m_bGoalReached = true;
m_Goal.m_Father = current->m_Father;
}
//Sinon...
else
{
//On retire le premier élément des noeuds à parcourir
m_OpenSet.erase(m_OpenSet.begin());
m_ClosedSet.push_back(m_ScorePool->getAvailableScore((*current)));
//Parcours des voisins du noeuds courrant
std::list<Node *> neighboursList = (current->m_N)->neighbours();
for(auto itCurrentNeighbour = neighboursList.begin(); itCurrentNeighbour != neighboursList.end(); ++itCurrentNeighbour)
{
neighbour.m_N = (*itCurrentNeighbour);
//Pour chaque voisin on test s'il n'a pas déjà été exploré
if(std::find_if(m_ClosedSet.begin(), m_ClosedSet.end(), Score::CompareScoreByPointer(&neighbour)) == m_ClosedSet.end())
{
double possibleGScore = current->m_dGScore + computeMovingCost((*current), neighbour);
std::vector<Score *>::iterator ite = std::find_if(m_OpenSet.begin(), m_OpenSet.end(), Score::CompareScoreByPointer(&neighbour));
//Si le voisin en cours n'a pas été exploré ou qu'il se trouve dans liste des noeuds à parcourir
//et que le nouveau coût pour l'atteindre a diminué
if(ite == m_OpenSet.end() || possibleGScore < (*ite)->m_dGScore)
{
neighbour.m_Father = current;
neighbour.m_dGScore = possibleGScore;
neighbour.m_dFScore = neighbour.m_dGScore + (this->computeHScore(neighbour)); //TODO Possibilité d'ajouter un tie breaker ici
if(ite == m_OpenSet.end())
m_OpenSet.push_back(m_ScorePool->getAvailableScore(neighbour));
}
}
}
}
//Tri de la liste des noeuds à parcourir par ordre croissant de f score
std::make_heap(m_OpenSet.begin(), m_OpenSet.end(), CompareScore);
std::sort_heap(m_OpenSet.begin(), m_OpenSet.end(), CompareScore);
}
return reconstructPath();
}
bool PathPlanner::aStar(Eigen::Vector2i start, Eigen::Vector2i end, std::vector<
Eigen::Vector2i> & path)
{
std::map<std::pair<int, int>, std::pair<int, int> > cameFrom;
bool ** closedSet = new bool *[image.cols];
for(int i = 0; i < image.cols; i++)
{
closedSet[i] = new bool[image.rows];
for(int j = 0; j < image.rows; j++)
{
closedSet[i][j] = false;
}
}
bool ** openSet = new bool *[image.cols];
for(int i = 0; i < image.cols; i++)
{
openSet[i] = new bool[image.rows];
for(int j = 0; j < image.rows; j++)
{
openSet[i][j] = false;
}
}
float ** gScore = new float *[image.cols];
for(int i = 0; i < image.cols; i++)
{
gScore[i] = new float[image.rows];
for(int j = 0; j < image.rows; j++)
{
gScore[i][j] = 0;
}
}
float ** fScore = new float *[image.cols];
for(int i = 0; i < image.cols; i++)
{
fScore[i] = new float[image.rows];
for(int j = 0; j < image.rows; j++)
{
fScore[i][j] = 0;
}
}
openSet[start(0)][start(1)] = true;
gScore[start(0)][start(1)] = 0;
fScore[start(0)][start(1)] = gScore[start(0)][start(1)]
+ heuristic(start, end);
std::priority_queue<std::pair<float, Eigen::Vector2i>,
std::vector<std::pair<int, Eigen::Vector2i> >,
PathPlanner::Comparator> queue;
queue.push(std::pair<float, Eigen::Vector2i>(0, start));
while(!queue.empty())
{
Eigen::Vector2i current = queue.top().second;
queue.pop();
int x = current(0);
int y = current(1);
if(current == end)
{
path =
reconstructPath(cameFrom, std::pair<int, int>(end(0), end(1)));
for(int i = 0; i < image.cols; i++)
{
delete[] closedSet[i];
}
delete[] closedSet;
for(int i = 0; i < image.cols; i++)
{
delete[] openSet[i];
}
delete[] openSet;
for(int i = 0; i < image.cols; i++)
{
delete[] gScore[i];
}
delete[] gScore;
for(int i = 0; i < image.cols; i++)
{
delete[] fScore[i];
}
delete[] fScore;
return true;
}
openSet[x][y] = false;
closedSet[x][y] = true;
std::vector<Eigen::Vector2i> neighbours = getNeighbours(current);
for(size_t i = 0; i < neighbours.size(); i++)
{
int nx = neighbours.at(i)(0);
int ny = neighbours.at(i)(1);
if(closedSet[nx][ny])
continue;
float gTentative = gScore[x][y]
+ heuristic(current, neighbours.at(i));
if(!openSet[nx][ny] || gTentative < gScore[nx][ny])
{
cameFrom[std::pair<int, int>(neighbours.at(i)(0), neighbours.at(i)(1))] =
std::pair<int, int>(current(0), current(1));
gScore[nx][ny] = gTentative;
fScore[nx][ny] = gScore[nx][ny]
+ heuristic(neighbours.at(i), end);
openSet[nx][ny] = true;
queue.push(std::pair<float, Eigen::Vector2i>(fScore[nx][ny], neighbours.at(i)));
}
}
}
for(int i = 0; i < image.cols; i++)
{
delete[] closedSet[i];
}
delete[] closedSet;
for(int i = 0; i < image.cols; i++)
{
delete[] openSet[i];
}
delete[] openSet;
for(int i = 0; i < image.cols; i++)
{
delete[] gScore[i];
}
delete[] gScore;
for(int i = 0; i < image.cols; i++)
{
delete[] fScore[i];
}
delete[] fScore;
return false;
}
std::pair<std::vector<StateOfCar>, Seed> AStarSeed::findPathToTargetWithAstar(const cv::Mat& img,const State& start,const State& goal) {
// USE : for garanteed termination of planner
int no_of_iterations = 0;
fusionMap = img;
MAP_MAX_ROWS = img.rows;
MAP_MAX_COLS = img.cols;
if(DT==1)
distanceTransform();
if (DEBUG) {
image = fusionMap.clone();
}
StateOfCar startState(start), targetState(goal);
std::map<StateOfCar, open_map_element> openMap;
std::map<StateOfCar,StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> openSet;
openSet.push(startState);
if (startState.isCloseTo(targetState)) {
std::cout<<"Bot is On Target"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(startState)){
std::cout<<"Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(targetState)){
std::cout<<"Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!openSet.empty() && ros::ok()) {
// std::cout<<"openSet size : "<<openSet.size()<<"\n";
if(no_of_iterations > MAX_ITERATIONS){
std::cerr<<"Overflow : openlist size : "<<openSet.size()<<"\n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar currentState=openSet.top();
if (openMap.find(currentState)!=openMap.end() && openMap[currentState].membership == CLOSED) {
openSet.pop();
}
currentState=openSet.top();
if (DEBUG) {
std::cout<<"current x : "<<currentState.x()<<" current y : "<<currentState.y()<<std::endl;
plotPointInMap(currentState);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (currentState.isCloseTo(targetState)) {
std::cout<<"openSet size : "<<openSet.size()<<"\n";
// std::cout<<"Target Reached"<<std::endl;
return reconstructPath(currentState, came_from);
}
openSet.pop();
openMap[currentState].membership = UNASSIGNED;
openMap[currentState].cost=-currentState.gCost();
openMap[currentState].membership=CLOSED;
std::vector<StateOfCar> neighborsOfCurrentState = neighborNodesWithSeeds(currentState);
for (unsigned int iterator=0; iterator < neighborsOfCurrentState.size(); iterator++) {
StateOfCar neighbor = neighborsOfCurrentState[iterator];
double tentativeGCostAlongFollowedPath = neighbor.gCost() + currentState.gCost();
double admissible = neighbor.distanceTo(targetState);
double consistent = admissible;
double intensity = fusionMap.at<uchar>(neighbor.y(), neighbor.x());
double consistentAndIntensity = (neighbor.hCost()*neighbor.hCost()+2 + intensity*intensity)/(neighbor.hCost()+intensity+2);
if (!((openMap.find(neighbor) != openMap.end()) &&
(openMap[neighbor].membership == OPEN))) {
came_from[neighbor] = currentState;
neighbor.gCost( tentativeGCostAlongFollowedPath) ;
if(DT==1)
neighbor.hCost(consistentAndIntensity);
else
neighbor.hCost( consistent) ;
neighbor.updateTotalCost();
openSet.push(neighbor);
openMap[neighbor].membership = OPEN;
openMap[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
std::cerr<<"NO PATH FOUND"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
/////////////////////////////////////////
// A* search, This is where it all happens
// returns PS_COMPLETE and sets the m_GP to the complete path
// returns PS_TIMEOUT if a search is taking longer than 5mins
// returns PS_PATHNOTFOUND if there is no path to the goal available
/////////////
PathState pathPlan::AStarSearch()
{
int i,j;
bool newNode,oldNode;
float g_score;
goalPath v_GP;
rankPoint tPoint;
rankPoint midpoint;
QList<rankPoint> prospectPoints; // visible nodes from current location
QList<rankPoint> openSet; // contains all possible nodes to go to
QList<rankPoint> closedSet; // contains all nodes already visited
memset(&midpoint,0,sizeof(rankPoint));
midpoint.point = m_startPoint.point;
midpoint.parentIndex = -1;
midpoint.gScore = 0;
midpoint.hScore = midpoint.point.distance(m_goalPoint.point);
midpoint.fScore = midpoint.gScore + midpoint.hScore;
openSet << midpoint;
this->generateCspace(); // create the C-Space to compute the path in
while(!openSet.isEmpty())
{
if(m_time.elapsed() > 300000) return PS_TIMEOUT; // 5 minute limit and no path to the goal found exit
midpoint = openSet.takeFirst(); // remove node in openSet with the lowest fScore
btCollisionObject *objBlock = this->isRayBlocked(midpoint,m_goalPoint); // is the ray blocked?
if(objBlock == NULL || objBlock == m_goalOccluded) // if the goal is clear or the blocking object is the occluded goal object
{
m_GP = reconstructPath(midpoint,closedSet);
return PS_COMPLETE;
}
closedSet << midpoint; // add current node to already visited set
prospectPoints.clear();
prospectPoints = getVisablePointsFrom(midpoint); // get all visible nodes
for(i=0; i<prospectPoints.size(); i++) // loop through all visible prospect nodes
{
oldNode = false;
for( j = 0; j < closedSet.size(); j++){
if(prospectPoints[i].object == closedSet[j].object && prospectPoints[i].corner == closedSet[j].corner){
oldNode = true;
break;
}
}
if(oldNode) continue; // skip points that have already been visited
g_score = midpoint.gScore + midpoint.point.distance(prospectPoints[i].point);
newNode = true;
for( j = 0; j < openSet.size(); j++){ // check if the prospective node is new or has a lower g score
if(prospectPoints[i].object == openSet[j].object && prospectPoints[i].corner == openSet[j].corner){
newNode = false;
if(g_score < openSet[j].gScore){
openSet[j].parentIndex = closedSet.size()-1;
openSet[j].gScore = g_score;
openSet[j].fScore = openSet[j].gScore + openSet[j].hScore;
}
break;
}
}
if(newNode){ // add new nodes to the open set
prospectPoints[i].parentIndex = closedSet.size()-1;
prospectPoints[i].gScore = g_score;
prospectPoints[i].hScore = m_goalPoint.point.distance(prospectPoints[i].point);
prospectPoints[i].fScore = prospectPoints[i].gScore + prospectPoints[i].hScore;
openSet << prospectPoints[i];
}
}
openSet = quickSortFScoreLessthan(openSet);
if(m_drawSwitch && m_view) // draw the path while searching
{
v_GP = reconstructPath(midpoint,closedSet);
m_pointPath.clear();
m_pointPath = v_GP.points;
m_pointPath.pop_back();
m_view->updateGL();
}
////////////////////////////////////////////
// rover POV sensor visibility switch
if(m_visibilityType && m_range != 0){
if(openSet.isEmpty() || closedSet.isEmpty()){
m_GP.length = m_startPoint.point.distance(m_goalPoint.point);
m_GP.points << m_startPoint << m_goalPoint;
}
else
m_GP = reconstructPath(openSet.first(),closedSet); // build the path
return PS_COMPLETE;
}
}
return PS_PATHNOTFOUND;
}
vector<sf::Vector2f> Enemy::pathToTarget(){
int i,min,idx,my,mx;
float tx;
float ty;
float speedX = 1;
float speedY = 1;
vector<sf::Vector2f> ret;
sf::Vector2f target;
target.x = floor(state->player->sprite.getPosition().x/speedX)*speedX;
target.y = floor(state->player->sprite.getPosition().y/speedY)*speedY;
if(x == target.x && y==target.y)
return ret;
vector<sf::Vector2f> set;
vector<bool> closed;
vector<float> fscore;
vector<float> gscore;
vector<int> came_from;
sf::Vector2f tmp;
tmp.x = x;
tmp.y = y;
set.push_back(tmp);
closed.push_back(false);
came_from.push_back(-1);
//Need function to calculate distance
fscore.push_back(abs(target.x-tmp.x)+abs(target.y-tmp.y));
gscore.push_back(0);
int count=0;
while(count<=80){
++count;
//printf("COUNT: %d SIZE: %d\n",count,(int)set.size());
for(i=0;i<(int)closed.size();i++){
if(closed[i]==false){
idx = i;
min = fscore[idx];
break;
}
}
//printf("IDX: %d\n",idx);
if(i==(int)closed.size()) break;
for(i=idx+1;i<(int)fscore.size();i++){
if(fscore[i] < min && closed[i]==false){
min = fscore[i];
idx = i;
}
}
//printf("IDX: %d\n",idx);
if(target.x==set[idx].x && target.y==set[idx].y){
return reconstructPath(came_from,set,idx);
}
sf::Vector2f current;
current.x = floor(set[idx].x/speedX)*speedX;
current.y = floor(set[idx].y/speedY)*speedY;
closed[idx]=true;
for(ty = current.y-speedY; ty <= current.y+speedY;ty+=speedY){
if(ty<0) continue;
if(ty>state->windowHeight) continue;
for(tx = current.x-speedX; tx <= current.x+speedX;tx+=speedX){
if(tx<0) continue;
if(tx>state->windowWidth) continue;
sf::FloatRect *bounds = new sf::FloatRect(tx,ty,sprite.getGlobalBounds().width,sprite.getGlobalBounds().height);
for(my=0;my<state->tilemap->height;my++){
for(mx=0;mx<state->tilemap->width;mx++){
if(state->tilemap->rawMap[my][mx]->type==WALL && bounds->intersects(state->tilemap->rawMap[my][mx]->sprite.getGlobalBounds())) break;
}
if(mx<state->tilemap->width) break;
}
if(my<state->tilemap->height) continue;
for(i=0;i<(int)set.size();i++){
if(set[i].x==tx && set[i].y==ty){
break;
}
}
if(i==(int)set.size()){
tmp.x = tx;
tmp.y = ty;
//Add distance function
set.push_back(tmp);
gscore.push_back(gscore[idx]+1);
came_from.push_back(idx);
fscore.push_back(abs(target.x-tmp.x)+abs(target.y-tmp.y));
closed.push_back(false);
}
else if(closed[i]==true) continue;
else if(closed[i]==false){
float tmp_g_score = gscore[idx]+1;
if(tmp_g_score < gscore[i]){
gscore[i]=tmp_g_score;
came_from[i]=idx;
fscore[i]=abs(target.x-tmp.x)+abs(target.y-tmp.y);
closed[i]=false;
}
}
}
}
}
return ret;
}
bool AStar::getShortestPath(Node _start, Node _end){
Node goal = _end;
Node start =_start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
while(!(openSet.size() == 0)){
#if DEBUG
printf("Size of open list is %d \n", openSet.size());
#endif
Node current = getMinimumNode(openSet);
#if DEBUG
printf("Expanding node:id= %lld\n", current.id);
#endif
if(current.data == goal.data){
cout<<"\nPath found.Reconstructing the path.\n";
reconstructPath(current.id,0);
return true;
}
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf ("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
Node nodeInClosedList = getNodeFromSet(closedSet,nbr);
int temp_g_score = current.g_score + distance(current,nbr);
if(temp_g_score < nodeInClosedList.g_score){
printf(" Removing a Node from closed list id %lld in previous %lld \n", nodeInClosedList.id,came_from_map[nodeInClosedList.id]);
printf(" New gscore is %d , old gscore is %d new wannabe parent %lld\n",temp_g_score, nodeInClosedList.g_score,current.id );
printf(" hscore for %lld wannabe parent %d \n" ,current.id , H(current,goal));
//printf(" hscore for %lld previous parent \n" ,current.id , H(current,goal));
// reconstructPath(nodeInClosedList.id],0);
//exit(0);
removeNodeFromSet(closedSet,nodeInClosedList);
}
else{
continue;
}
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
}
printf("Solution Not Found. Goal is not reachable from start\n");
return false;
}
neighbourList* aStarPathFinder(node* f_startNode_pst, node* f_endNode_pst)
{
int32 unitGscore_i32 = 1;
int32 gScoreStart_i32 = 0;
int32 hScoreStart_i32;
aStarNode* startAStarNode = createAStarNode(f_startNode_pst);
aStarNode* startCameFrom_pst = NULL;
aStarList* openAStarList_pst = createAStarList();
aStarList* closedAStarList_pst = createAStarList();
if(startAStarNode && openAStarList_pst && closedAStarList_pst)
{
hScoreStart_i32 = heuristicEstimate(f_startNode_pst, f_startNode_pst, f_endNode_pst);
setAllInfoAStarNode(startAStarNode, f_startNode_pst, startCameFrom_pst, gScoreStart_i32, hScoreStart_i32);
addToEndAStarList(openAStarList_pst, startAStarNode);
while(!isAStarListEmpty(openAStarList_pst))
{
aStarNode* LowestFscoreNode_pst = getLowest(openAStarList_pst);
if (f_endNode_pst == LowestFscoreNode_pst ->informationNode_pst)
{
// Reconstruct path, destroy the AStarlists created above
// and return the node list to process
neighbourList* AstarPath_pst = reconstructPath(startAStarNode, LowestFscoreNode_pst);
destroyAStarList(openAStarList_pst);
destroyAStarList(closedAStarList_pst);
return AstarPath_pst;
}
removeFromAStarList(openAStarList_pst, LowestFscoreNode_pst);
addToEndAStarList(closedAStarList_pst, LowestFscoreNode_pst);
node* currentNode_pst = LowestFscoreNode_pst ->informationNode_pst;
neighbourList* NeighbourList_pst = getNeighbourList(currentNode_pst);
if(!isNeighbourListEmpty(NeighbourList_pst))
{
neighbourListNode* nextNeighbour_pst = NULL;
neighbourListNode* neighbour_pst = NeighbourList_pst->head;
for (; neighbour_pst != NULL; neighbour_pst = nextNeighbour_pst)
{
nextNeighbour_pst = neighbour_pst->next;
node* neighbourNode_pst = neighbour_pst ->neighbourNode_pst;
if(searchNeighbourInAStarList(closedAStarList_pst, neighbourNode_pst))
{
continue;
}
int32 neighbourGscore_i32 = LowestFscoreNode_pst ->gScore_i32 + unitGscore_i32;
aStarListNode* searchResult_pst = searchNeighbourInAStarList(openAStarList_pst, neighbourNode_pst);
if (!searchResult_pst)
{
aStarNode* neighbourAStarNode = createAStarNode(neighbourNode_pst);
int32 neighbourHscore_i32 = heuristicEstimate(f_startNode_pst, neighbourNode_pst, f_endNode_pst);
setAllInfoAStarNode(neighbourAStarNode, neighbourNode_pst, LowestFscoreNode_pst,
neighbourGscore_i32, neighbourHscore_i32);
addToEndAStarList(openAStarList_pst, neighbourAStarNode);
}
else if (neighbourGscore_i32 < searchResult_pst ->aStarNode_pst ->gScore_i32)
{
aStarNode* neighbourAStarNode = searchResult_pst ->aStarNode_pst;
setCameFromPtr(neighbourAStarNode, LowestFscoreNode_pst);
setGScore(neighbourAStarNode, neighbourGscore_i32);
setFScore(neighbourAStarNode, neighbourGscore_i32, neighbourAStarNode ->hScore_i32);
}
}
}
destroyNeighbourList(NeighbourList_pst);
}
return NULL;
}
return NULL;
}
std::vector<Point> Pathfinder::findPath(const Map& map, const Point& start, const Point& goal)
{
//closeSet{}
std::vector<std::shared_ptr<Node>> closedSet;
//if dir==4
const int dir = 4;
static int x[dir] = { 1, 0, -1, 0 };
static int y[dir] = { 0, 1, 0, -1 };
//openSet{startNode}
auto startNode = std::make_shared<Node>(start.getX(), start.getY());
auto goalNode = std::make_shared<Node>(goal.getX(), goal.getY());
std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>, ComparePriority> openSet;
openSet.push(startNode);
//startNode.g<-0
startNode->g = 0;
//startNode.h<- EUCLIDEANDISTANCE(startNode, goalNode)
startNode->h = euclideanDistance(start, goal);
//startNode.cameFrom<-null
startNode->cameFrom = nullptr;
//while openSet is not empty do
while (!openSet.empty())
{
//currentNode<- node in openSet with lowest g+h score
auto currentNode = openSet.top();
//auto currentNode = std::make_shared<Node>(start.getX(), start.getY());
//if currentNode = goalNode then
if (currentNode == goalNode)
{
//return RECONSTRUCTPATH(goalNode)
return reconstructPath(goalNode);
}
//end if
//remove currentNode from openSet
openSet.pop();
//add currentNode to closeSet
closedSet.push_back(currentNode);
//for each neighbourNode adjacent to currentNode do
static int i;
for (i = 0; i < dir; i++)
{
double currentXPos = currentNode->point.getX();
double currentYPos = currentNode->point.getY();
double nextXPos = currentXPos + x[i];
double nextYPos = currentYPos + y[i];
const Point neighbourNode (nextXPos, nextYPos);
//if neighbourNode is not a wall and neighbourNode not in closedSet then
if (neighbourNode != map.isWall && std::find(closedSet.begin(), closedSet.end(), neighbourNode) == closedSet.end())
{
//gtentative<-currentNode.g+EUCLIDEANDISTANCE(currentNode, neighbourNode)
double gTentative = currentNode->g + euclideanDistance(currentNode->point, neighbourNode);
//if neighbourNode not in openSet or gtentative < neighbourNode.g then
if (!isInOpenSet(neighbourNode) || gTentative < neighbourNode->g)
{
//neighbourNode.g = gtentative
neighbourNode->g = gTentative;
//neighbourNode.h <- EUCLIDEANDISTANCE(neighbourNode, goalNode)
neighbourNode->h = euclideanDistance(neighbourNode, goalNode);
//add neighbourNode to openSet (if it is not already in)
if (!isInOpenSet(neighbourNode))
{
Node neighbourNode(openSet.top());
}
}
//end if
}
//end if
}
//end for
}
//end while
//return "Failed to find a path"
//end procedure
}
// vanilla 8 direction search
int aStarSearch::eightDirectionSearch(const int a_startX, const int a_startY, const int a_targetX, const int a_targetY, const int a_mapWidth, const int a_mapHeight, map& a_map, int* a_outBuffer, const int a_outBufferSize)
{
// create open set, priority queue returns the greater of the two compared elements, so tell it to use > instead of < to compare
// the pair to be put in queue is the (f cost, position)
std::priority_queue<std::pair<double,int>, std::vector<std::pair<double,int> >, std::greater<std::pair<double,int> > > openSet;
std::unordered_map<int,int> cameFrom;
std::unordered_map<int,double> costSoFar;
// keep track of who is visited, the bool is sort of pointless..
std::unordered_map<int,bool> visited;
// get heuristic
octileHeuristic heuristic;
int startPosition = a_startX + a_startY * a_mapWidth;
int finalPosition = a_targetX + a_targetY * a_mapWidth;
// start with initial condition and put it in queue
std::pair<double,int> startPair(0,startPosition);
cameFrom[startPosition] = startPosition;
openSet.push(startPair);
// one step in order: N S E W NE NW SE SW
const int* oneStepMove = a_map.getOneStepMove();
while (!openSet.empty())
{
// get best position so far
int currentPosition = openSet.top().second;
openSet.pop();
// never visited
if (!visited.count(currentPosition))
{
// put it in visited
visited[currentPosition] = true;
if (currentPosition == finalPosition)
{
return reconstructPath(startPosition, currentPosition, a_map, cameFrom, a_outBuffer, a_outBufferSize);
}
// consider the 8 possible moves
double cost;
for (int successor = 0; successor < 8; successor++)
{
int nextPos = currentPosition + oneStepMove[successor];
// check if move is possible
if (a_map[nextPos])
{
// for visualization purposes
a_map[nextPos] = 2;
// straight moves cost 1, diagonal moves cost sqrt(2)
if (successor < 4)
{
cost = costSoFar[currentPosition] + 1;
}
else
{
cost = costSoFar[currentPosition] + sqrt(2);
}
// if the neighbor is not in open set or cost from this node is better
if (!costSoFar.count(nextPos) || cost < costSoFar[nextPos])
{
costSoFar[nextPos] = cost;
double priority = cost + heuristic.estimateCost(nextPos, a_targetX, a_targetY, a_mapWidth);
std::pair<double,int> nextPair(priority,nextPos);
openSet.push(nextPair);
cameFrom[nextPos] = currentPosition;
}
}
}
}
}
// couldn't find a solution
return -1;
}
// vanilla jps
int aStarSearch::jumpPointSearch(const int a_startX, const int a_startY, const int a_targetX, const int a_targetY, const int a_mapWidth, const int a_mapHeight, map& a_map, int* a_outBuffer, const int a_outBufferSize)
{
// compute start and final positions
int startPosition = a_startX + a_startY * a_mapWidth;
int finalPosition = a_targetX + a_targetY * a_mapWidth;
// create open set, priority queue returns the greater of the two compared elements, so tell it to use > instead of < to compare
// the pair to be put in queue is the (f cost, position)
std::priority_queue<std::pair<double,int>, std::vector<std::pair<double,int> >, std::greater<std::pair<double,int> > > openSet;
std::unordered_map<int,int> cameFrom;
std::unordered_map<int,double> costSoFar;
// keep track of who is visited, the bool is sort of pointless..
std::unordered_map<int,bool> visited;
// get heuristic
octileHeuristic heuristic;
double priority, cost;
// get jump point finder
jumpPointTools jpTools;
/*
// get dead end detector and detect deadend
deadendDetector deDetector;
std::vector<int> relevantZones(a_map.getNumZones()+1,0);
deDetector.deadendSearch(a_map.getZone(startPosition), a_map.getZone(finalPosition), a_map.getZoneConnections(), relevantZones);
*/
cameFrom[startPosition] = startPosition;
// one step in order: N S E W NE NW SE SW
const int* oneStepMove = a_map.getOneStepMove();
Direction directions [] = { Direction::N,
Direction::S,
Direction::E,
Direction::W,
Direction::NE,
Direction::NW,
Direction::SE,
Direction::SW};
// start problem off by moving one step in every direction from starting position
visited[startPosition] = true;
for (int successor = 0; successor < 8; successor++)
{
int nextPos = startPosition + oneStepMove[successor];
// check if move is possible
if (a_map[nextPos])
{
// for visualization purposes
a_map[nextPos] = 2;
// straight moves cost 1, diagonal moves cost sqrt(2)
if (successor < 4)
{
cost = costSoFar[startPosition] + 1;
}
else
{
cost = costSoFar[startPosition] + sqrt(2);
}
costSoFar[nextPos] = cost;
priority = cost + heuristic.estimateCost(nextPos, a_targetX, a_targetY, a_mapWidth);
std::pair<double,int> nextPair(priority,nextPos);
openSet.push(nextPair);
cameFrom[nextPos] = startPosition;
}
}
// look at open set
while (!openSet.empty())
{
// get best position so far
int currentPosition = openSet.top().second;
openSet.pop();
// haven't visited this node yet
if (!visited.count(currentPosition))
{
visited[currentPosition] = true;
// check end condition
if (currentPosition == finalPosition)
{
return reconstructPath(startPosition, currentPosition, a_map, cameFrom, a_outBuffer, a_outBufferSize);
}
// find out how parent got here, currentPosition = parent + parentDirection
Direction parentDirection = jpTools.findParentDirection(currentPosition, cameFrom[currentPosition], oneStepMove, directions);
// find successors!
std::vector<std::pair<int, Direction> > successors;
jpTools.findJumpPoints(currentPosition, a_map, parentDirection, finalPosition, oneStepMove, successors);
for (int j = 0; j < successors.size(); j++)
{
// what is successor and what direction to get there
int nextPos = successors[j].first;
Direction nextDir = successors[j].second;
// check if move is possible
if (a_map[nextPos])
{
// for visualization purposes
a_map[nextPos] = 2;
// find out how many jumps it took to get here
int numOfJumps = 0;
int temp = currentPosition;
while (temp != nextPos)
{
numOfJumps++;
temp += oneStepMove[nextDir];
}
// straight moves cost 1, diagonal moves cost sqrt(2)
if (nextDir < 4)
{
cost = costSoFar[currentPosition] + numOfJumps;
}
else
{
cost = costSoFar[currentPosition] + numOfJumps*sqrt(2);
}
// if the neighbor is not in open set or cost from this node is better
if (!costSoFar.count(nextPos) || cost < costSoFar[nextPos])
{
priority = cost + heuristic.estimateCost(nextPos, a_targetX, a_targetY, a_mapWidth);
std::pair<double,int> nextPair(priority,nextPos);
openSet.push(nextPair);
// update entire path to the jump point for cost and came from
for (int j = 1; j <= numOfJumps; j++)
{
int updatePos = currentPosition + j*oneStepMove[nextDir];
a_map[updatePos] = 2;
if (nextDir < 4)
{
costSoFar[updatePos] = costSoFar[currentPosition] + j;
}
else
{
costSoFar[updatePos] = costSoFar[currentPosition] + j * sqrt(2);
}
cameFrom[updatePos] = updatePos - oneStepMove[nextDir];
}
}
}
}
}
}
// couldn't find a solution
return -1;
}
QList<const StarObject *> StarHopper::computePath( const SkyPoint &src, const SkyPoint &dest, float fov_, float maglim_ ) {
fov = fov_;
maglim = maglim_;
start = &src;
end = &dest;
came_from.clear();
result_path.clear();
// Implements the A* search algorithm
QList<SkyPoint const *> cSet;
QList<SkyPoint const *> oSet;
QHash<SkyPoint const *, double> g_score;
QHash<SkyPoint const *, double> f_score;
QHash<SkyPoint const *, double> h_score;
kDebug() << "StarHopper is trying to compute a path from source: " << src.ra().toHMSString() << src.dec().toDMSString() << " to destination: " << dest.ra().toHMSString() << dest.dec().toDMSString() << "; a starhop of " << src.angularDistanceTo( &dest ).Degrees() << " degrees!";
oSet.append( &src );
g_score[ &src ] = 0;
h_score[ &src ] = src.angularDistanceTo( &dest ).Degrees()/fov;
f_score[ &src ] = h_score[ &src ];
while( !oSet.isEmpty() ) {
kDebug() << "Next step";
// Find the node with the lowest f_score value
SkyPoint const *curr_node = NULL;
double lowfscore = 1.0e8;
foreach( const SkyPoint *sp, oSet ) {
if( f_score[ sp ] < lowfscore ) {
lowfscore = f_score[ sp ];
curr_node = sp;
}
}
kDebug() << "Lowest fscore (vertex distance-plus-cost score) is " << lowfscore << " with coords: " << curr_node->ra().toHMSString() << curr_node->dec().toDMSString() << ". Considering this node now.";
if( curr_node == &dest || (curr_node != &src && h_score[ curr_node ] < 0.5) ) {
// We are at destination
reconstructPath( came_from[ curr_node ] );
kDebug() << "We've arrived at the destination! Yay! Result path count: " << result_path.count();
// Just a test -- try to print out useful instructions to the debug console. Once we make star hopper unexperimental, we should move this to some sort of a display
kDebug() << "Star Hopping Directions: ";
const SkyPoint *prevHop = start;
foreach( const StarObject *hopStar, result_path ) {
QString direction;
double pa; // should be 0 to 2pi
dms angDist = prevHop->angularDistanceTo( hopStar, &pa );
dms dmsPA;
dmsPA.setRadians( pa );
direction = KSUtils::toDirectionString( dmsPA );
kDebug() << " Slew " << angDist.Degrees() << " degrees " << direction << " to find a " << hopStar->spchar() << " star of mag " << hopStar->mag();
prevHop = hopStar;
}
kDebug() << " The destination is within a field-of-view";
return result_path;
}
vector<D3DXVECTOR3> AStar::findPath(PathNode* _start, PathNode* _end)
{
//time it for debugging
clock_t clockStart = clock();
//empty the closed set vector
vector<PathNode*>().swap(mClosedSet);
//empty the open set and put in only the start node
vector<PathNode*>().swap(mOpenSet);
mOpenSet.push_back(_start);
//set all node gScores to max
for (PathNode* PN : mPathNodes)
PN->setGScore(INT_MAX);
//set start node properties
_start->setGScore(0);
_start->setFScore(findHeuristic(_start, _end));
//while the open set is not empty
while (mOpenSet.size() > 0)
{
//the node in open set having the lowest f score
PathNode* current;
findLowestFScore(current);
//if we found the goal, return the path
if (current == _end || ((clock() - clockStart) / (float)CLOCKS_PER_SEC) > MAX_PATH_TIME_LOW)
{
vector<D3DXVECTOR3> rePath;
reconstructPath(rePath, _start, current);
return rePath;
}
//save current to closed set
mClosedSet.push_back(current);
//remove current from open set
for (vector<PathNode*>::iterator iter = mOpenSet.begin(); iter != mOpenSet.end(); ++iter)
{
if (*iter == current)
{
mOpenSet.erase(iter);
break;
}
}
//for each linked node in the current node
for (PathNode* PN : current->getLinks())
{
//if it is already in the closed set, continue
if (inClosedSet(PN))
continue;
//find tentative gScore
int tempGScore = current->getGScore() + findHeuristic(current, _end);
//if link node is not in open set or tempGScore < present GScore
if (!inOpenSet(PN) || tempGScore < PN->getGScore())
{
//set the came from of the link to the current node
PN->setCameFrom(current);
//set g score
PN->setGScore(tempGScore);
PN->setFScore(PN->getGScore() + findHeuristic(PN, _end));
//if link is not in open set, add to it
if (!inOpenSet(PN))
mOpenSet.push_back(PN);
}
}
}
//finished loop without finding goal
OutputDebugString(L"ERROR: No path found.");
return vector<D3DXVECTOR3>();
}
std::pair <std::vector<StateOfCar>, Seed> AStarSeed::findPathToTarget(const cv::Mat& map, const State& start, const State& goal, const int distance_transform, const int debug_current_state, int& status) {
// USE : for guaranteed termination of planner
int no_of_iterations = 0;
int max_iterations = 10000;
node_handle.getParam("local_planner/max_iterations", max_iterations);
fusion_map = map;
map_max_rows = map.rows;
map_max_cols = map.cols;
if (distance_transform == 1) {
distanceTransform();
}
if (debug_current_state) {
image = fusion_map.clone();
}
StateOfCar start_state(start), target_state(goal);
std::map<StateOfCar, open_map_element> open_map;
std::map<StateOfCar, StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> open_set;
open_set.push(start_state);
if (start_state.isCloseTo(target_state)) {
status = 1;
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(start_state)) {
std::cout << "Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(target_state)) {
std::cout << "Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!open_set.empty() && ros::ok()) {
if (no_of_iterations > max_iterations) {
status = open_set.size();
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar current_state = open_set.top();
if (open_map.find(current_state) != open_map.end() && open_map[current_state].membership == CLOSED) {
open_set.pop();
}
current_state = open_set.top();
if (debug_current_state) {
std::cout << "current x : " << current_state.x() << " current y : " << current_state.y() << std::endl;
plotPointInMap(current_state);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (current_state.isCloseTo(target_state)) {
status = 2;
return reconstructPath(current_state, came_from);
}
open_set.pop();
open_map[current_state].membership = UNASSIGNED;
open_map[current_state].cost = -current_state.gCost();
open_map[current_state].membership = CLOSED;
std::vector<StateOfCar> neighbors_of_current_state = neighborNodesWithSeeds(current_state);
for (unsigned int iterator = 0; iterator < neighbors_of_current_state.size(); iterator++) {
StateOfCar neighbor = neighbors_of_current_state[iterator];
double tentative_gcost_along_followed_path = neighbor.gCost() + current_state.gCost();
double admissible = neighbor.distanceTo(target_state);
double consistent = admissible;
if (!((open_map.find(neighbor) != open_map.end()) &&
(open_map[neighbor].membership == OPEN))) {
came_from[neighbor] = current_state;
neighbor.hCost(consistent);
neighbor.gCost(tentative_gcost_along_followed_path);
neighbor.updateTotalCost();
open_set.push(neighbor);
open_map[neighbor].membership = OPEN;
open_map[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
status = 0;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
