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