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