需要用到的类
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坐标点集合按照规则放入，直接调用返回点的顺序集合
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
