package com.zy.asrs.wcs.core.utils;
|
|
import com.zy.asrs.framework.common.SpringUtils;
|
import com.zy.asrs.wcs.core.model.MapNode;
|
import com.zy.asrs.wcs.core.model.NavigateNode;
|
import com.zy.asrs.wcs.core.model.enums.MapNodeType;
|
import com.zy.asrs.wcs.core.model.enums.NavigationMapType;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.HashMap;
|
import java.util.List;
|
|
/**
|
* A*算法使用工具
|
*/
|
public class NavigateUtils {
|
|
public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints) {
|
//通过开始编号和结束编号获取对应的xy轴坐标
|
int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
|
int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点
|
|
ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
|
whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
|
|
//获取当前节点计算的层高，并赋值到每一个节点中
|
int lev = Utils.getLev(startPoint);
|
|
//初始化开始节点
|
NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
|
//开始节点无父节点
|
start.setFather(null);
|
|
NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
|
NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
|
//开始节点，不纳入禁用节点内计算
|
|
NavigateNode res_node = solution.astarSearch(start, end);
|
if (res_node == null) {
|
System.err.println("未找到路径");
|
return null;
|
}
|
ArrayList<NavigateNode> list = new ArrayList<>();
|
|
//渲染
|
NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
|
while (res_node != null) {
|
res_node.setDirection(null);
|
res_node.setIsInflectionPoint(false);
|
res_node.setZ(lev);//设置层高
|
|
//寻找拐点
|
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;//不是拐点直接返回
|
}
|
|
//拐点方向
|
String direction = calcDirection(currentNode, fatherNode);
|
|
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";
|
}
|
}
|
|
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 (NavigateNode navigateNode : mapList) {
|
data.add(navigateNode);
|
//拐点
|
if (navigateNode.getIsInflectionPoint()) {
|
//分割数据
|
list.add(data);//添加某一段数据
|
direction = navigateNode.getDirection();//更新行走方向
|
data = new ArrayList<>();
|
data.add(navigateNode);//将拐点的终点，更新成下一段命令的起点坐标
|
} else {
|
//直行线路
|
navigateNode.setDirection(direction);//设置行走方向
|
}
|
Integer distance = getXToNextDistance(navigateNode);//获取当前点到下一点的行走距离
|
navigateNode.setMoveDistance(distance);
|
}
|
|
//将最后一段数据添加进入
|
list.add(data);
|
|
return list;
|
}
|
|
//获取从x点到下一点的行走距离
|
public static Integer getXToNextDistance(NavigateNode xNode) {
|
NavigateMapData mapData = SpringUtils.getBean(NavigateMapData.class);
|
mapData.setLev(xNode.getZ());
|
List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null, null);
|
if (lists != null) {
|
MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
|
if (mapNode != null) {
|
switch (xNode.getDirection()) {
|
case "top":
|
return mapNode.getTop();
|
case "bottom":
|
return mapNode.getBottom();
|
case "left":
|
return mapNode.getLeft();
|
case "right":
|
return mapNode.getRight();
|
}
|
}
|
return 0;
|
}
|
return 0;
|
}
|
|
/**
|
* 根据原始节点结果，计算总行走距离
|
*/
|
public static Integer getOriginPathAllDistance(List<NavigateNode> path) {
|
ArrayList<ArrayList<NavigateNode>> sectionPath = NavigateUtils.getSectionPath(path);
|
Integer allDistance = 0;
|
for (ArrayList<NavigateNode> navigateNodes : sectionPath) {
|
Integer distance = NavigateUtils.getCurrentPathAllDistance(navigateNodes);
|
allDistance += distance;
|
}
|
return allDistance;
|
}
|
|
/**
|
* 获取当前路径总行走距离
|
*/
|
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 Integer getMiddleToDistDistance(List<NavigateNode> path, NavigateNode middlePath) {
|
//最后一条节点不计算进行走距离
|
NavigateNode lastPath = path.get(path.size() - 1);
|
//总距离
|
int allDistance = 0;
|
boolean flag = false;
|
for (NavigateNode navigateNode : path) {
|
if (!flag && navigateNode.equals(middlePath)) {
|
flag = true;
|
}
|
|
if (navigateNode.equals(lastPath)) {
|
continue;//最后一条节点不计算进行走距离
|
}
|
|
if (flag) {
|
allDistance += navigateNode.getMoveDistance();
|
}
|
}
|
return allDistance;
|
}
|
|
/**
|
* 检测路径是否可用(可走)
|
*/
|
public static boolean checkPathIsAvailable(List<NavigateNode> path, Integer shuttleNo, Integer lev) {
|
NavigateSolution solution = new NavigateSolution(NavigationMapType.DFX.id, lev, null, Utils.getShuttlePoints(shuttleNo, lev));//获取无白名单地图(该地图包含小车坐标)
|
int[][] map = solution.map;
|
for (NavigateNode node : path) {
|
int value = map[node.getX()][node.getY()];
|
if (value != MapNodeType.NORMAL_PATH.id && value != MapNodeType.MAIN_PATH.id && value != MapNodeType.CHARGE.id && value != MapNodeType.CONVEYOR_CAR_GO.id) {//母轨道3、子轨道0、充电桩5、小车可走输送站
|
return false;
|
}
|
}
|
return true;
|
}
|
|
public static void main(String[] args) {
|
//计算路径
|
List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id, null);
|
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);
|
}
|
|
}
|
|
}
|
