Pathfinding issue. Not taking most direct path

Question

I have a c# pathfinding implementation I am using. I am running into some issues with it. It works fine, but it will not give me the type of paths I desire. The best example of when I look for a path between A to B in an obstacle free environment instead of zigzagging directly to the target (I want that), it will take less turns and make more of an L shape or an obtuse angle. Does my algorithm favor fewer turns? I messed with the heuristic for hours and tried many things. Using a tie breaker, trying the manhattan & octile heuristics. Changing the heuristic hasn't seemed to affect the behavior. Now I'm thinking it's somewhere else in the system?

This picture is the behavior I get, and the picture below is what I desire.

Here's my long pathfinding code. Forgive the messiness: using UnityEngine; using System.Collections;

public class PathFinder {

//Declare readonlyants
private int onClosedList = 10;
private readonly int notStarted = 0;
private readonly int found = 1;
private readonly int nonexistent = 2;
private readonly int walkable = 0; // walkability array readonlyants
private readonly int unwalkable = 1;
private readonly int diagonal_cost = 14;
private readonly int horizontal_cost = 10;

//Create needed arrays
private int[] openList;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1 dimensional array holding ID# of open list items
private int[,] whichList;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2 dimensional array used to record
//Whether a cell is on the open list or on the closed list.
private int[] openX;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array stores the x location of an item on the open list
private int[] openY;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array stores the y location of an item on the open list
private int[,] parentX;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store parent of each cell (x)
private int[,] parentY;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store parent of each cell (y)
private float[] Fcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array to store F cost of a cell on the open list
private float[,] Gcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store G cost for each cell.
private float[] Hcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array to store H cost of a cell on the open list
private int pathLength; //stores length of the found path for critter
private int pathLocation; //stores current position along the chosen path for critter
private Vector3[] pathBank;



//Path reading variables
private int pathStatus;
private int xPath;
private int yPath;

public PathFinder (int MapSizeX, int MapSizeY, float WaypointDistance)
{

    int totalX = Mathf.RoundToInt (MapSizeX / WaypointDistance);
    int totalY = Mathf.RoundToInt (MapSizeY / WaypointDistance);
    //Correct is totalX * totalY, but we don't want it to try to create paths 5000 units long and lag the server, 
    //so we ruduce the array size allowing us to set a MAXIMUM path length befoe it gives up.
    int total = totalX * totalY;//2000;//totalX * totalY; 

    openList = new int[total]; 
    whichList = new int[totalX + 1, totalY + 1];
    openX = new int[total];
    openY = new int[total];
    parentX = new int[totalX, totalY];
    parentY = new int[totalX, totalY];
    Fcost = new float[total];
    Gcost = new float[totalX, totalY];
    Hcost = new float[total];

}

//==========================================================
//READ PATH DATA: These functions read the path data and convert
//it to screen pixel coordinates.

public Vector3[] GetPath (float WaypointDistance)
{
    Vector3[] pathBank2 = new Vector3 [pathBank.Length];
    for (int i = 0; i < pathBank2.Length; i++) {
        pathBank2 [i] = new Vector3 (Mathf.RoundToInt (pathBank [i].x * WaypointDistance), pathBank [i].y, Mathf.RoundToInt (pathBank [i].z * WaypointDistance));
    }
    return pathBank2;
}

//-----------------------------------------------------------------------------
// Name: FindPath
// Desc: Finds a path using A*
//-----------------------------------------------------------------------------
public int FindPath (ref Vector3[,,] ClipMap, Vector3 startPos, Vector3 endPos, int heightLevel, float entitySize, int MapSizeX, int MapSizeY, float WaypointDistance)
{

    int entityClearance = Mathf.CeilToInt (entitySize / 2 / WaypointDistance);
    int onOpenList = 0;
    int parentXval = 0;
    int parentYval = 0;
    int a = 0;
    int b = 0;
    int m = 0;
    int u = 0;
    int v = 0;
    int temp = 0;
    int corner = 0;
    int numberOfOpenListItems = 0;
    int addedGCost = 0;
    float tempGcost = 0;
    int path = 0;
    int tempx;
    int pathX;
    int pathY;
    int cellPosition;
    int newOpenListItemID = 0;
    //1. Convert location data to coordinates in the walkability array.
    int startX = Mathf.RoundToInt (startPos.x / WaypointDistance);
    int startY = Mathf.RoundToInt (startPos.z / WaypointDistance);
    int targetX = Mathf.RoundToInt (endPos.x / WaypointDistance);
    int targetY = Mathf.RoundToInt (endPos.z / WaypointDistance);

    if (targetX >= ClipMap.GetLength (0) || targetX < 0 || targetY >= ClipMap.GetLength (1) || targetY < 0 || heightLevel < 0 || heightLevel >= ClipMap.GetLength (2)) {
        Debug.Log ("Here");
        return nonexistent;
    }
    if(ClipMap[targetX, targetY, heightLevel].x == unwalkable) {
        Debug.Log ("Here");
        return nonexistent;

    }
    //If starting location and target are in the same location...
    if (startX == targetX && startY == targetY && pathLocation >= 0) {
        if (ClipMap [startX, startY, heightLevel].x == walkable) {
            pathBank = new Vector3[] {new Vector3 (startX, ClipMap [startX, startY, heightLevel].y, startY)};
            return found;
        } else {
            Debug.Log ("Here");
            return nonexistent;
        }
    }

    // If standing on unwalkable square, continue on last path
    if (ClipMap [startX, startY, heightLevel].x == unwalkable) {
        Debug.Log ("Here");
        return nonexistent;
    }

    //  If target square is unwalkable, return that it's a nonexistent path.
    if (ClipMap [startX, startY, heightLevel].x == unwalkable || ClipMap [targetX, targetY, heightLevel].z < entityClearance) {
        Debug.Log ("Here");
        goto noPath;
    }

    //3.Reset some variables that need to be cleared
    if (onClosedList > 1000000) { //reset whichList occasionally
        for (int x = 0; x < Mathf.RoundToInt (ClipMap.GetLength (0) / WaypointDistance); x++) {
            for (int y = 0; y < Mathf.RoundToInt (ClipMap.GetLength (1) / WaypointDistance); y++) {
                whichList [x, y] = 0;
            }
        }
        onClosedList = 10;
    }
    onClosedList = onClosedList + 2; //changing the values of onOpenList and onClosed list is faster than redimming whichList() array
    onOpenList = onClosedList - 1;
    pathLength = notStarted; //i.e, = 0
    pathLocation = notStarted; //i.e, = 0
    Gcost [startX, startY] = 0; //reset starting square's G value to 0

    //4.Add the starting location to the open list of squares to be checked.
    numberOfOpenListItems = 1;
    openList [1] = 1; //assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
    openX [1] = startX;
    openY [1] = startY;

    //5.Do the following until a path is found or deemed nonexistent.
    do {

        //6.If the open list is not empty, take the first cell off of the list.
        //  This is the lowest F cost cell on the open list.
        if (numberOfOpenListItems != 0) {

            //7. Pop the first item off the open list.
            parentXval = openX [openList [1]];
            parentYval = openY [openList [1]]; //record cell coordinates of the item
            whichList [parentXval, parentYval] = onClosedList; //add the item to the closed list

            //  Open List = Binary Heap: Delete this item from the open list, which
            //  is maintained as a binary heap. For more information on binary heaps, see:
            //  http://www.policyalmanac.org/games/binaryHeaps.htm
            numberOfOpenListItems = numberOfOpenListItems - 1; //reduce number of open list items by 1

            //  Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
            openList [1] = openList [numberOfOpenListItems + 1]; //move the last item in the heap up to slot #1
            v = 1;

            //  Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
            do {
                u = v;
                if (2 * u + 1 <= numberOfOpenListItems) { //if both children exist
                    //Check if the F cost of the parent is greater than each child.
                    //Select the lowest of the two children.
                    if (Fcost [openList [u]] >= Fcost [openList [2 * u]]) {
                        v = 2 * u;
                    }
                    if (Fcost [openList [v]] >= Fcost [openList [2 * u + 1]]) {
                        v = 2 * u + 1;
                    }
                } else {
                    if (2 * u <= numberOfOpenListItems) { //if only child #1 exists
                        //Check if the F cost of the parent is greater than child #1    
                        if (Fcost [openList [u]] >= Fcost [openList [2 * u]]) {
                            v = 2 * u;
                        }
                    }
                }

                if (u != v) { //if parent's F is > one of its children, swap them
                    temp = openList [u];
                    openList [u] = openList [v];
                    openList [v] = temp;
                } else
                    break; //otherwise, exit loop

            } while (true); //reorder the binary heap


            //7.Check the adjacent squares. (Its "children" -- these path children
            //  are similar, conceptually, to the binary heap children mentioned
            //  above, but don't confuse them. They are different. Path children
            //  are portrayed in Demo 1 with grey pointers pointing toward
            //  their parents.) Add these adjacent child squares to the open list
            //  for later consideration if appropriate (see various if statements
            //  below).
            for (b = parentYval - 1; b <= parentYval + 1; b++) {
                for (a = parentXval - 1; a <= parentXval + 1; a++) {

                    //  If not off the map (do this first to avoid array out-of-bounds errors)
                    if (a != -1 && b != -1 && a != ClipMap.GetLength (0) && b != ClipMap.GetLength (1)) {

                        //  If not already on the closed list (items on the closed list have
                        //  already been considered and can now be ignored).            
                        if (whichList [a, b] != onClosedList) {
                            //  If not a wall/obstacle square.
                            if (ClipMap [a, b, heightLevel].x != unwalkable && ClipMap [a, b, heightLevel].z >= entityClearance) {

                                //  Don't cut across corners
                                corner = walkable;
                                if (a == parentXval - 1) {
                                    if (b == parentYval - 1) {
                                        if (ClipMap [parentXval - 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval - 1, parentYval, heightLevel].z < entityClearance || ClipMap [parentXval, parentYval - 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval - 1, heightLevel].z < entityClearance) {
                                            corner = unwalkable;
                                        }
                                    } else if (b == parentYval + 1) {
                                        if (ClipMap [parentXval, parentYval + 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval + 1, heightLevel].z < entityClearance || ClipMap [parentXval - 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval - 1, parentYval, heightLevel].z < entityClearance) {
                                            corner = unwalkable;
                                        }
                                    }
                                } else if (a == parentXval + 1) {
                                    if (b == parentYval - 1) {
                                        if (ClipMap [parentXval, parentYval - 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval - 1, heightLevel].z < entityClearance || ClipMap [parentXval + 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval + 1, parentYval, heightLevel].z < entityClearance) {
                                            corner = unwalkable;
                                        }
                                    } else if (b == parentYval + 1) {
                                        if (ClipMap [parentXval + 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval + 1, parentYval, heightLevel].z < entityClearance || ClipMap [parentXval, parentYval + 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval + 1, heightLevel].z < entityClearance) {
                                            corner = unwalkable;
                                        }
                                    }
                                }
                                if (corner == walkable) {

                                    //  If not already on the open list, add it to the open list.           
                                    if (whichList [a, b] != onOpenList) {

                                        //Create a new open list item in the binary heap.
                                        newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
                                        m = numberOfOpenListItems + 1;
                                        openList [m] = newOpenListItemID; //place the new open list item (actually, its ID#) at the bottom of the heap
                                        if (newOpenListItemID >= openX.Length) {
                                            Debug.Log ("Here");
                                            return nonexistent;
                                        }
                                        openX [newOpenListItemID] = a;
                                        openY [newOpenListItemID] = b; //record the x and y coordinates of the new item

                                        //Figure out its G cost
                                        if (Mathf.Abs (a - parentXval) == 1 && Mathf.Abs (b - parentYval) == 1) {
                                            addedGCost = diagonal_cost; //cost of going to diagonal squares
                                        } else {
                                            addedGCost = horizontal_cost; //cost of going to non-diagonal squares
                                        }
                                        Gcost [a, b] = Gcost [parentXval, parentYval] + addedGCost;

                                        //Figure out its H and F costs and parent
                                        float d = horizontal_cost;
                                        float d2 = diagonal_cost;
                                        //Hcost [openList [m]] = 10 * (Mathf.Abs (a - targetX) + Mathf.Abs (b - targetY));
                                        int dx = Mathf.Abs (a - targetX);
                                        int dy = Mathf.Abs (b - targetY);
                                        /*int dx2 = startX - targetX;
                                        int dy2 = startY - targetY;
                                        int cross = Mathf.Abs (dx*dy2 - dx2*dy);
                                        float p = 0.001f;*/

                                        Hcost [openList [m]] = (d * (dx + dy) + (d2 - 2.0f * d) * (float) Mathf.Min (dx, dy));


                                        Fcost [openList [m]] = Gcost [a, b] + Hcost [openList [m]];
                                        parentX [a, b] = parentXval;
                                        parentY [a, b] = parentYval;

                                        //Move the new open list item to the proper place in the binary heap.
                                        //Starting at the bottom, successively compare to parent items,
                                        //swapping as needed until the item finds its place in the heap
                                        //or bubbles all the way to the top (if it has the lowest F cost).
                                        while (m != 1) { //While item hasn't bubbled to the top (m=1)
                                            //Check if child's F cost is < parent's F cost. If so, swap them.   
                                            if (Fcost [openList [m]] <= Fcost [openList [m / 2]]) {
                                                temp = openList [m / 2];
                                                openList [m / 2] = openList [m];
                                                openList [m] = temp;
                                                m = m / 2;
                                            } else
                                                break;
                                        }
                                        numberOfOpenListItems = numberOfOpenListItems + 1; //add one to the number of items in the heap

                                        //Change whichList to show that the new item is on the open list.
                                        whichList [a, b] = onOpenList;
                                    }

                                    //8.If adjacent cell is already on the open list, check to see if this 
                                    //  path to that cell from the starting location is a better one. 
                                    //  If so, change the parent of the cell and its G and F costs. 
                                    else { //If whichList(a,b) = onOpenList

                                        //Figure out the G cost of this possible new path
                                        if (Mathf.Abs (a - parentXval) == 1 && Mathf.Abs (b - parentYval) == 1) {
                                            addedGCost = diagonal_cost; //cost of going to diagonal tiles
                                        } else {
                                            addedGCost = horizontal_cost; //cost of going to non-diagonal tiles
                                        }
                                        tempGcost = Gcost [parentXval, parentYval] + addedGCost;

                                        //If this path is shorter (G cost is lower) then change
                                        //the parent cell, G cost and F cost.       
                                        if (tempGcost < Gcost [a, b]) { //if G cost is less,
                                            parentX [a, b] = parentXval; //change the square's parent
                                            parentY [a, b] = parentYval;
                                            Gcost [a, b] = tempGcost; //change the G cost

                                            //Because changing the G cost also changes the F cost, if
                                            //the item is on the open list we need to change the item's
                                            //recorded F cost and its position on the open list to make
                                            //sure that we maintain a properly ordered open list.
                                            for (int x = 1; x <= numberOfOpenListItems; x++) { //look for the item in the heap
                                                if (openX [openList [x]] == a && openY [openList [x]] == b) { //item found
                                                    Fcost [openList [x]] = Gcost [a, b] + Hcost [openList [x]]; //change the F cost

                                                    //See if changing the F score bubbles the item up from it's current location in the heap
                                                    m = x;
                                                    while (m != 1) { //While item hasn't bubbled to the top (m=1)
                                                        //Check if child is < parent. If so, swap them. 
                                                        if (Fcost [openList [m]] < Fcost [openList [m / 2]]) {
                                                            temp = openList [m / 2];
                                                            openList [m / 2] = openList [m];
                                                            openList [m] = temp;
                                                            m = m / 2;
                                                        } else
                                                            break;
                                                    }
                                                    break; //exit for x = loop
                                                } //If openX(openList(x)) = a
                                            } //For x = 1 To numberOfOpenListItems
                                        } //If tempGcost < Gcost(a,b)

                                    } //else If whichList(a,b) = onOpenList
                                } //If not cutting a corner
                            } //If not a wall/obstacle square.
                        } //If not already on the closed list
                    } //If not off the map
                } //for (a = parentXval-1; a <= parentXval+1; a++){
            } //for (b = parentYval-1; b <= parentYval+1; b++){

        } //if (numberOfOpenListItems != 0)

        //9.If open list is empty then there is no path.    
        else {
            Debug.Log ("Here");
            path = nonexistent;
            break;
        }

        //If target is added to open list then path has been found.
        if (whichList [targetX, targetY] == onOpenList) {
            path = found;
            break;
        }

    } while (true); //Do until path is found or deemed nonexistent

    //10.Save the path if it exists.
    if (path == found) {

        //a.Working backwards from the target to the starting location by checking
        //  each cell's parent, figure out the length of the path.
        pathX = targetX;
        pathY = targetY;
        do {
            //Look up the parent of the current cell.   
            tempx = parentX [pathX, pathY];
            pathY = parentY [pathX, pathY];
            pathX = tempx;

            //Figure out the path length
            pathLength = pathLength + 1;
        } while (pathX != startX || pathY != startY);

        //b.Resize the data bank to the right size in bytes
        pathBank = new Vector3[pathLength];

        //c. Now copy the path information over to the databank. Since we are
        //  working backwards from the target to the start location, we copy
        //  the information to the data bank in reverse order. The result is
        //  a properly ordered set of path data, from the first step to the
        //  last.
        pathX = targetX;
        pathY = targetY;
        cellPosition = pathLength; //start at the end
        do {
            cellPosition = cellPosition - 1; //work backwards 1 integer
            pathBank [cellPosition] = new Vector3 (pathX, ClipMap [pathX, pathY, heightLevel].y, pathY);

            //d.Look up the parent of the current cell. 
            tempx = parentX [pathX, pathY];
            pathY = parentY [pathX, pathY];
            pathX = tempx;

            //e.If we have reached the starting square, exit the loop.  
        } while (pathX != startX || pathY != startY);

    }
    return path;


    //13.If there is no path to the selected target, return non existant
noPath:
        return nonexistent;
}

Answer 1

What you need to use is not a pathfinder, but a line drawing algorithm. Basically, you're just drawing a line from A to B. If there were obstacles, you would need a pathfinder.

Check out Bresenham's Line Algorithm (https://en.wikipedia.org/wiki/Bresenham's_line_algorithm), that will get you there.

Answer 2

I had the same question as this. For anyone who finds it, my quick hack was to just add a random int between 0 and 3 to the movement cost when calculating neighbors. It's a quick hack that ends up with paths that are a little more random, but not so much so that it breaks the purpose of the search. While it sometimes creates these weird paths in open areas, more often, it creates a more direct path, albeit not the lowest cost.

Answer 3

What you are encountering is not an error. The unfortunate fact is that when pathfinding across empty space there are multiple paths A* can find which have equivalent value, and so it will choose a 'random' one based upon factors like the order it selects neighbours.

This problem is called 'tie breaking', which is to say A* makes a comparison and finds two or more neighbours with equivalent cost. In that case it will likely choose the first. Assuming your code checks north, east, south, west, it would explain why it seems to prefer going east for a long time.

There are multiple ways to try and resolve the issue (and unfortunately no quick fixes), for example, changing move cost or heuristic to alternate between horizonal and vertical, or changing the order neighbours are selected, or updating the priority so that new neighbours are pushed into higher priority than old neighbours of the same value.

Here’s the hack for A* and Dijkstra’s Algorithm: in the graph class, make the movement cost depend on (x + y) % 2:

when 0: make horizontal movement slightly more expensive when 1: make vertical movement slightly more expensive

Red Blob Games has a great repository of information on the issue, specifically here 'Ugly Paths' and here 'Heuristics'.

Steven van Dijk suggests that a more straightforward way to do this would to pass h to the comparison function. When the f values are equal, the comparison function would break the tie by looking at h.

Another way to break ties is to add a deterministic random number to the heuristic or edge costs. (One way to choose a deterministic random number is to compute a hash of the coordinates.) This breaks more ties than adjusting h as above. Thanks to Cris Fuhrman for suggesting this.

A different way to break ties is to prefer paths that are along the straight line from the starting point to the goal:

dx1 = current.x - goal.x 
dy1 = current.y - goal.y 
dx2 = start.x - goal.x 
dy2 = start.y - goal.y 
cross = abs(dx1*dy2 - dx2*dy1) 
heuristic += cross*0.001 

This code computes the vector cross-product between the start to goal vector and the current point to goal vector. When these vectors don’t line up, the cross product will be larger. The result is that this code will give some slight preference to a path that lies along the straight line path from the start to the goal. When there are no obstacles, A* not only explores less of the map, the path looks very nice as well

I don't know which option may work best for you, you'll probably have to try a few before finding which one is best suited to your needs.

EDIT:

I was encountering the same problem, and found the best solution is a combination of the above heuristic + cross, as well as the Breadth First hack which changes the order neighbours are selected.

Here’s the hack for Breadth First Search: in the graph class, make the list of neighbors depend on (x + y) % 2:

when 0: return the South, North, West, East neighbors when 1: return the East, West, North, South neighbors

That seems to resolve the issue both for straight diagonal lines, and irregular lines.