java的a星算法玩

package com;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

public class TMain {

    public static int[][] Fours = {
            {-1,0},
            {1,0},
            {0,-1},
            {0,1}
    };

    // 来个A星算法玩玩
    public static class Point2
    {
        public int x;
        public int y;

        // F = G + H; G(起点-当前点) H(当前点-终点)
        public float F;
        public float G;
        public float H;

        public Point2 parent;

        public Point2(int x, int y){
            this.x = x;
            this.y = y;
        }

        public void CalcF(Point2 s, Point2 e){
            this.G = Math.abs(s.x - this.x) + Math.abs(s.y - this.y);
            this.H = Math.abs(e.x - this.x) + Math.abs(e.y - this.y);
            this.F = this.G + this.H;
        }

        boolean eq(Point2 p){
            return p.x == this.x && p.y == this.y;
        }

        public String toString(){
            return "("+x+","+y+")F="+this.F;
        }
    }

    public static boolean Container(List<Point2> l, Point2 p ){
        for (Point2 pp : l) {
            if (pp.eq(p))
                return true;
        }
        return false;
    }

    public static Point2 SearMinF(List<Point2> l){
        Collections.sort(l, new Comparator<Point2>(){
            @Override
            public int compare(Point2 o1, Point2 o2) {
                if (o1.F > o2.F)
                    return 1;
                if (o1.F == o2.F)
                    return 0;
                return -1;
            }
        });
        // 如果有多个相同F的，获取最近加入的那个
        if (l.size() > 0)
            return l.get(0);
        return null;
    }

    /**
     * 查找4方的可以行走的线路
     * @param cur
     * @param s
     * @param e
     * @param w
     * @param h
     * @param mp
     * @return
     */
    public static List<Point2> SearNearPoint(Point2 cur, Point2 s, Point2 e, int w, int h, int[][] mp){
        List<Point2> nears = new ArrayList<>();
        for (int[] k : Fours){
            int x = cur.x + k[0];
            int y = cur.y + k[1];
            if (0 <= x && x < w && 0 <= y && y < h && mp[y][x] == 0){
                Point2 p = new Point2(x, y);
                p.CalcF(s, e);
                p.parent = cur;
                nears.add(p);
            }
        }
        return nears;
    }

    public static List<Point2> BuildPath(Point2 e){
        List<Point2> rs = new ArrayList<>();
        Point2 p = e.parent;
        while (p != null){
            rs.add(p);
            p = p.parent;
        }

        Collections.reverse(rs);
        return rs;
    }

    public static void Remove(List<Point2> l, Point2 p){
        if (!l.remove(p)){
            for (Point2 point2 : l) {
                if (point2.eq(p)){
                    l.remove(point2);
                    return;
                }
            }
        }
    }

    public static List<Point2> Search(Point2 s, Point2 e, int[][] mp){

        List<Point2> rs = new ArrayList<>();

        int h = mp.length;
        int w = mp[0].length;

        if (e.eq(s)){
            rs.add(s);
            rs.add(e);
            return rs;
        }

        List<Point2> opens = new ArrayList<>();
        List<Point2> closes = new ArrayList<>();

        s.CalcF(s, e);
        opens.add(s);

        while (opens.size() > 0){
            Point2 cur = SearMinF(opens);
            if (e.eq(cur)){
                return BuildPath(cur);
            }
            List<Point2> nears = SearNearPoint(cur, s, e, w, h, mp);
            for (Point2 n : nears) {
                if (!Container(closes, n) && !Container(opens, n)){
                    opens.add(n);
                }
            }

            Remove(opens, cur);
            closes.add(cur);
        }
        return rs;

    }

    public static void main(String[] args) {
        int[][] mp = {
                {1,0,0,0,1,1,1,0,0,0},
                {1,0,0,0,1,1,1,0,1,0},
                {1,0,0,0,1,1,1,0,1,0},
                {1,0,0,0,0,0,1,0,1,0},
                {1,0,0,0,1,0,1,0,1,0},
                {1,0,0,0,1,0,0,0,1,0},
                {1,0,0,0,1,1,1,1,1,0},
                {1,0,0,0,1,1,1,1,1,0},
                {1,0,0,0,1,1,0,0,0,0},
                {1,0,0,0,1,1,1,1,1,1}
                };

        List<Point2> rs = Search(new Point2(1,0), new Point2(6,8), mp);
        if (rs.size() == 0){
            System.out.println("没有找到路线");
            return;
        }
        if (rs.size() > 0){
            for (Point2 p : rs) {
                mp[p.y][p.x] = 3;
            }
        }

        for (int[] nx : mp) {
            String s = "";
            for (int k : nx){
                    s = s + k;
            }
            System.out.println("" + s);
        }
    }

}