c# - 如何根据 g-score 修正路径？

Question

我构建了 A* 引擎，用于在具有方形节点的网格中寻找最佳路径。每个节点有八个可能的连接到其他节点(上、下、左、右、topLeft、topRight、downLeft、downRight)。

我无法根据节点的 g 分数更正路径。当我发现当前节点的连接节点(相邻节点)时，必须进行此更正，并且对于已经在开放列表但不在封闭列表或不可行走的每个相邻节点，我需要做一些事情来检查该节点是否更适合路径使用g-score 来做决定。问题是我不知道该怎么做。

我有简单的代码来发现相邻节点:

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

我找到了解决方案，但该解决方案在一种情况下没有给出正确的路径。我会把我所有的代码放在一起，这样人们就可以改进和使用这个解决方案。如果您以任何方式改进此代码，请在此处发布，以便我们看到您的解决方案。谢谢!

首先，我更改了我的 calculateScore(...) 方法和他调用的方法。然后我实现上一篇文章中缺少的代码。

节点类:

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; 
    }

}

引擎类:

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;
    }

}