/*
BEFORE ATTEMPTING TO READ THIS FILE, PLEASE HAVE A BASIC UNDERSTANDING OF
MT-D*LITE FROM READING ITS ORIGINAL RESEARCH PAPER'S PSEUDO-CODE.
*/
GridWorld::GridWorld(unsigned int length, unsigned int width, int radius,
Coords startCoords, Coords goalCoords){
this->length = length;
this->width = width;
this->radius = radius;
for (unsigned int y = 0; y < length; y++){
for (unsigned int x = 0; x < width; x++){
world.push_back(new Tile(x, y, 10));
}
}
//In version B, start and goal are switch
goal = getTileAt(startCoords.first, startCoords.second);
start = getTileAt(goalCoords.first, goalCoords.second);
//Initializing the pathfinder's default values
km = 0;
previous = goal;
start->rhs = 0;
start->isOpen = true;
start->h = calculateH(start);
start->key = GridWorld::KeyPair(start->h, 0);
open.push_back(start);
}
GridWorld::KeyPair GridWorld::calculateKey(GridWorld::Tile*& tile){
double key2 = std::min(tile->g, tile->rhs);
double key1 = key2 + calculateH(tile) + km;
//H-value should be recalculated as it can change during incremental search
return KeyPair(key1, key2);
}
float PathNode::calculateF(PathNode* end)
{
g = calculateG(parent);
h = calculateH(end);
return f = g + h;
}
//void OrthographicGridPathfinder::addNeighbors(bool *nodesToAdd, PathNode *centerNode, PathNode *destination, list<PathNode> *openList, list<PathNode> *closedList)
void OrthographicGridPathfinder::addNeighbors(bool *nodesToAdd, PathNode *centerNode, PathNode *destination, map<int, PathNode> *openNodes, map<int, PathNode> *closedNodes)
{
int adjacencyIndex = 0;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
bool shouldBeAdded = nodesToAdd[adjacencyIndex];
if (shouldBeAdded)
{
int col = i + centerNode->column - 1;
int row = j + centerNode->row - 1;
PathNode testNode;
testNode.column = col;
testNode.row = row;
testNode.parentNode = centerNode;
testNode.G = calculateG(testNode);
testNode.H = calculateH(testNode, destination);
// BEFORE WE ADD IT, CHECK TO SEE IF WE'VE ALREADY
// FOUND A FASTER WAY OF GETTING TO THIS NODE THAT
// IS IN THE OPEN LIST
// if (containsPathNode(openList, testNode))
int index = getGridIndex(testNode.column, testNode.row);
if (openNodes->find(index) != openNodes->end())
{
// PathNode *dup = findDupNodeInList(testNode, openList);
PathNode *dup = &openNodes->at(index);
if ((testNode.G+testNode.H) < (dup->G + dup->H))
{
// IT'S BETTER THAN WHAT'S ALREADY THERE, SO
// UPDATE THE ONE THAT'S ALREADY THERE
dup->column = testNode.column;
dup->row = testNode.row;
dup->G = testNode.G;
dup->H = testNode.H;
dup->parentNode = testNode.parentNode;
}
}
else
// openList->push_back(testNode);
(*openNodes)[index] = testNode;
}
adjacencyIndex++;
}
}
}
void AStarNode::update() {
calculateH();
calculateG();
}
void OrthographicGridPathfinder::buildPath( list<PathNode> *pathToFill,
float startX, float startY,
float endX, float endY)
{
// SO BUILD THE PATH
pathToFill->clear();
vector<bool> *pathGridPointer = &pathGrid;
GameStateManager *gsm = game->getGSM();
World *world = gsm->getWorld();
int gridWidth = getGridWidth();
int gridHeight = getGridHeight();
// DETERMINE THE START COLUMN AND START ROW
int startColumn = (int)(startX/gridWidth);
int startRow = (int)(startY/gridHeight);
// DETERMINE THE END COLUMN AND END ROW
int endColumn = (int)(endX/gridWidth);
int endRow = (int)(endY/gridHeight);
// IF THE DESTINATION IS A COLLIDABLE TILE LOCATION
// THEN EXIT
int endIndex = getGridIndex(endColumn, endRow);
bool endIndexIsWalkable = pathGrid[getGridIndex(endColumn, endRow)];
if (!endIndexIsWalkable)
return;
map<int, PathNode> openNodes;
map<int, PathNode> closedNodes;
// list<PathNode> openList;
// list<PathNode> closedList;
bool pathFound = false;
bool nodesToAdd[9];
PathNode startNode;
startNode.column = startColumn;
startNode.row = startRow;
startNode.parentNode = NULL;
startNode.G = 0;
PathNode endNode;
endNode.column = endColumn;
endNode.row = endRow;
endNode.parentNode = NULL;
startNode.H = calculateH(startNode, &endNode);
// openList.push_back(startNode);
int nodeIndex = getGridIndex(startColumn, startRow);
openNodes[nodeIndex] = startNode;
while (!pathFound)
{
// IF THERE ARE NO MORE NODES TO SEARCH THROUGH TO FIND
// OUR DESTINATION THEN WE'RE DONE
// if (openList.size() == 0)
if (openNodes.size() == 0)
{
pathToFill->clear();
return;
}
// FIRST GET THE CLOSEST NODE FROM THE OPEN LIST
// PathNode *foundNode = findCheapestNode(&openList);
PathNode *foundNode = findCheapestNode(&openNodes);
PathNode cheapestNode;
cheapestNode.column = foundNode->column;
cheapestNode.row = foundNode->row;
cheapestNode.G = foundNode->G;
cheapestNode.H = foundNode->H;
cheapestNode.parentNode = foundNode->parentNode;
// removeNodeFromList(&cheapestNode, &openList);
openNodes.erase(getGridIndex(cheapestNode.column, cheapestNode.row));
// IS THE CHEAPEST NODE THE DESTINATION?
if ((cheapestNode.column == endNode.column) && (cheapestNode.row == endNode.row))
{
// WE'VE REACHED THE DESTINATION
pathFound = true;
PathNode *traveller = &cheapestNode;
while (traveller != NULL)
{
PathNode nodeToAdd;
nodeToAdd.column = traveller->column;
nodeToAdd.row = traveller->row;
pathToFill->push_front(nodeToAdd);
traveller = traveller->parentNode;
}
}
else
{
// WE'LL NEED TO LOOK AT THE CHEAPEST NODE'S NEIGHBORS
// FIRST LET'S PUT IT INTO THE CLOSED LIST SO WE DON'T
// END UP IN AN INFINITELY CIRCULAR LOOP
// closedList.push_back(cheapestNode);
closedNodes[getGridIndex(cheapestNode.column, cheapestNode.row)] = cheapestNode;
// PathNode *nodeJustAdded = &closedList.back();
PathNode *nodeJustAdded = &closedNodes[getGridIndex(cheapestNode.column, cheapestNode.row)];
// NOW FIGURE OUT WHICH NEIGHBORS MIGHT BE USABLE
// findNeighborsToCheck(world, nodesToAdd, nodeJustAdded, &closedList);
findNeighborsToCheck(nodesToAdd, nodeJustAdded, &closedNodes);
// NOW ADD THE NEIGHBORS TO OUR OPEN LIST
// addNeighbors(nodesToAdd, nodeJustAdded, &endNode, &openList, &closedList);
addNeighbors(nodesToAdd, nodeJustAdded, &endNode, &openNodes, &closedNodes);
}
}
PathNode lastNode = pathToFill->back();
destinationPathfindingCell.setCenterX(getColumnCenterX(lastNode.column));
destinationPathfindingCell.setCenterY(getRowCenterY(lastNode.row));
destinationPathfindingCell.setWidth(this->getGridWidth());
destinationPathfindingCell.setHeight(this->getGridHeight());
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = goal;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (tile != start && tile->rhs > neighbours[i]->g + newCostToTile){
tile->successor = neighbours[i];
tile->rhs = neighbours[i]->g + newCostToTile;
}
}
else if (tile != start && tile->successor == neighbours[i]){
TilePair minSucc(getMinSuccessor(tile));
tile->rhs = minSucc.second;
tile->successor = (tile->rhs == PF_INFINITY ? 0 : minSucc.first);
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (neighbours[i] != start && neighbours[i]->rhs > tile->g + newCostToTile){
neighbours[i]->successor = tile;
neighbours[i]->rhs = tile->g + newCostToTile;
updateVertex(neighbours[i]);
}
}
else if (neighbours[i] != start && neighbours[i]->successor == tile){
TilePair minSucc(getMinSuccessor(neighbours[i]));
neighbours[i]->rhs = minSucc.second;
neighbours[i]->successor = (neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first);
updateVertex(neighbours[i]);
}
}
computeShortestPath();
}
void NodeAstar::calculateScores(NodeAstar* finish)
{
G = calculateG(parent);
H = calculateH(finish);
F = G + H;
}
void GOAPAstar::Plan(GOAPlanner *ap)
{
// clear open and closed lists
_open.clear();
_closed.clear();
// TODO: Early out if _current == _desired, add plan WANDER
WorldState goal = ap->_desired;
// put start in the open list
astarnode start;
start.ws = ap->_current;
start.parent_ws = ap->_current;
start.g = 0;
start.h = calculateH(ap->_current, goal);
start.f = start.g + start.h;
start.action_name = "";
_open.push_back(start);
for (;;)
{
if (_open.size() == 0)
return;
// find the node with the lowest rank
astarnode curr = openPopLowest();
auto care = goal.GetCare();
bool match = ((curr.ws.GetFlags() & care) == (goal.GetFlags() & care));
// if we've reached our goal state
if (match)
{
reconstructPlan(ap, &curr);
// Success
return;
}
// add current to closed
_closed.push_back(curr);
// fill the transitions array
getPossibleStateTransitions(ap, curr.ws);
// iterate over all possible transitions
for (auto &pair : _transitions)
{
AIAction &action = pair.first;
WorldState &future = pair.second;
astarnode neighbor;
int cost = curr.g + action.GetCost();
int open_index = nodeInOpened(future);
int close_index = nodeInClosed(future);
// if neighbor is in OPEn and cost less than g(neighbor)
if (open_index >= 0 && cost < _open[open_index].g)
{
// remove neighbor from OPEN, because new patch is better
_open.erase(_open.begin() + open_index);
open_index = -1;
}
// if neighbor in CLOSED and cost less than g(neighbor)
if (close_index >= 0 && cost < _closed[close_index].g)
{
// remove neighbor from CLOSED
_closed.erase(_closed.begin() + close_index);
}
// if neighbor not in OPEN and neighbor not in CLOSED
if (close_index == -1 && open_index == -1)
{
neighbor.ws = future;
neighbor.g = cost;
neighbor.h = calculateH(neighbor.ws, goal);
neighbor.f = neighbor.g + neighbor.h;
neighbor.action_name = action.GetName();
neighbor.parent_ws = curr.ws;
_open.push_back(neighbor);
}
}
}
return;
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = start;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
double currentRHS, otherG;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = tile->rhs;
otherG = neighbours[i]->g;
if (oldCostToTile > newCostToTile){
if (tile != goal){
tile->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (tile != goal){
tile->rhs = getMinSuccessor(tile).second;
}
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = neighbours[i]->rhs;
otherG = tile->g;
if (oldCostToTile > newCostToTile){
if (neighbours[i] != goal){
neighbours[i]->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (neighbours[i] != goal){
neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
}
}
updateVertex(neighbours[i]);
}
computeShortestPath();
}
