| | |
|---|
| | | package com.zy.common.utils; |
|---|
| | | |
|---|
| | | import java.util.ArrayList; |
|---|
| | | import java.util.Collections; |
|---|
| | | import java.util.HashSet; |
|---|
| | | import java.util.List; |
|---|
| | | import org.springframework.stereotype.Component; |
|---|
| | | |
|---|
| | | import com.alibaba.fastjson.JSON; |
|---|
| | | import com.alibaba.fastjson.JSONObject; |
|---|
| | | import com.core.exception.CoolException; |
|---|
| | | import com.zy.common.model.NavigateNode; |
|---|
| | | import com.zy.core.enums.ShuttleTaskModeType; |
|---|
| | | |
|---|
| | | import java.util.*; |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * A*算法使用工具 |
|---|
| | | */ |
|---|
| | | @Component |
|---|
| | | public class NavigateUtils { |
|---|
| | | |
|---|
| | | public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType) { |
|---|
| | | //通过开始编号和结束编号获取对应的xy轴坐标 |
|---|
| | | int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点 |
|---|
| | | int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点 |
|---|
| | | public synchronized List<NavigateNode> calcByStationId(Integer startStationId, Integer endStationId) { |
|---|
| | | NavigateSolution navigateSolution = new NavigateSolution(); |
|---|
| | | List<List<NavigateNode>> stationMap = navigateSolution.getStationMap(); |
|---|
| | | |
|---|
| | | //初始化开始节点 |
|---|
| | | NavigateNode start = new NavigateNode(startArr[0], startArr[1]); |
|---|
| | | //开始节点无父节点 |
|---|
| | | start.setFather(null); |
|---|
| | | NavigateNode startNode = navigateSolution.findStationNavigateNode(stationMap, startStationId); |
|---|
| | | if (startNode == null){ |
|---|
| | | throw new CoolException("未找到该 起点 对应的节点"); |
|---|
| | | } |
|---|
| | | |
|---|
| | | NavigateNode end = new NavigateNode(endArr[0], endArr[1]); |
|---|
| | | NavigateSolution solution = new NavigateSolution(mapType); |
|---|
| | | NavigateNode res_node = solution.astarSearch(start, end); |
|---|
| | | NavigateNode endNode = navigateSolution.findStationNavigateNode(stationMap, endStationId); |
|---|
| | | if (endNode == null){ |
|---|
| | | throw new CoolException("未找到该 终点 对应的节点"); |
|---|
| | | } |
|---|
| | | |
|---|
| | | NavigateNode res_node = navigateSolution.astarSearchJava(stationMap, startNode, endNode); |
|---|
| | | if (res_node == null) { |
|---|
| | | System.out.println("未找到路径"); |
|---|
| | | return null; |
|---|
| | | } else { |
|---|
| | | ArrayList<NavigateNode> list = new ArrayList<>(); |
|---|
| | | |
|---|
| | | //渲染 |
|---|
| | | NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算 |
|---|
| | | while (res_node != null) { |
|---|
| | | res_node.setDirection(null); |
|---|
| | | res_node.setIsInflectionPoint(false); |
|---|
| | | |
|---|
| | | //寻找拐点 |
|---|
| | | HashMap<String, Object> result = searchInflectionPoint(res_node, fatherNode, res_node.getFather());//分别传入当前节点、父节点、下一节点 |
|---|
| | | //判断当前节点是否为拐点 |
|---|
| | | if (Boolean.parseBoolean(result.get("result").toString())) { |
|---|
| | | //当前为拐点 |
|---|
| | | res_node.setIsInflectionPoint(true); |
|---|
| | | //拐点方向 |
|---|
| | | res_node.setDirection(result.get("direction").toString()); |
|---|
| | | } |
|---|
| | | list.add(res_node); |
|---|
| | | |
|---|
| | | fatherNode = res_node;//把当前节点保存成一个父节点 |
|---|
| | | res_node = res_node.getFather();//迭代操作 |
|---|
| | | } |
|---|
| | | |
|---|
| | | Collections.reverse(list); |
|---|
| | | |
|---|
| | | //将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多) |
|---|
| | | for (NavigateNode navigateNode : list) { |
|---|
| | | //父节点设置为null，不影响计算结果，不影响后续操作。 |
|---|
| | | //此操作仅为后续排查处理提供视觉方便。 |
|---|
| | | navigateNode.setFather(null); |
|---|
| | | } |
|---|
| | | |
|---|
| | | //起始节点计算方向 |
|---|
| | | String direction = calcDirection(list.get(0), list.get(1)); |
|---|
| | | NavigateNode startNode = list.get(0); |
|---|
| | | startNode.setDirection(direction); |
|---|
| | | //更新节点列表 |
|---|
| | | list.set(0, startNode); |
|---|
| | | return list; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | //判断当前节点到下一个节点是否为拐点 |
|---|
| | | public static HashMap<String,Object> searchInflectionPoint(NavigateNode currentNode, NavigateNode fatherNode, NavigateNode nextNode) { |
|---|
| | | HashMap<String, Object> map = new HashMap<>(); |
|---|
| | | map.put("result", false);//是否为拐点，true：拐点，false：直线 |
|---|
| | | // 第一个点或直线点 |
|---|
| | | if (fatherNode == null || nextNode == null || nextNode.getX() == fatherNode.getX() || nextNode.getY() == fatherNode.getY()) { |
|---|
| | | return map;//不是拐点直接返回 |
|---|
| | | throw new CoolException("未找到该路径"); |
|---|
| | | } |
|---|
| | | |
|---|
| | | //拐点方向 |
|---|
| | | String direction = calcDirection(currentNode, fatherNode); |
|---|
| | | ArrayList<NavigateNode> list = new ArrayList<>(); |
|---|
| | | // 使用 visited 集合防止父链出现环导致死循环，同时设置安全步数上限 |
|---|
| | | HashSet<NavigateNode> visited = new HashSet<>(); |
|---|
| | | int maxSteps = stationMap.size() * stationMap.get(0).size() + 5; // 安全上限 |
|---|
| | | int steps = 0; |
|---|
| | | while (res_node != null && visited.add(res_node) && steps++ < maxSteps) { |
|---|
| | | list.add(res_node); |
|---|
| | | res_node = res_node.getFather();//迭代操作 |
|---|
| | | } |
|---|
| | | if (steps >= maxSteps) { |
|---|
| | | throw new CoolException("路径回溯超出安全上限，疑似存在父链循环"); |
|---|
| | | } |
|---|
| | | Collections.reverse(list); |
|---|
| | | //将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多) |
|---|
| | | for (NavigateNode navigateNode : list) { |
|---|
| | | //父节点设置为null，不影响计算结果，不影响后续操作。 |
|---|
| | | //此操作仅为后续排查处理提供视觉方便。 |
|---|
| | | navigateNode.setFather(null); |
|---|
| | | } |
|---|
| | | |
|---|
| | | map.put("result", true);//拐点 |
|---|
| | | map.put("direction", direction);//拐点方向（从当前节点视角看的方向） |
|---|
| | | return map; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 计算方向 |
|---|
| | | */ |
|---|
| | | public static String calcDirection(NavigateNode currentNode, NavigateNode fatherNode) { |
|---|
| | | //拐点方向 |
|---|
| | | String direction = ""; |
|---|
| | | // 普通拐点 |
|---|
| | | //计算拐点方向 |
|---|
| | | if (fatherNode.getX() != currentNode.getX()) { |
|---|
| | | //x轴数据有差异，判断x轴方向 |
|---|
| | | //当前节点X - 父节点X |
|---|
| | | if (currentNode.getX() - fatherNode.getX() > 0) { |
|---|
| | | //大于0，方向top |
|---|
| | | direction = "top"; |
|---|
| | | }else { |
|---|
| | | //小于0，方向bottom |
|---|
| | | direction = "bottom"; |
|---|
| | | //去重 |
|---|
| | | HashSet<Integer> set = new HashSet<>(); |
|---|
| | | List<NavigateNode> fitlerList = new ArrayList<>(); |
|---|
| | | for(NavigateNode navigateNode : list){ |
|---|
| | | JSONObject valuObject = JSON.parseObject(navigateNode.getNodeValue()); |
|---|
| | | if(set.add(valuObject.getInteger("stationId"))){ |
|---|
| | | fitlerList.add(navigateNode); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | if (fatherNode.getY() != currentNode.getY()) { |
|---|
| | | //y轴数据有差异，判断y轴方向 |
|---|
| | | //当前节点Y - 父节点Y |
|---|
| | | if (currentNode.getY() - fatherNode.getY() > 0) { |
|---|
| | | //大于0，方向left |
|---|
| | | direction = "left"; |
|---|
| | | }else { |
|---|
| | | //小于0，方向right |
|---|
| | | direction = "right"; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | return direction; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 获取分段路径，每当有一个拐点则进行一次分段，最终返回总分段数据 |
|---|
| | | */ |
|---|
| | | public static ArrayList<ArrayList<NavigateNode>> getSectionPath(List<NavigateNode> mapList) { |
|---|
| | | ArrayList<ArrayList<NavigateNode>> list = new ArrayList<>(); |
|---|
| | | ArrayList<NavigateNode> data = new ArrayList<>(); |
|---|
| | | String direction = mapList.get(0).getDirection();//行走方向 |
|---|
| | | |
|---|
| | | for (int i = 0; i < mapList.size(); i++) { |
|---|
| | | NavigateNode mapNode = mapList.get(i); |
|---|
| | | boolean isInflectionPoint = mapNode.getIsInflectionPoint(); |
|---|
| | | data.add(mapNode); |
|---|
| | | if (isInflectionPoint) { |
|---|
| | | //拐点 |
|---|
| | | //分割数据 |
|---|
| | | list.add(data);//添加某一段数据 |
|---|
| | | direction = mapNode.getDirection();//更新行走方向 |
|---|
| | | data = new ArrayList<>(); |
|---|
| | | data.add(mapNode);//将拐点的终点，更新成下一段命令的起点坐标 |
|---|
| | | }else { |
|---|
| | | //直行线路 |
|---|
| | | mapNode.setDirection(direction);//设置行走方向 |
|---|
| | | } |
|---|
| | | Integer distance = getXToNextDistance(mapNode);//获取当前点到下一点的行走距离 |
|---|
| | | mapNode.setMoveDistance(distance); |
|---|
| | | } |
|---|
| | | |
|---|
| | | //将最后一段数据添加进入 |
|---|
| | | list.add(data); |
|---|
| | | |
|---|
| | | return list; |
|---|
| | | } |
|---|
| | | |
|---|
| | | //获取从x点到下一点的行走距离 |
|---|
| | | public static Integer getXToNextDistance(NavigateNode xNode) { |
|---|
| | | NavigateMapData mapData = new NavigateMapData(); |
|---|
| | | ArrayList<ArrayList<JSONObject>> lists = mapData.getJsonData(1); |
|---|
| | | if (lists != null) { |
|---|
| | | JSONObject jsonObject = lists.get(xNode.getX()).get(xNode.getY()); |
|---|
| | | if (jsonObject != null) { |
|---|
| | | return Integer.parseInt(jsonObject.getOrDefault(xNode.getDirection(), 0).toString()); |
|---|
| | | } |
|---|
| | | return 0; |
|---|
| | | } |
|---|
| | | return 0; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 获取当前路径总行走距离 |
|---|
| | | */ |
|---|
| | | public static Integer getCurrentPathAllDistance(List<NavigateNode> path) { |
|---|
| | | if (path.size() == 1) { |
|---|
| | | //路径只有一条数据，则直接返回行走距离 |
|---|
| | | return path.get(0).getMoveDistance(); |
|---|
| | | } |
|---|
| | | |
|---|
| | | //总距离 |
|---|
| | | int allDistance = 0; |
|---|
| | | for (int i = 0; i < path.size() - 1; i++) {//路径中最后一个节点不统计到总距离，最后一个节点并不会再行走 |
|---|
| | | allDistance += path.get(i).getMoveDistance(); |
|---|
| | | } |
|---|
| | | return allDistance; |
|---|
| | | } |
|---|
| | | |
|---|
| | | public static void main(String[] args) { |
|---|
| | | //计算路径 |
|---|
| | | List<NavigateNode> calc = calc("1000901", "1800201", ShuttleTaskModeType.PAK_OUT.id); |
|---|
| | | System.out.println(calc); |
|---|
| | | System.out.println("------------------------"); |
|---|
| | | // List<NavigateNode> calc = calc("0501401", "0201801", "out"); |
|---|
| | | //将路径分割成多段 |
|---|
| | | ArrayList<ArrayList<NavigateNode>> data = getSectionPath(calc); |
|---|
| | | for (ArrayList<NavigateNode> list : data) { |
|---|
| | | Integer currentPathAllDistance = getCurrentPathAllDistance(list);//计算当前路径总距离 |
|---|
| | | System.out.println(currentPathAllDistance); |
|---|
| | | System.out.println(list); |
|---|
| | | } |
|---|
| | | |
|---|
| | | return fitlerList; |
|---|
| | | } |
|---|
| | | |
|---|
| | | } |
|---|
