Start算法实现

package com.oracle.Astar;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.TreeMap;



class Node implements Comparable<Node>{
    private int cross;
    private int column;
    private int h_score;
    private int g_score;
    private int f_score;
    Node(int cross,int column) {
        this.cross = cross;
        this.column = column;
    }
    Node(int cross,int column,int h_score,int g_score) {
        this.cross = cross;
        this.column = column;
        this.h_score = h_score;
        this.g_score = g_score;
        this.f_score = h_score + g_score;
    }
    public int getCross() {
        return cross;
    }
    public void setCross(int cross) {
        this.cross = cross;
    }
    public int getColumn() {
        return column;
    }
    public void setColumn(int column) {
        this.column = column;
    }
    public int getH_score() {
        return h_score;
    }
    public void setH_score(int h_score) {
        this.h_score = h_score;
    }
    public int getG_score() {
        return g_score;
    }
    public void setG_score(int g_score) {
        this.g_score = g_score;
    }
    public int getF_score() {
        return f_score;
    }
    public void setF_score(int f_score) {
        this.f_score = f_score;
    }

    @Override
    public boolean equals(Object obj) {
        Node o = (Node)obj;
        if(this.cross == o.cross && this.column == o.column) {
            return true;
        }else {
            return false;
        }
    }
    @Override
    public String toString() {
        return "("+this.cross+","+this.column+")";
    }
    @Override
    public int compareTo(Node o) {
        if(this.f_score > o.f_score) {
            return 1;
        }else if(this.f_score == o.f_score){
            return 0;
        }else {
            return -1;
        }
    }
    @Override
    public int hashCode() {
        String str = this.cross+""+this.column;
        return str.hashCode();
    }
}

class Astart{
    private Node start;
    private Node goal;
    public List<Node> openset = new ArrayList<>(); 
    public List<Node> closeset = new ArrayList<>();
    public Map<Node, Node> comaFrom  = new HashMap<>();
    public List<Node> path = new ArrayList<>();
    public int[][] nodes;

    Astart(int[][] nodes, Node start, Node goal) {
        this.nodes = nodes;
        this.start = start;
        this.goal = goal;
    }

    /**
     * 搜索路径
     * */
    public void find(Node node){
        ArrayList<Node> Neighbors = getNeighbors(nodes, node);
        if(closeset.contains(goal)) {
            //关闭表中有goal点，搜索完成
        }else {
            if(node.equals(this.start)) {
                //考虑起始点
                closeset.add(node);
            }
            for (Node n : Neighbors) {
                n.setG_score(getG_score(n, node));
                n.setH_score(getH_score(n, goal));
                n.setF_score(getF_score(n));
                if(openset.contains(n)) {
                    //G值判定
                    if(n.getG_score() < openset.get(openset.indexOf(n)).getG_score()) {
                        openset.add(n);
                        comaFrom.put(n, node);
                    }
                }else {
                    if( !(nodes[n.getCross()][n.getColumn()] == 1 || closeset.contains(n)) ){
                        //考虑相邻点为关闭点或者墙时
                        openset.add(n);
                        comaFrom.put(n, node);
                    }else {
                        closeset.add(n);
                    }
                }
            }
            Collections.sort(openset);
            node = openset.get(0);
            openset.remove(node);
            closeset.add(node);
            find(node);

        }
    }

    /**
     * 得到某点的所有相邻点
     * */
    public ArrayList<Node> getNeighbors(int[][] nodes, Node node) {
        int cross = node.getCross();
        int column = node.getColumn();
        ArrayList<Node> neighborNodes = new ArrayList<Node>();
        for(int i = cross-1; i <= cross+1; i++) {
            for(int j = column-1; j <= column+1; j++) {
                if(i>=0 && j>=0 && i<5 && j<7){
                    if(i == cross && j == column) {

                    }else{
                        neighborNodes.add(new Node(i,j));
                    }
                }
            }
        }
        return neighborNodes; 
    }

    /**
     * h_score-----当前点到目标点（B点）的理论移动距离,不考虑不可通过区域
     * */
    public int getH_score(Node node, Node goal) {
        return Math.abs(node.getCross()-goal.getCross())+Math.abs(node.getColumn()-goal.getColumn());
    }

    /**
     * g_score-----起始点（A点）到当前点的实际移动距离
     * */
    public int getG_score(Node node, Node fatherNode) {
        if(fatherNode.getColumn() == node.getColumn() || fatherNode.getCross() == node.getCross()) {
            return 10+fatherNode.getF_score();
        }else {
            return 14+fatherNode.getF_score();
        }
    }

    /**
     * f_score-----g_score + h_score
     * */
    public int getF_score(Node node) {
        return node.getH_score() + node.getG_score();
    }

    /**
     * 从comaFrom表中得到起点到终点的最短路径
     * */
    public void getPath(Node goal) {
        Node fatherNode = comaFrom.get(goal);
        if(fatherNode.equals(start)) {
            path.add(fatherNode);
        }else {
            path.add(fatherNode);
            getPath(fatherNode);
        }
    }

}

public class test {

    public static void main(String[] args) {
        int[][] nodes = {{0,0,0,0,0,0,0},
                        {0,0,0,1,0,0,0},
                        {0,0,0,1,0,0,0},
                        {0,0,0,1,0,0,0},
                        {0,0,0,0,0,0,0},};
        Node start = new Node(2,1,0,0);
        Node goal = new Node(2,5);
        Astart a = new Astart(nodes, start, goal);
        a.find(start);
        a.getPath(goal);
        Collections.reverse(a.path);
        System.out.println(a.path);

    }




}