//returns true if there were no prob creating a path //error only occur if there's something wrong with the parameter //can (and will) return true even if there is no way to the destination bool PathPlanner::Planning(int start[2], int goal[2]) { if(!inBounds(start) || !inBounds(goal)) return false; std::vector<Path> pathList; Path init(0,computeHeuristic(start[0],start[1],goal[0],goal[1])); init.nodes.push_back(&nodeMap[start[0]][start[1]]); nodeMap[start[0]][start[1]].state = FRONTIER; pathList.push_back(init); Path newpath; Path toexpand; Node* lastnode; while(pathList.size() != 0 ) { std::sort(pathList.begin(),pathList.end()); toexpand = pathList.back(); pathList.pop_back(); if(toexpand.nodes.back() == &nodeMap[goal[0]][goal[1]]) { path = toexpand; return true; } lastnode = toexpand.nodes.back(); if( lastnode->state != EXPLORED) { //bottom expandNode(toexpand,&pathList,0,1,3,goal); //right expandNode(toexpand,&pathList,1,0,3,goal); // left expandNode(toexpand,&pathList,-1,0,3,goal); //top expandNode(toexpand,&pathList,0,-1,3,goal); //top-left expandNode(toexpand,&pathList,-1,-1,4,goal); //top-right expandNode(toexpand,&pathList,1,-1,4,goal); //bottom-left expandNode(toexpand,&pathList,-1,1,4,goal); //bottom-right expandNode(toexpand,&pathList,1,1,4,goal); toexpand.nodes.back()->state = EXPLORED; } } return false; }
void PathPlanner::expandNode(Path toexpand,std::vector<Path>* pathList, int xoffset,int yoffset,int costcoef, int goal[2]) { if(nodeMap[toexpand.nodes.back()->x+xoffset][toexpand.nodes.back()->y+yoffset].state <= FRONTIER) { toexpand.currentCost += costcoef*nodeMap[toexpand.nodes.back()->x+xoffset][toexpand.nodes.back()->y+yoffset].single_cost; toexpand.heuristicCost = computeHeuristic(toexpand.nodes.back()->x+xoffset,toexpand.nodes.back()->y+yoffset,goal[0],goal[1]); toexpand.nodes.push_back(&nodeMap[toexpand.nodes.back()->x+xoffset][toexpand.nodes.back()->y+yoffset]); pathList->push_back(toexpand); } }
bool cTerrainNaviMeshCell::queryPath(uint cellIndex, cNavigationHeap* heap, uint caller, float arriveCost) { sTerrainNaviMeshCell* cell = m_cellList + cellIndex; sTerrainNaviPathCell* pathCell = m_pathDataList + cellIndex; if (pathCell->m_sessionId != heap->getSessionId()) { pathCell->m_sessionId = heap->getSessionId(); if (caller) { pathCell->m_open = true; pathCell->m_heuristic = computeHeuristic(heap->getGoal(), cell->m_center); pathCell->m_arriveCost = arriveCost; if (caller == cell->m_link[0]) pathCell->m_arriveWall = 0; else if (caller == cell->m_link[1]) pathCell->m_arriveWall = 1; else if (caller == cell->m_link[2]) pathCell->m_arriveWall = 2; } else { pathCell->m_open = false; pathCell->m_arriveCost = 0; pathCell->m_heuristic = 0; pathCell->m_arriveWall = 0; } heap->addCell(cell->m_cellIndex, pathCell->getPathFindingCost()); return true; } // 방문한 셀 일 경우에 else if (pathCell->m_open) { // const가 더 작으면 if (arriveCost + pathCell->m_heuristic < pathCell->m_arriveCost + pathCell->m_heuristic) { pathCell->m_arriveCost = arriveCost; if (caller == cell->m_link[0]) pathCell->m_arriveWall = 0; else if (caller == cell->m_link[1]) pathCell->m_arriveWall = 1; else if (caller == cell->m_link[2]) pathCell->m_arriveWall = 2; heap->adjustCell(cell->m_cellIndex, pathCell->getPathFindingCost()); return true; } } return false; }
/*implements A* algorithm*/ tree astar(int x, int y, int d, int heuristic){ char a = 'X';//X simbolize that no action was taken to get there tree t, t1, t2; char **mat1, **mat2; int i, j; int level; double f; queue_vector aux; int k; //copy the starting matrix mat1 = newMatrix(); for(i = 0; i < n; i++){ for(j = 0; j < m; j++) mat1[i][j] = w[i][j]; } t1 = insertTree(NULL, d, x, y, mat1, a, 0); t = t1; //compute the f value according to the heuristic f = computeHeuristic(x, y, 0, heuristic); insertQueue(t1, f); while(n_queue > 0){ //remove the first element of the queue removeQueue(&aux.t, &aux.f); t1 = aux.t; d = t1 -> dirt; mat1 = t1 -> mat; a = t1 -> action; x = t1 -> x; y = t1 -> y; level = t1 -> level; //increments the number of nodes expanded expanded++; //adds into the queue if(y != 0 && mat1[y-1][x] != '#'){ mat2 = newMatrix(); copyMatrix(mat1, mat2); a = mat1[y-1][x]; mat2[y-1][x] = '@'; mat2[y][x] = '_'; if(a == '*') k = d-1; else k = d; if(checkDuplicate(t, mat2, hashFunction(x, y-1), k) == 0){ //inserts in lowercase if there is dirt if(a == '*'){ removeListDirt(x, y-1); t2 = insertTree(t1, d-1, x, y-1, mat2, 'n', level + 1); if(d-1 == 0) return t2; } else t2 = insertTree(t1, d, x, y-1, mat2, 'N', level + 1); t1 -> N = t2; //compute the f value according to the heuristic f = computeHeuristic(x, y-1, level+1, heuristic); insertQueue(t2, f); generated++; } } if(y != n - 1 && mat1[y+1][x] != '#'){ mat2 = newMatrix(); copyMatrix(mat1, mat2); a = mat1[y+1][x]; mat2[y+1][x] = '@'; mat2[y][x] = '_'; if(a == '*') k = d-1; else k = d; if(checkDuplicate(t, mat2, hashFunction(x, y+1), k) == 0){ //inserts in lowercase if there is dirt if(a == '*'){ removeListDirt(x, y+1); t2 = insertTree(t1, d-1, x, y+1, mat2, 's', level + 1); if(d-1 == 0) return t2; } else t2 = insertTree(t1, d, x, y+1, mat2, 'S', level + 1); t1 -> S = t2; //compute the f value according to the heuristic f = computeHeuristic(x, y-1, level+1, heuristic); insertQueue(t2, f); generated++; } } if(x != 0 && mat1[y][x-1] != '#'){ mat2 = newMatrix(); copyMatrix(mat1, mat2); a = mat1[y][x-1]; mat2[y][x-1] = '@'; mat2[y][x] = '_'; if(a == '*') k = d-1; else k = d; if(checkDuplicate(t, mat2, hashFunction(x - 1, y), k) == 0){ //inserts in lowercase if there is dirt if(a == '*'){ removeListDirt(x-1, y); t2 = insertTree(t1, d-1, x-1, y, mat2, 'w', level + 1); if(d-1 == 0) return t2; } else t2 = insertTree(t1, d, x-1, y, mat2, 'W', level + 1); t1 -> W = t2; //compute the f value according to the heuristic f = computeHeuristic(x, y-1, level+1, heuristic); insertQueue(t2, f); generated++; } } if(x != m - 1 && mat1[y][x+1] != '#'){ mat2 = newMatrix(); copyMatrix(mat1, mat2); a = mat1[y][x+1]; mat2[y][x+1] = '@'; mat2[y][x] = '_'; if(a == '*') k = d-1; else k = d; if(checkDuplicate(t, mat2, hashFunction(x + 1, y), k) == 0){ //inserts in lowercase if there is dirt if(a == '*'){ removeListDirt(x+1, y); t2 = insertTree(t1, d-1, x+1, y, mat2, 'e', level + 1); if(d-1 == 0) return t2; } else t2 = insertTree(t1, d, x+1, y, mat2, 'E', level + 1); t1 -> E = t2; //compute the f value according to the heuristic f = computeHeuristic(x, y-1, level+1, heuristic); insertQueue(t2, f); generated++; } } orderQueue(); } return NULL; }