Advertisement

Problem With A* Pathfinding

Started by July 02, 2007 11:40 PM
1 comment, last by Fisco 17 years, 4 months ago
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;

}
Firstly, your operator< looks like it's the wrong way around.

Secondly, and most crucially, the closed list is NOT the path. It's a list of all previously examined nodes, including some or many that got rejected as being off the main path. Each node in A* has a parent node, which is its predecessor on any potential path. When you add a node to the open list, you need to record its parent/predecessor node. When you replace a node on the open list with a more efficient way of reaching that node, you also need to replace its parent node reference. And when you reach the destination, you use those parent node references to trace the route back to the origin.
Advertisement
I know its not C* but here is a solid OO Java Astar...

P.S how do you post code?

package searchalgorithms;


import java.io.Serializable;
import java.util.*;

/**
The AStarSearch class, along with the AStarNode class,
implements a generic A* search algorithm. The AStarNode
class should be subclassed to provide searching capability.
*/
public class AStarSearch {


/**
A simple priority list, also called a priority queue.
Objects in the list are ordered by their priority,
determined by the object's Comparable interface.
The highest priority item is first in the list.
*/
public static class PriorityList extends LinkedList implements Serializable {

/**
*
*/
private static final long serialVersionUID = -8373908426544337649L;

public void add(Comparable object) {
for (int i=0; i<size(); i++) {
if (object.compareTo(get(i)) <= 0) {
add(i, object);
return;
}
}
addLast(object);
}
}

/**
Construct the path, not including the start node.
*/
protected List constructPath(AStarNode node) {
LinkedList path = new LinkedList();
while (node.pathParent != null) {
path.addFirst(node);
node = node.pathParent;
}
return path;
}


/**
Find the path from the start node to the end node. A list
of AStarNodes is returned, or null if the path is not
found.
*/
public List findPath(AStarNode startNode, AStarNode goalNode) {

PriorityList openList = new PriorityList();
LinkedList closedList = new LinkedList();

startNode.costFromStart = 0;
startNode.estimatedCostToGoal =
startNode.getEstimatedCost(goalNode);
startNode.pathParent = null;
openList.add(startNode);

while (!openList.isEmpty()) {
AStarNode node = (AStarNode)openList.removeFirst();
if (node == goalNode) {
// construct the path from start to goal
return constructPath(goalNode);
}

List neighbors = node.getNeighbors();
for (int i=0; i<neighbors.size(); i++) {
AStarNode neighborNode =
(AStarNode)neighbors.get(i);
boolean isOpen = openList.contains(neighborNode);
boolean isClosed =
closedList.contains(neighborNode);
float costFromStart = node.costFromStart +
node.getCost(neighborNode);

// check if the neighbor node has not been
// traversed or if a shorter path to this
// neighbor node is found.
if ((!isOpen && !isClosed) ||
costFromStart < neighborNode.costFromStart)
{
neighborNode.pathParent = node;
neighborNode.costFromStart = costFromStart;
neighborNode.estimatedCostToGoal =
neighborNode.getEstimatedCost(goalNode);
if (isClosed) {
closedList.remove(neighborNode);
}
if (!isOpen) {
openList.add(neighborNode);
}
}
}
closedList.add(node);
}

// no path found
return null;
}

}

package searchalgorithms;

import java.io.Serializable;
import java.util.List;

/**
The AStarNode class, along with the AStarSearch class,
implements a generic A* search algorithm. The AStarNode
class should be subclassed to provide searching capability.
*/
public abstract class AStarNode implements Comparable, Serializable {

AStarNode pathParent;
float costFromStart;
float estimatedCostToGoal;


public float getCost() {
return costFromStart + estimatedCostToGoal;
}


public int compareTo(Object other) {
float thisValue = this.getCost();
float otherValue = ((AStarNode)other).getCost();

float v = thisValue - otherValue;
return (v>0)?1:(v<0)?-1:0; // sign function
}


/**
Gets the cost between this node and the specified
adjacent (AKA "neighbor" or "child") node.
*/
public abstract float getCost(AStarNode node);


/**
Gets the estimated cost between this node and the
specified node. The estimated cost should never exceed
the true cost. The better the estimate, the more
effecient the search.
*/
public abstract float getEstimatedCost(AStarNode node);


/**
Gets the children (AKA "neighbors" or "adjacent nodes")
of this node.
*/
public abstract List getNeighbors();
}

This topic is closed to new replies.

Advertisement