Java 设置A*搜索

我正在编写一个程序的一小部分，其中我必须编写一个寻路算法。该函数采用了所谓的“路线”，每个路线定义了二维空间中的起点和终点。该算法需要找到最短、最有效（在一定程度上）的路径（从原点）通过这些路线，从而使总行程最小化

我做了一些研究，并开始走上一条我认为可行的道路。到目前为止，我已经将路线转换成了一个有向图，所有的路线都连接在一起，就好像它是一个理想化的路线图。然后我尝试在这个图上执行A*搜索。我使用的启发式方法计算了“路线”剩余行驶的总距离，我使用的“距起点的距离”（G）值只是到达当前点的累计行驶距离。这对一些输入有效，但另一些输入根本不返回路径，我似乎不明白为什么

可能是我的启发式错误，这导致了某个地方的错误计算，还是更可能是a*过程本身错误？还是我完全走错了路

我将把getPath函数放在下面（用Java编写），以防万一

提前谢谢

public ArrayList<Vector2> getPath()
{
    PriorityQueue<SearchNode> openList = new PriorityQueue<SearchNode>(10, new SearchNodeComparator());
    ArrayList<SearchNode> closedList = new ArrayList<SearchNode>();

    map.startJobs();
    searchDepth = 0;

    SearchNode start = searchableGraph.getNode(new Vector2(0, 0));
    int goalsLeft = map.getJobCount();

    start.setDistanceTraveled(0);

    openList.add(start);

    while (openList.size() > 0)
    {
        SearchNode current = openList.peek();
        searchDepth++;

        if (map.isJobEndPoint(current.getValue()))
        {
            map.completeJob(current.getValue());
            goalsLeft--;

        }

        if (reachedGoalState(current, searchableGraph.getNodes().size()))
        {
            return getFinalPath(current);
        }
        else
        {
            ArrayList<SearchNode> neighbours = getNeighbours(current);

            for (int i = 0; i < neighbours.size(); i++)
            {
                SearchNode node = neighbours.get(i);        
                System.out.print("Inspecting node" + node.getValue().toString());

                float distanceTraveled = current.getDistanceTraveled() + getDistance(current.getValue(), node.getValue());

                float heuristic = heuristic(node);

                if (!openList.contains(node) && !closedList.contains(node))
                {

                    node.setDistanceTraveled(distanceTraveled);

                    node.setDistanceToGoal(distanceTraveled + heuristic);

                    node.setParent(current);

                    openList.add(node);
                }
                else if(openList.contains(node))
                {
                    if (node.getDistanceTraveled() <= distanceTraveled)
                    {

                        node.setDistanceToGoal(distanceTraveled + heuristic);


                        node.setParent(current);
                    }

                }
            }

            openList.remove(current);
            closedList.add(current);
        }
    }

    return new ArrayList<Vector2>();
}

public ArrayList getPath（）
{
PriorityQueue openList=newpriorityqueue（10，newsearchnodecomparator（））；
ArrayList closedList=新的ArrayList（）；
map.startJobs（）；
搜索深度=0；
SearchNode start=searchableGraph.getNode（新向量2（0,0））；
int goalsLeft=map.getJobCount（）；
start.setDistanceTraveled（0）；
openList.add（开始）；
while（openList.size（）>0）
{
SearchNode current=openList.peek（）；
searchDepth++；
if（map.isJobEndPoint（current.getValue（）））
{
map.completeJob（current.getValue（））；
左门--；
}
if（达到目标状态（当前，searchableGraph.getNodes（）.size（））
{
返回getFinalPath（当前）；
}
其他的
{
ArrayList Neights=GetNeights（当前）；
对于（int i=0；i<0.size（）；i++）
{
SearchNode=neights.get（i）；
System.out.print（“检查节点”+节点.getValue（）.toString（））；
float distanceTraveled=current.getDistanceTraveled（）+getDistance（current.getValue（），node.getValue（））；
浮动启发式=启发式（节点）；
如果（！openList.contains（节点）和&！closedList.contains（节点））
{
node.setDistanceTraveled（distanceTraveled）；
node.setDistanceToGoal（distanceTraveled+启发式）；
node.setParent（当前）；
添加（节点）；
}
else if（openList.contains（节点））
{
if（node.getDistanceTraveled（）
我使用的启发式算法计算出了剩下的“路线”的总距离

A*使用的启发式必须是；也就是说，它必须永不结束​估计到终点的距离

如果我正确理解你的描述，你的启发式是不允许的，因此不能保证有效