From 0836f55508cc79dfa3554a67058955ebf4830f62 Mon Sep 17 00:00:00 2001
From: DELL <DELL@qq.com>
Date: 星期四, 27 十一月 2025 15:11:20 +0800
Subject: [PATCH] #
---
src/main/java/com/zy/common/utils/NavigateSolution.java | 278 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 278 insertions(+), 0 deletions(-)
diff --git a/src/main/java/com/zy/common/utils/NavigateSolution.java b/src/main/java/com/zy/common/utils/NavigateSolution.java
new file mode 100644
index 0000000..e6cb942
--- /dev/null
+++ b/src/main/java/com/zy/common/utils/NavigateSolution.java
@@ -0,0 +1,278 @@
+package com.zy.common.utils;
+
+import com.alibaba.fastjson.JSON;
+import com.alibaba.fastjson.JSONObject;
+import com.baomidou.mybatisplus.mapper.EntityWrapper;
+import com.core.common.SpringUtils;
+import com.core.exception.CoolException;
+import com.zy.asrs.entity.BasMap;
+import com.zy.asrs.service.BasMapService;
+import com.zy.common.model.NavigateNode;
+import com.zy.core.enums.MapNodeType;
+import java.util.*;
+
+/**
+ * A*绠楁硶瀹炵幇绫�
+ */
+public class NavigateSolution {
+
+ // Open琛ㄧ敤浼樺厛闃熷垪
+ public PriorityQueue<NavigateNode> Open = new PriorityQueue<NavigateNode>();
+ //Close琛ㄧ敤鏅�氱殑鏁扮粍
+ public ArrayList<NavigateNode> Close = new ArrayList<NavigateNode>();
+ //鐢ㄦ潵瀛樻斁宸茬粡鍑虹幇杩囩殑缁撶偣銆�
+ Map<String, Integer> bestGMap = new HashMap<>();
+
+ public List<List<NavigateNode>> getStationMap(int lev) {
+ BasMapService basMapService = SpringUtils.getBean(BasMapService.class);
+ BasMap basMap = basMapService.selectOne(new EntityWrapper<BasMap>().eq("lev", lev));
+ if (basMap == null) {
+ throw new CoolException("鍦板浘涓嶅瓨鍦�");
+ }
+
+ List<List<JSONObject>> dataList = JSON.parseObject(basMap.getData(), List.class);
+
+ List<List<NavigateNode>> navigateNodeList = new ArrayList<>();
+ for (int i = 0; i < dataList.size(); i++) {
+ List<JSONObject> row = dataList.get(i);
+ List<NavigateNode> navigateNodeRow = new ArrayList<>();
+ for (int j = 0; j < row.size(); j++) {
+ JSONObject map = row.get(j);
+ NavigateNode navigateNode = new NavigateNode(i, j);
+
+ String nodeType = map.getString("type");
+ if(nodeType == null) {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }else if(nodeType.equals("devp") || nodeType.equals("merge")){
+ navigateNode.setValue(MapNodeType.NORMAL_PATH.id);
+
+ JSONObject valueObj = JSON.parseObject(map.getString("value"));
+ List<String> directionList = valueObj.getJSONArray("direction").toJavaList(String.class);
+ navigateNode.setDirectionList(directionList);
+ }else {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }
+
+ navigateNode.setNodeType(nodeType);
+ navigateNode.setNodeValue(map.getString("value"));
+ navigateNodeRow.add(navigateNode);
+ }
+ navigateNodeList.add(navigateNodeRow);
+ }
+
+ return navigateNodeList;
+ }
+
+ public List<List<NavigateNode>> getRgvTrackMap(int lev) {
+ BasMapService basMapService = SpringUtils.getBean(BasMapService.class);
+ BasMap basMap = basMapService.selectOne(new EntityWrapper<BasMap>().eq("lev", lev));
+ if (basMap == null) {
+ throw new CoolException("鍦板浘涓嶅瓨鍦�");
+ }
+
+ List<List<JSONObject>> dataList = JSON.parseObject(basMap.getData(), List.class);
+
+ List<List<NavigateNode>> navigateNodeList = new ArrayList<>();
+ for (int i = 0; i < dataList.size(); i++) {
+ List<JSONObject> row = dataList.get(i);
+ List<NavigateNode> navigateNodeRow = new ArrayList<>();
+ for (int j = 0; j < row.size(); j++) {
+ JSONObject map = row.get(j);
+ NavigateNode navigateNode = new NavigateNode(i, j);
+
+ String nodeType = map.getString("type");
+ if(nodeType == null) {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }else if(nodeType.equals("rgv")){
+ navigateNode.setValue(MapNodeType.NORMAL_PATH.id);
+ JSONObject valueObj = JSON.parseObject(map.getString("value"));
+
+ //RGV鏆備笉鎺у埗琛岃蛋鏂瑰悜锛岄粯璁や笂涓嬪乏鍙抽兘鍙蛋
+ List<String> directionList = new ArrayList<>();
+ directionList.add("top");
+ directionList.add("bottom");
+ directionList.add("left");
+ directionList.add("right");
+ navigateNode.setDirectionList(directionList);
+ }else {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }
+
+ navigateNode.setNodeType(nodeType);
+ navigateNode.setNodeValue(map.getString("value"));
+ navigateNodeRow.add(navigateNode);
+ }
+ navigateNodeList.add(navigateNodeRow);
+ }
+
+ return navigateNodeList;
+ }
+
+ public NavigateNode astarSearchJava(List<List<NavigateNode>> map, NavigateNode start, NavigateNode end) {
+ // 娓呯悊涓婃鎼滅储鐨勭姸鎬侊紝闃叉澶嶇敤瀹炰緥瀵艰嚧璁板綍娈嬬暀
+ this.Open.clear();
+ this.Close.clear();
+ this.bestGMap.clear();
+ //鎶婄涓�涓紑濮嬬殑缁撶偣鍔犲叆鍒癘pen琛ㄤ腑
+ this.Open.add(start);
+ //涓诲惊鐜�
+ while (Open.size() > 0) {
+ //鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎
+ NavigateNode current_node = Open.poll();
+
+ if (current_node.getX() == end.getX() && current_node.getY() == end.getY()) {//鎵惧埌鐩爣缁撶偣灏辫繑鍥�
+ return current_node;
+ }
+
+ //灏嗚繖涓粨鐐瑰姞鍏ュ埌Close琛ㄤ腑
+ Close.add(current_node);
+ //瀵瑰綋鍓嶇粨鐐硅繘琛屾墿灞曪紝寰楀埌涓�涓洓鍛ㄧ粨鐐圭殑鏁扮粍
+ ArrayList<NavigateNode> neighbour_node = extend_current_node(map, current_node);
+ //瀵硅繖涓粨鐐归亶鍘嗭紝鐪嬫槸鍚︽湁鐩爣缁撶偣鍑虹幇
+ for (NavigateNode node : neighbour_node) {
+ // 宸插湪鍏抽棴鍒楄〃涓殑鑺傜偣涓嶅啀澶勭悊锛岄伩鍏嶇埗閾惧弽澶嶆敼鍐欏舰鎴愮幆
+ if (Close.contains(node)) {
+ continue;
+ }
+ // G + H + E (瀵瑰惎鍙戝嚱鏁板鍔犲幓鎷愮偣鏂规calcNodeExtraCost)
+ int gCost = calcNodeExtraCost(current_node, node, end);
+
+ //杩涜璁＄畻瀵笹, F, H 绛夊��
+ node.setG(1 + gCost);
+ node.init_node(current_node, end);
+ node.setH(calcNodeCost(node, end));
+ node.setF(node.getG() + node.getH());
+
+ String key = node.getX() + "_" + node.getY();
+ Integer recordedG = bestGMap.get(key);
+ // 浠呭綋鎵惧埌鏇村皬鐨勪唬浠锋椂鎵嶆洿鏂颁笌鍏pen锛岄伩鍏嶇瓑浠蜂唬浠峰弽澶嶅叆闃熷鑷寸埗閾炬姈鍔�
+ if (recordedG == null || node.getG() < recordedG) {
+ bestGMap.put(key, node.getG());
+
+ Open.add(node);
+ }
+ }
+ }
+ //濡傛灉閬嶅巻瀹屾墍鏈夊嚭鐜扮殑缁撶偣閮芥病鏈夋壘鍒版渶缁堢殑缁撶偣锛岃繑鍥瀗ull
+ return null;
+ }
+
+ public ArrayList<NavigateNode> extend_current_node(List<List<NavigateNode>> map, NavigateNode current_node) {
+ //鑾峰彇褰撳墠缁撶偣鐨剎, y
+ int x = current_node.getX();
+ int y = current_node.getY();
+ //濡傛灉褰撳墠缁撶偣鐨勯偦缁撶偣鍚堟硶锛屽氨鍔犲叆鍒皀eighbour_node
+ ArrayList<NavigateNode> neighbour_node = new ArrayList<NavigateNode>();
+
+ if(current_node.getValue() == MapNodeType.DISABLE.id) {
+ return neighbour_node;
+ }
+
+ List<String> directionList = current_node.getDirectionList();
+ if(directionList == null || directionList.size() == 0) {
+ return neighbour_node;
+ }
+
+ for(String direction : directionList) {
+ if(direction.equals("top")) {
+ NavigateNode node = getValidNavigateNode(map, x - 1, y);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }else if(direction.equals("bottom")) {
+ NavigateNode node = getValidNavigateNode(map, x + 1, y);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }else if(direction.equals("left")) {
+ NavigateNode node = getValidNavigateNode(map, x, y - 1);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }else if(direction.equals("right")) {
+ NavigateNode node = getValidNavigateNode(map, x, y + 1);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }
+ }
+
+ return neighbour_node;
+ }
+
+ public NavigateNode getValidNavigateNode(List<List<NavigateNode>> map, int x, int y) {
+ if (x < 0 || x >= map.size()
+ || y < 0 || y >= map.get(0).size()) {
+ return null;
+ }
+
+ NavigateNode node = map.get(x).get(y);
+ if(node.getValue() == MapNodeType.DISABLE.id) {
+ return null;
+ }
+
+ return node;
+ }
+
+ public NavigateNode findStationNavigateNode(List<List<NavigateNode>> map, int stationId) {
+ for(int x = 0; x < map.size(); x++) {
+ for(int y = 0; y < map.get(0).size(); y++) {
+ NavigateNode node = map.get(x).get(y);
+ if("devp".equals(node.getNodeType())) {
+ JSONObject valueObj = JSON.parseObject(node.getNodeValue());
+ if(valueObj.getInteger("stationId") == stationId) {
+ return node;
+ }
+ }
+ }
+ }
+ return null;
+ }
+
+ public NavigateNode findTrackSiteNoNavigateNode(List<List<NavigateNode>> map, int trackSiteNo) {
+ for(int x = 0; x < map.size(); x++) {
+ for(int y = 0; y < map.get(0).size(); y++) {
+ NavigateNode node = map.get(x).get(y);
+ if("rgv".equals(node.getNodeType())) {
+ JSONObject valueObj = JSON.parseObject(node.getNodeValue());
+ if(valueObj.getInteger("trackSiteNo") == trackSiteNo) {
+ return node;
+ }
+ }
+ }
+ }
+ return null;
+ }
+
+ //------------------A*鍚彂鍑芥暟------------------//
+
+ //璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻H鍊�(鏇煎搱椤挎硶锛氬潗鏍囧垎鍒彇宸�肩浉鍔�)
+ private int calcNodeCost(NavigateNode node1, NavigateNode node2) {
+ return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
+ }
+
+ //鍘婚櫎鎷愮偣绠楁硶锛岀粰鐩寸嚎澧炲姞浼樺厛绾�
+ private int calcNodeExtraCost(NavigateNode currNode, NavigateNode nextNode, NavigateNode endNode) {
+ // 绗竴涓偣鎴栫洿绾跨偣
+ if (currNode.getFather() == null || nextNode.getX() == currNode.getFather().getX()
+ || nextNode.getY() == currNode.getFather().getY()) {
+ return 0;
+ }
+
+ // 鎷愬悜缁堢偣鐨勭偣
+ if (nextNode.getX() == endNode.getX() || nextNode.getY() == endNode.getY()) {
+ return 1;
+ }
+
+ // 鏅�氭嫄鐐�
+ /*
+ 鎷愮偣鍒ゆ柇閫昏緫
+ 鎷垮埌鐖惰妭鐐瑰拰涓嬩竴鑺傜偣
+ 閫氳繃鍒ゆ柇鐖惰妭鐐瑰拰涓嬩竴鑺傜偣鐨剎鏁版嵁鍜寉鏁版嵁閮戒笉鐩稿悓鏃讹紝鍒欒〃鏄庡綋鍓嶅潗鏍囨槸涓�涓嫄鐐�
+ */
+ return 1;
+ }
+
+ //------------------A*鍚彂鍑芥暟-end------------------//
+
+}
--
Gitblit v1.9.1