java 三维坐标根据起点终点导航计算

àì夳堔傛蜴生んèń 2023-05-29 02:35 2阅读 0赞

需要用到的类

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坐标点集合按照规则放入，直接调用返回点的顺序集合