How to correct path based on g-score?

Question

I build A* engine for finding best path in grid with square nodes. Each node have eight possible connections to other nodes (up, down, left, right, topLeft, topRight, downLeft, downRight).

I have problem to correct path based on g-score of nodes. This correction must happen when i discover connected nodes (adjacent nodes) of current node, and for each adjacent node that is already in open list but not in closed list or unwalkable, I need to do something to check is that node better for path using g-score to make decision. The problem is that I don't know how to do that.

I have simple code where i do discovering adjacent nodes:

private ArrayList discoverChildren(Node parent) {
    ArrayList discoveredNodes = new ArrayList();
    if (parent.upNode != null && 
        parent.upNode.isWalkable &&
        !closedList.Contains(parent.upNode)) {
        if (!openList.Contains(parent.upNode)) {
            openList.Add(parent.upNode);
            parent.upNode.parent = parent;
            calculateScores(parent.upNode);
            discoveredNodes.Add(parent.upNode);
        } else {
            // Here i must check do I need to change path and update scores
        }       
    } ...

Answer 1

I found solution, but that solution don't give in one case correct path. I will put all my code, so people can improve and use this solution. If you improve this code in any way please post it here, so we can see your solution. Thanks!

First i made changes in my calculateScore(...) method and methods those he call. Then i implement missing code from previous post.

Node class:

using System;
using UnityEngine;
using System.Collections;

public class Node {
    public Cell cell {get; set;} // Unity Component that is linked to this node

    public bool isWalkable {get; set;}

    public int x {get; set;}
    public int z {get; set;}

    public Node rightNode {get; set;}
    public Node leftNode {get; set;}
    public Node upNode {get; set;}
    public Node downNode {get; set;}
    public Node upLeftNode {get; set;}
    public Node upRightNode {get; set;}
    public Node downLeftNode {get; set;}
    public Node downRightNode {get; set;}

    public Node[] children {get; set;}
    public Node parent {get; set;}

    public int gScore {get; set;}
    public int hScore {get; set;}
    public int fScore {get; set;}

    public void setGScore(Node start, bool isPenaltyApplied) {
        if (start.parent == null) {
            gScore = 0;
        } else {
            if (isPenaltyApplied) {
                gScore = start.parent.gScore + 14;
            } else {
                gScore = start.parent.gScore + 10;
            }
        }
    }

    public void setHScore(Node destination, bool isPenaltyApplied) {
        if (parent == null) {
            hScore = 0;
            return;
        }

        if (parent.hScore == 0) {
            hScore = Node.calculateHeuristic(this, destination) * 10;
        } else {
            hScore = parent.hScore + 10;
        }

        if (isPenaltyApplied) {
            hScore += 10;
        }
    }

    public void updateFScore() {
        fScore = gScore + hScore;
    }

    public Node[] getAllChildren() {
        if (this.children == null) {
            Node[] children = new Node[8];
            children[0] = rightNode;
            children[1] = leftNode;
            children[2] = upNode;
            children[3] = downNode;
            children[4] = upLeftNode;
            children[5] = upRightNode;
            children[6] = downLeftNode;
            children[7] = downRightNode;

            this.children = children;
        }
        return this.children;
    }

    public static int calculateHeuristic(Node from, Node to) {
        int distance = 0;

        int startX = from.x;
        int startZ = from.z;

        int destinationX = to.x;
        int destinationZ = to.z;

        while (startX != destinationX) {
            if (startX < destinationX) {
                startX++;
            } else if (startX > destinationX) {
                startX--;
            }

            distance++;
        }

        while (startZ != destinationZ) {
            if (startZ < destinationZ) {
                startZ++;
            } else if (startZ > destinationZ) {
                startZ--;
            }

            distance++;
        }

        return distance; 
    }

}

Engine class:

using System;
using UnityEngine;
using System.Collections;

public class Engine {
    private ArrayList openList;
    private ArrayList closedList;
    private ArrayList potentialPaths;

    public ArrayList path {get; private set;}
    public Node startNode {get; set;}
    public Node destinationNode {get; set;}

    public bool isPathFound {get; set;}

    public void startEngine() {
        if (startNode != null && destinationNode != null) {
            openList = new ArrayList();
            closedList = new ArrayList();

            traverseNodes();
            if (isPathFound) {
                path = findPath();
            }
        }
    }

    private ArrayList findPath() {
        ArrayList bestPath = new ArrayList();
        Node currentNode = destinationNode;

        while (!object.ReferenceEquals(currentNode, startNode)) {
            bestPath.Add(currentNode);
            clearUnnecessaryNode(currentNode);
            currentNode = currentNode.parent;
        }

        bestPath.Add(currentNode);
        return bestPath;
    }

    private void clearUnnecessaryNode(Node node) {
        Node firstParent = node.parent;
        if (object.ReferenceEquals(firstParent, startNode)) {
            return;
        }

        Node secondParent = firstParent.parent;
        if (object.ReferenceEquals(node.upRightNode, secondParent)) {
            if (node.rightNode != null &&
                node.upNode != null &&
                node.rightNode.isWalkable &&
                node.upNode.isWalkable) {
                node.parent = secondParent;
                path.Remove(firstParent);
            }
        }

        if (object.ReferenceEquals(node.upLeftNode, secondParent)) {
            if (node.leftNode != null &&
                node.upNode != null &&
                node.leftNode.isWalkable &&
                node.upNode.isWalkable) {
                node.parent = secondParent;
                path.Remove(firstParent);
            }
        }

        if (object.ReferenceEquals(node.downLeftNode, secondParent)) {
            if (node.leftNode != null &&
                node.downNode != null &&
                node.leftNode.isWalkable &&
                node.downNode.isWalkable) {
                node.parent = secondParent;
                path.Remove(firstParent);
            }
        }

        if (object.ReferenceEquals(node.downRightNode, secondParent)) {
            if (node.rightNode != null &&
                node.downNode != null &&
                node.rightNode.isWalkable &&
                node.downNode.isWalkable) {
                node.parent = secondParent;
                path.Remove(firstParent);
            }
        }
    }

    private void traverseNodes() {
        Node bestNode = startNode;
        openList.Add(bestNode);
        calculateScores(bestNode, false);

        while (!object.ReferenceEquals(bestNode, destinationNode)) {
            discoverChildren(bestNode);
            closedList.Add(bestNode);
            openList.Remove(bestNode);
            bestNode = chooseBestNode(bestNode);

            if (bestNode == null) {
                // Path not found
                return;
            }

            if (object.ReferenceEquals(bestNode, destinationNode)) {
                closedList.Add(bestNode);
            }
        }
        // Success
        isPathFound = true;
    }

    private void calculateScores(Node node, bool isPenaltyApplied) {
        node.setGScore(startNode, isPenaltyApplied);
        node.setHScore(destinationNode, isPenaltyApplied);
        node.updateFScore();
    }

    private ArrayList discoverChildren(Node parent) {
        ArrayList discoveredNodes = new ArrayList();
        int currentCost = 0;
        int newCost = 0;
        if (parent.upNode != null && 
            parent.upNode.isWalkable &&
            !closedList.Contains(parent.upNode)) {
            if (!openList.Contains(parent.upNode)) {
                openList.Add(parent.upNode);
                parent.upNode.parent = parent;
                calculateScores(parent.upNode, false);
                discoveredNodes.Add(parent.upNode);
            } else {
                currentCost = parent.upNode.parent.gScore + parent.upNode.gScore;
                newCost = parent.gScore + parent.upNode.gScore;
                if (newCost < currentCost) {
                    parent.upNode.parent = parent;
                    calculateScores(parent.upNode, false);
                }
            }       
        }
        if (parent.downNode != null && 
            parent.downNode.isWalkable &&
            !closedList.Contains(parent.downNode)) {
            if (!openList.Contains(parent.downNode)) {
                openList.Add(parent.downNode);
                parent.downNode.parent = parent;
                calculateScores(parent.downNode, false);
                discoveredNodes.Add(parent.downNode);
            } else {
                currentCost = parent.downNode.parent.gScore + parent.downNode.gScore;
                newCost = parent.gScore + parent.downNode.gScore;
                if (newCost < currentCost) {
                    parent.downNode.parent = parent;
                    calculateScores(parent.downNode, false);
                }
            }
        }
        if (parent.leftNode != null && 
            parent.leftNode.isWalkable &&
            !closedList.Contains(parent.leftNode)) {
            if (!openList.Contains(parent.leftNode)) {
                openList.Add(parent.leftNode);
                parent.leftNode.parent = parent;
                calculateScores(parent.leftNode, false);
                discoveredNodes.Add(parent.leftNode);
            } else {
                currentCost = parent.leftNode.parent.gScore + parent.leftNode.gScore;
                newCost = parent.gScore + parent.leftNode.gScore;
                if (newCost < currentCost) {
                    parent.leftNode.parent = parent;
                    calculateScores(parent.leftNode, false);
                }
            }
        }
        if (parent.rightNode != null && 
            parent.rightNode.isWalkable &&
            !closedList.Contains(parent.rightNode)) {
            if (!openList.Contains(parent.rightNode)) {
                openList.Add(parent.rightNode);
                parent.rightNode.parent = parent;
                calculateScores(parent.rightNode, false);
                discoveredNodes.Add(parent.rightNode);
            } else {
                currentCost = parent.rightNode.parent.gScore + parent.rightNode.gScore;
                newCost = parent.gScore + parent.rightNode.gScore;
                if (newCost < currentCost) {
                    parent.rightNode.parent = parent;
                    calculateScores(parent.rightNode, false);
                }
            }
        }
        if (parent.upRightNode != null && 
            parent.upNode != null &&
            parent.rightNode != null &&
            parent.upRightNode.isWalkable && 
            parent.upNode.isWalkable &&
            parent.rightNode.isWalkable &&
            !closedList.Contains(parent.upRightNode)) {
            if (!openList.Contains(parent.upRightNode)) {
                openList.Add(parent.upRightNode);
                parent.upRightNode.parent = parent;
                calculateScores(parent.upRightNode, true);
                discoveredNodes.Add(parent.upRightNode);
            } else {
                currentCost = parent.upRightNode.parent.gScore + parent.upRightNode.gScore;
                newCost = parent.gScore + parent.upRightNode.gScore;
                if (newCost < currentCost) {
                    parent.upRightNode.parent = parent;
                    calculateScores(parent.upRightNode, true);
                }
            }
        }
        if (parent.upLeftNode != null && 
            parent.upNode != null &&
            parent.leftNode != null &&
            parent.upLeftNode.isWalkable && 
            parent.upNode.isWalkable &&
            parent.leftNode.isWalkable &&
            !closedList.Contains(parent.upLeftNode)) {
            if (!openList.Contains(parent.upLeftNode)) {
                openList.Add(parent.upLeftNode);
                parent.upLeftNode.parent = parent;
                calculateScores(parent.upLeftNode, true);
                discoveredNodes.Add(parent.upLeftNode);
            } else {
                currentCost = parent.upLeftNode.parent.gScore + parent.upLeftNode.gScore;
                newCost = parent.gScore + parent.upLeftNode.gScore;
                if (newCost < currentCost) {
                    parent.upLeftNode.parent = parent;
                    calculateScores(parent.upLeftNode, true);
                }
            }
        }
        if (parent.downRightNode != null && 
            parent.downNode != null &&
            parent.rightNode != null &&
            parent.downRightNode.isWalkable && 
            parent.downNode.isWalkable &&
            parent.rightNode.isWalkable &&
            !closedList.Contains(parent.downRightNode)) {
            if (!openList.Contains(parent.downRightNode)) {
                openList.Add(parent.downRightNode);
                parent.downRightNode.parent = parent;
                calculateScores(parent.downRightNode, true);
                discoveredNodes.Add(parent.downRightNode);
            } else {
                currentCost = parent.downRightNode.parent.gScore + parent.downRightNode.gScore;
                newCost = parent.gScore + parent.downRightNode.gScore;
                if (newCost < currentCost) {
                    parent.downRightNode.parent = parent;
                    calculateScores(parent.downRightNode, true);
                }
            }
        }
        if (parent.downLeftNode != null && 
            parent.downNode != null &&
            parent.leftNode != null &&
            parent.downLeftNode.isWalkable && 
            parent.downNode.isWalkable &&
            parent.leftNode.isWalkable &&
            !closedList.Contains(parent.downLeftNode)) {
            if (!openList.Contains(parent.downLeftNode)) {
                openList.Add(parent.downLeftNode);
                parent.downLeftNode.parent = parent;
                calculateScores(parent.downLeftNode, true);
                discoveredNodes.Add(parent.downLeftNode);
            } else {
                currentCost = parent.downLeftNode.parent.gScore + parent.downLeftNode.gScore;
                newCost = parent.gScore + parent.downLeftNode.gScore;
                if (newCost < currentCost) {
                    parent.downLeftNode.parent = parent;
                    calculateScores(parent.downLeftNode, true);
                }
            }
        }

        return discoveredNodes;
    }

    private Node chooseBestNode(Node closedNode) {
        if (openList.Count == 0) {
            return null;
        }

        Node bestNode = (Node)openList[0];
        foreach (Node node in openList) {
            if (node.fScore < bestNode.fScore) {
                bestNode = node;
            } else if (node.fScore == bestNode.fScore) {
                if (node.gScore < bestNode.gScore) {
                    bestNode = node;
                } else if (object.ReferenceEquals(node, closedNode.upNode) ||
                    object.ReferenceEquals(node, closedNode.leftNode) ||
                    object.ReferenceEquals(node, closedNode.rightNode) ||
                    object.ReferenceEquals(node, closedNode.downNode)) {
                    // priority have cell that is not in corner of current cell
                    bestNode = node;
                }
            }
        }

        return bestNode;
    }

}