需要用到的类
package com.jwuwb.library.service.nav.core; import java.util.ArrayList; import java.util.Arrays; public class GisPoint { //坐标 private Vector3 coordinate; //父节点 private GisPoint parent; //ID 连接信息 private String[] idConnections; //ID private String id; //F private double F; //G private double G; //H private double H; //是否可用 private boolean isValidate; //连接点 private ArrayList<GisPoint> connections; //构造函数 public GisPoint() { coordinate = new Vector3(0, 0, 0); parent = null; F = 0; G = 0; H = 0; isValidate = true; connections = new ArrayList<GisPoint>(); } //构造函数 public GisPoint(double x, double y, double z) { coordinate = new Vector3(x, y, z); parent = null; F = 0; G = 0; H = 0; isValidate = true; connections = new ArrayList<GisPoint>(); } //添加连接 public void addConnection(GisPoint point) { connections.add(point); } //获取F public double getF() { return getG() + getH(); } //获取坐标 public Vector3 getCoordinate() { return coordinate; } //设置坐标 public void setCoordinate(Vector3 coordinate) { this.coordinate = coordinate; } //获取父 public GisPoint getParent() { return parent; } //设置父 public void setParent(GisPoint parent) { this.parent = parent; } //获取 G public double getG() { return G; } //设置G public void setG(double g) { G = g; } //获取 H public double getH() { return H; } //设置 H public void setH(double h) { H = h; } //是否可用 public boolean isValidate() { return isValidate; } //设置是否可用 public void setValidate(boolean validate) { isValidate = validate; } //获取连接数量 public int connectionCount() { return connections.size(); } //获取连接点 public GisPoint getConnectionPoint(int i) { return connections.get(i); } //获取ID连接 public String[] getIdConnections() { return idConnections; } //设置ID连接 public void setIdConnections(String[] idConnections) { this.idConnections = idConnections; } //获取ID public String getId() { return id; } //设置ID public void setId(String id) { this.id = id; } //克隆 @Override public GisPoint clone() { GisPoint ret = new GisPoint(); ret.setCoordinate(getCoordinate()); ret.setValidate(true); ret.setParent(null); ret.setId(getId()); ret.idConnections = getIdConnections(); return ret; } } package com.jwuwb.library.service.nav.core; import java.util.ArrayList; import java.util.HashMap; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; public class GisSpace { /** * 本类唯一对象 */ private static GisSpace gisSpace = null; /** * gis 点信息 */ private Map<String, ArrayList<GisPoint>> mGisPoints; /** * 构造函数 */ private GisSpace() { mGisPoints = new ConcurrentHashMap<>(new HashMap<>()); } /** * 获取本类唯一对象 * * @return */ public static GisSpace getRef() { if (gisSpace == null) gisSpace = new GisSpace(); return gisSpace; } /** * 构建gis连接 * * @param gisSet */ public static void connectionGis(ArrayList<GisPoint> gisSet) { for (GisPoint gp : gisSet) { for (String conId : gp.getIdConnections()) { for (GisPoint toGp : gisSet) { if (conId.equals(toGp.getId())) { gp.addConnection(toGp); break; } } } } } /** * 添加GIS信息点 * * @param mapUniqueKey 地图UID * @param point 点坐标 */ public void addGisPoint(String mapUniqueKey, ArrayList<GisPoint> point) { mGisPoints.put(mapUniqueKey, point); } /** * 拷贝一个GIS列表 * * @return */ public ArrayList<GisPoint> cloneGisPoints() { ArrayList<GisPoint> ret = new ArrayList<GisPoint>(); ArrayList<GisPoint> forMapGis = mGisPoints.get("map"); if (forMapGis != null) { for (int i = 0; i < forMapGis.size(); i++) { GisPoint oldGP = mGisPoints.get("map").get(i); ret.add(oldGP.clone()); } } //建立 GIS 连接 connectionGis(ret); return ret; } /** * 清除所有GIS点 */ public void clearGisPoint() { mGisPoints.clear(); } } package com.jwuwb.library.service.nav.core; import java.util.ArrayList; public class Navigation { //GIS信息点 private ArrayList<GisPoint> mGISPoints = null; //可用的GIS信息列表 private ArrayList<GisPoint> mAvaliableGisPoint = null; public Navigation() { mAvaliableGisPoint = new ArrayList<GisPoint>(); mGISPoints = GisSpace.getRef().cloneGisPoints(); } //导航 public ArrayList<Point> doNav(Point start, Point end) { //如果 gisPoints 没有则返回空 if (mGISPoints.size() == 0) { return new ArrayList<Point>(); } //获取未知点到起点最近的一个 GIS 点 GisPoint startGP = getClosestGisPoint(start); //获取未知点到结束点最近的一个 GIS 点 GisPoint endGP = getClosestGisPoint(end); //起点等于终点 if (startGP == endGP) { ArrayList<Point> retV = new ArrayList<>(); retV.add(start); retV.add(startGP.getCoordinate()); retV.add(end); return retV; } //添加起点 mAvaliableGisPoint.add(startGP); //初始化 H 值 for (int i = 0; i < mGISPoints.size(); i++) { GisPoint igp = mGISPoints.get(i); initGisPointH(igp, endGP); igp.setValidate(true); igp.setParent(null); } //获取 F 值最小的 GIS 点 GisPoint curGP = getMinFGisPoint(); //标注搜索是否结束 boolean skip = false; //开始搜索 do { //搜索列表为空则退出 if (mAvaliableGisPoint.size() == 0) { break; } //从可用列表中移除 removeGisPointFromAvaliableList(curGP); curGP.setValidate(false); //搜索当前点的连接点 for (int i = 0; i < curGP.connectionCount(); i++) { //获取连接点 GisPoint newGP = curGP.getConnectionPoint(i); //如果搜索已经到了结束点则跳出 if (newGP == endGP) { skip = true; endGP.setParent(curGP); break; } //计算新的G值 double tempG = curGP.getCoordinate().distance(newGP.getCoordinate()); double newG = tempG + curGP.getG(); //如果当前点已经失效，则继续下一个点 if (newGP.isValidate() == false) { continue; } //当前点已经在可用列表中 if (isGisPointInAvaliableList(newGP) == true) { if (newGP.getG() < newG) { } else { newGP.setParent(curGP); newGP.setG(newG); } } else { newGP.setParent(curGP); newGP.setG(newG); mAvaliableGisPoint.add(newGP); } } //选择 F 值最小的一个 GIS 点,继续搜索 curGP = getMinFGisPoint(); } while (skip == false); //取出点信息 ArrayList<Point> ret = new ArrayList<Point>(); getNavPathPoint(endGP, ret); return ret; } //取出 路径的 gis 点 void getNavPathPoint(GisPoint parent, ArrayList<Point> ret) { if (parent == null) return; ret.add(parent.getCoordinate()); if (parent != null) { getNavPathPoint(parent.getParent(), ret); } } //获取离某点最近的GIS点 private GisPoint getClosestGisPoint(Point p) { double minDis = 99999999999.0; GisPoint ret = null; for (int i = 0; i < mGISPoints.size(); i++) { GisPoint gp = mGISPoints.get(i); double dis = gp.getCoordinate().distance(p); if (dis < minDis) { minDis = dis; ret = gp; } } return ret; } //点是否在可用列表中 private boolean isGisPointInAvaliableList(GisPoint p) { for (int i = 0; i < mAvaliableGisPoint.size(); i++) { GisPoint gp = mAvaliableGisPoint.get(i); if (gp == p) { return true; } } return false; } //获取 G 值最小的一个点 private GisPoint getMinGGisPoint() { double minG = 9999999999999.0; GisPoint ret = null; for (int i = 0; i < mAvaliableGisPoint.size(); i++) { GisPoint gp = mAvaliableGisPoint.get(i); if (gp.getG() < minG) { minG = gp.getG(); ret = gp; } } return ret; } //获取 F 值最小的一个点 private GisPoint getMinFGisPoint() { double minF = 9999999999999.0; GisPoint ret = null; for (int i = 0; i < mAvaliableGisPoint.size(); i++) { GisPoint gp = mAvaliableGisPoint.get(i); if (gp.getF() < minF) { minF = gp.getF(); ret = gp; } } return ret; } //从可用列表中移除 private void removeGisPointFromAvaliableList(GisPoint p) { for (int i = 0; i < mAvaliableGisPoint.size(); i++) { GisPoint gp = mAvaliableGisPoint.get(i); if (gp == p) { mAvaliableGisPoint.remove(p); return; } } } //初始化 H 值 private void initGisPointH(GisPoint cur, GisPoint endP) { double dis = cur.getCoordinate().distance(endP.getCoordinate()); cur.setH(dis); } } package com.jwuwb.library.service.nav.core; public class Point{ private double x; private double y; private double z; //构造函数 public Point() { x = 0; y = 0; z = 0; } //构造函数 public Point(double _x, double _y, double _z) { x = _x; y = _y; z = _z; } //计算2线段是否相交 public static boolean IsSegmentsCross(Point a, Point b, Point c, Point d) { // 三角形abc 面积的2倍 double area_abc = (a.x - c.x) * (b.z - c.z) - (a.z - c.z) * (b.x - c.x); // 三角形abd 面积的2倍 double area_abd = (a.x - d.x) * (b.z - d.z) - (a.z - d.z) * (b.x - d.x); // 面积符号相同则两点在线段同侧,不相交 if (area_abc * area_abd >= 0) { return false; } // 三角形cda 面积的2倍 double area_cda = (c.x - a.x) * (d.z - a.z) - (c.z - a.z) * (d.x - a.x); // 三角形cdb 面积的2倍 double area_cdb = area_cda + area_abc - area_abd; if (area_cda * area_cdb >= 0) { return false; } //计算交点坐标 double t = area_cda / (area_abd - area_abc); double dx = t * (b.x - a.x), dy = t * (b.z - a.z); // x: a.x + dx //z: a.z + dy return true; } //从字符串获取Point类型 public static Point FromString(String str, String splitStr) { Point ret = new Point(0, 0, 0); String[] sourceStrArray = str.split(splitStr); if (sourceStrArray.length == 3) { double x = Double.parseDouble(sourceStrArray[0]); double y = Double.parseDouble(sourceStrArray[1]); double z = Double.parseDouble(sourceStrArray[2]); ret.x = x; ret.y = y; ret.z = z; } return ret; } //获取X public double getX() { return x; } //设置X public void setX(double x) { this.x = x; } //获取Y public double getY() { return y; } //设置Y public void setY(double y) { this.y = y; } //获取Z public double getZ() { return z; } //设置z public void setZ(double z) { this.z = z; } //加 public Point add(Point v) { Point ret = new Point(); ret.x = x + v.x; ret.y = y + v.y; ret.z = z + v.z; return ret; } //减 public Point sub(Point v) { Point ret = new Point(); ret.x = x - v.x; ret.y = y - v.y; ret.z = z - v.z; return ret; } //乘 public Point mul(Point v) { Point ret = new Point (); ret.x = x * v.x; ret.y = y * v.y; ret.z = z * v.z; return ret; } //乘 public Point mul(double v) { Point ret = new Point (); ret.x = x * v; ret.y = y * v; ret.z = z * v; return ret; } //除 public Point div(Point v) { Point ret = new Point (); ret.x = x / v.x; ret.y = y / v.y; ret.z = z / v.z; return ret; } //点积 public double dot(Point v) { double ret = 0; ret = x * v.x + y * v.y + z * v.z; return ret; } //叉乘 public Point cross(Point v) { Point ret = new Point (); ret.x = y * v.z - z * v.y; ret.y = z * v.x - x * v.z; ret.z = x * v.y - y * v.x; return ret; } //长度 public double length() { return Math.sqrt(x * x + y * y + z * z); } //距离 public double distance(Point v) { double x1 = x - v.x; double y1 = y - v.y; double z1 = z - v.z; return Math.sqrt(x1 * x1 + y1 * y1 + z1 * z1); } //归一化 public Point normalize() { double len = length(); return new Point (x / len, y / len, z / len); } @Override public String toString() { return "Point {" + "x=" + x + ", y=" + y + ", z=" + z + '}'; } } package com.jwuwb.library.service.nav; import java.util.ArrayList; import java.util.List; public class AStar { public static final int[][] NODES = { {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, }; public static final int STEP = 10; private ArrayList<Node> openList = new ArrayList<Node>(); private ArrayList<Node> closeList = new ArrayList<Node>(); public Node findMinFNodeInOpneList() { Node tempNode = openList.get(0); for (Node node : openList) { if (node.F < tempNode.F) { tempNode = node; } } return tempNode; } public ArrayList<Node> findNeighborNodes(Node currentNode) { ArrayList<Node> arrayList = new ArrayList<Node>(); // 只考虑上下左右，不考虑斜对角 int topX = currentNode.x; int topY = currentNode.y - 1; if (canReach(topX, topY) && !exists(closeList, topX, topY)) { arrayList.add(new Node(topX, topY)); } int bottomX = currentNode.x; int bottomY = currentNode.y + 1; if (canReach(bottomX, bottomY) && !exists(closeList, bottomX, bottomY)) { arrayList.add(new Node(bottomX, bottomY)); } int leftX = currentNode.x - 1; int leftY = currentNode.y; if (canReach(leftX, leftY) && !exists(closeList, leftX, leftY)) { arrayList.add(new Node(leftX, leftY)); } int rightX = currentNode.x + 1; int rightY = currentNode.y; if (canReach(rightX, rightY) && !exists(closeList, rightX, rightY)) { arrayList.add(new Node(rightX, rightY)); } return arrayList; } public boolean canReach(int x, int y) { if (x >= 0 && x < NODES.length && y >= 0 && y < NODES[0].length) { return NODES[x][y] == 0; } return false; } public Node findPath(Node startNode, Node endNode) { // 把起点加入 open list openList.add(startNode); while (openList.size() > 0) { // 遍历 open list ，查找 F值最小的节点，把它作为当前要处理的节点 Node currentNode = findMinFNodeInOpneList(); // 从open list中移除 openList.remove(currentNode); // 把这个节点移到 close list closeList.add(currentNode); ArrayList<Node> neighborNodes = findNeighborNodes(currentNode); for (Node node : neighborNodes) { if (exists(openList, node)) { foundPoint(currentNode, node); } else { notFoundPoint(currentNode, endNode, node); } } if (find(openList, endNode) != null) { return find(openList, endNode); } } return find(openList, endNode); } private void foundPoint(Node tempStart, Node node) { int G = calcG(tempStart, node); if (G < node.G) { node.parent = tempStart; node.G = G; node.calcF(); } } private void notFoundPoint(Node tempStart, Node end, Node node) { node.parent = tempStart; node.G = calcG(tempStart, node); node.H = calcH(end, node); node.calcF(); openList.add(node); } private int calcG(Node start, Node node) { int G = STEP; int parentG = node.parent != null ? node.parent.G : 0; return G + parentG; } private int calcH(Node end, Node node) { int step = Math.abs(node.x - end.x) + Math.abs(node.y - end.y); return step * STEP; } public static void main(String[] args) { Node startNode = new Node(5, 1); Node endNode = new Node(5, 5); Node parent = new AStar().findPath(startNode, endNode); for (int i = 0; i < NODES.length; i++) { for (int j = 0; j < NODES[0].length; j++) { System.out.print(NODES[i][j] + ", "); } System.out.println(); } ArrayList<Node> arrayList = new ArrayList<>(); while (parent != null) { arrayList.add(new Node(parent.x, parent.y)); parent = parent.parent; } System.out.println("\n"); for (int i = 0; i < NODES.length; i++) { for (int j = 0; j < NODES[0].length; j++) { if (exists(arrayList, i, j)) { System.out.print("@, "); } else { System.out.print(NODES[i][j] + ", "); } } System.out.println(); } } public static Node find(List<Node> nodes, Node point) { for (Node n : nodes) if ((n.x == point.x) && (n.y == point.y)) { return n; } return null; } public static boolean exists(List<Node> nodes, Node node) { for (Node n : nodes) { if ((n.x == node.x) && (n.y == node.y)) { return true; } } return false; } public static boolean exists(List<Node> nodes, int x, int y) { for (Node n : nodes) { if ((n.x == x) && (n.y == y)) { return true; } } return false; } public static class Node { public Node(int x, int y) { this.x = x; this.y = y; } public int x; public int y; public int F; public int G; public int H; public void calcF() { this.F = this.G + this.H; } public Node parent; } } public JSONArray calculate(String startPos, String endPos) throws Exception { Point startPoint = Point.FromString(startPos, ","); Point endPoint = Point.FromString(endPos, ","); //获取导航数据 Navigation nav = new Navigation(); List<Point> resultPoints = nav.doNav(startPoint, endPoint); JSONArray array = new JSONArray(); array.add(startPoint); for (int i = resultPoints.size() - 1; i >= 0; i--) { array.add(resultPoints.get(i)); } array.add(endPoint); return array; }最后是怎么使用，初始化可以将我们的x,y,z坐标点集合按照规则放入，直接调用返回点的顺序集合
