I am having trouble finding a few errors in my code for A*. I use an object oriented approach to make it simpler to use from the game. The Collision field is represented as an array of booleans.
In my result, the red squares are barriers, and the white squares represent the white. The problem is the path becomes two squares wide where it should not.
http://garagedever.ulmb.com/Problem.JPG
Here is my code - hopefully documented enough to read
cPathFinder.h :
#ifndef __H_PATHFINDER
#define __H_PATHFINDER
#include "globals.h"
#include <vector.h>
#include <algorithm>
#include "math.h"
// Represents a single path node on a closed or open list
class cPathNode
{
public:
// heruistic
int m_H;
// move score
int m_G;
// score
int m_F;
// position
int m_PosX;
int m_PosY;
cPathNode() {}
cPathNode(int p_PosX, int p_PosY);
// constructor for ease of use
cPathNode(int p_PosX, int p_PosY, int p_H, int p_G);
// operators
bool operator<(const cPathNode &a) const;
bool operator==(const cPathNode &a) const;
};
// Finds the shortest path from one location to another while avoiding obstacles
// specified in the collision map
class cPathFinder
{
// collision map
bool * m_CollisionMap;
// map dimensions
int m_MapWidth;
int m_MapHeight;
// open and closed lists
vector<cPathNode> m_OpenList;
vector<cPathNode> m_ClosedList;
// temp nodes
cPathNode m_Current;
cPathNode m_N;
cPathNode m_E;
cPathNode m_Goal;
// retrieves the boolean value from the collision map
bool IsCollision(int p_X, int p_Y);
// adds a node to the open list
void AddToOpenList(int p_X, int p_Y);
public:
cPathFinder() {}
// creates the collision map from the array specified
void CreateMap(bool * p_ColMap, int p_MapWidth, int p_MapHeight);
// finds the shortest path from start node to end node
vector<cPathNode> FindPath(cPathNode p_Start, cPathNode p_End);
};
#endif
cPathFinder.cpp
#include "cPathFinder.h"
cPathNode::cPathNode(int p_PosX, int p_PosY)
{
m_PosX = p_PosX;
m_PosY = p_PosY;
}
cPathNode::cPathNode(int p_PosX, int p_PosY, int p_H, int p_G)
{
// set member varaibles
m_PosX = p_PosX;
m_PosY = p_PosY;
m_H = p_H;
m_G = p_G;
// set F to the sum of G and H
m_F = m_H + m_G;
}
bool cPathNode::operator<(const cPathNode &a) const
{
if(a.m_F > m_F)
return true;
else
return false;
}
bool cPathNode::operator==(const cPathNode &a) const
{
if((a.m_PosX == m_PosX) && (a.m_PosY == m_PosY))
return true;
else
return false;
}
void cPathFinder::CreateMap(bool * p_ColMap, int p_MapWidth, int p_MapHeight)
{
// set member variables
m_CollisionMap = p_ColMap;
m_MapWidth = p_MapWidth;
m_MapHeight = p_MapHeight;
}
bool cPathFinder::IsCollision(int p_X, int p_Y)
{
return m_CollisionMap[(p_Y * m_MapWidth) + p_X];
}
void cPathFinder::AddToOpenList(int p_X, int p_Y)
{
// make sure the node is within the map bounds
if(!((p_X >= 0) && (p_X < m_MapWidth) && (p_Y >= 0) && (p_Y < m_MapHeight)))
return;
// make sure the node is not a collision
if(IsCollision(p_X, p_Y))
return;
// set temp pos
m_N.m_PosX = p_X;
m_N.m_PosY = p_Y;
// set temp node attributes
m_N.m_G = (abs(m_N.m_PosX - m_Goal.m_PosX) + abs(m_N.m_PosY - m_Goal.m_PosY)) * 10;
m_N.m_H = m_Current.m_H + 5;
m_N.m_F = m_N.m_G + m_N.m_H;
// should we add the node to the open list
bool add = true;
// index of the node to add
int index;
for(int i = 0; i < m_OpenList.size(); i++)
{
if((m_N.m_PosX == m_OpenList.m_PosX) && (m_N.m_PosY == m_OpenList.m_PosY))
{
add = false;
index = i;
break;
}
}
// add the node if it's not in the open list already
if(add)
{
//allegro_message("Adding to Open List: %i, %i, %i, %i, %i", m_N.m_PosX,
// m_N.m_PosY, m_N.m_H, m_N.m_G, m_N.m_F);
m_OpenList.push_back(m_N);
}
else
{
// see if this path to this node is better
m_E = m_OpenList[index];
if(m_N.m_H < m_E.m_H)
{
// if it is recalculate the values
m_E.m_G = m_N.m_G;
m_E.m_H = m_N.m_H;
m_E.m_F = m_N.m_F;
// resort with the new values
sort(m_OpenList.begin(), m_OpenList.end());
}
}
}
vector<cPathNode> cPathFinder::FindPath(cPathNode p_Start, cPathNode p_End)
{
// set the member var
m_Goal = p_End;
// Set the heuristic and move cost
p_Start.m_H = 0;
p_Start.m_G = abs((p_Start.m_PosX - p_End.m_PosX) + (p_Start.m_PosY - p_End.m_PosY)) * 10;
// set total cost
p_Start.m_F = p_Start.m_G + p_Start.m_H;
// clear the open and closed lists for start
m_OpenList.clear();
m_ClosedList.clear();
// put the starting node on the list
m_OpenList.push_back(p_Start);
// enter the main path finding loop
bool done = false;
while(!done)
{
// sort the list by F score
sort(m_OpenList.begin(), m_OpenList.end());
// the current node is the front node with the lowest F score
m_Current = m_OpenList.front();
// add the current node to the closed list
m_ClosedList.push_back(m_OpenList.front());
// remove it from the open list
m_OpenList.erase(m_OpenList.begin());
// see if we have reached the target node
if((m_Current.m_PosX == p_End.m_PosX) && (m_Current.m_PosY == p_End.m_PosY))
break;
// add the nodes around the current node to the open list
AddToOpenList(m_Current.m_PosX, m_Current.m_PosY - 1);
AddToOpenList(m_Current.m_PosX, m_Current.m_PosY + 1);
AddToOpenList(m_Current.m_PosX - 1, m_Current.m_PosY);
AddToOpenList(m_Current.m_PosX + 1, m_Current.m_PosY);
}
// sort the final path nodes
sort(m_ClosedList.begin(), m_ClosedList.end());
// add the goal node to the path
m_ClosedList.insert(m_ClosedList.begin(), m_Goal);
vector<cPathNode> ret;
// add the nodes in reverse order to the return path
for(int i = m_ClosedList.size() - 1; i > 0; i--)
{
ret.push_back(m_ClosedList);
}
return ret;
}