|
@@ -0,0 +1,440 @@
|
|
|
+package com.fdkankan.indoor.base.convert;
|
|
|
+
|
|
|
+import java.util.List;
|
|
|
+
|
|
|
+import com.fdkankan.indoor.base.convert.kesar.AStar;
|
|
|
+import com.fdkankan.indoor.base.convert.kesar.Coord;
|
|
|
+import com.fdkankan.indoor.base.convert.kesar.MapInfo;
|
|
|
+import com.fdkankan.indoor.base.convert.kesar.Node;
|
|
|
+
|
|
|
+
|
|
|
+import com.fdkankan.indoor.core.entity.dto.RouteInputDto;
|
|
|
+import net.sf.json.JSONArray;
|
|
|
+import net.sf.json.JSONObject;
|
|
|
+
|
|
|
+/**
|
|
|
+ * star算法
|
|
|
+ * 最优路径算法
|
|
|
+ */
|
|
|
+public class GetRoute {
|
|
|
+
|
|
|
+ // public static String inputFilePath = "F:\\2021\\navvis\\���Ի���\\test3(v2.7.3)\\routeMap.txt";
|
|
|
+ public static String inputFilePath = "F:\\test\\project\\age_laser\\routeMap.txt";
|
|
|
+
|
|
|
+ // 起始点
|
|
|
+ private static float startX = 1.745f;
|
|
|
+ private static float startY = -14.45f;
|
|
|
+ private static float startZ = -0.042078f;
|
|
|
+
|
|
|
+ // 终点
|
|
|
+ private static float endX = 3.74493f;
|
|
|
+ private static float endY = -14.4489f;
|
|
|
+ private static float endZ = -0.042078f;
|
|
|
+
|
|
|
+ private static int minStartId = -1;
|
|
|
+ private static int minEndId = -1;
|
|
|
+ private static Node start = null;
|
|
|
+ private static Node end = null;
|
|
|
+ private static AStar g_AStar = new AStar();
|
|
|
+
|
|
|
+ //"id" "x" ,"y " ,"z" ,"weight" ,"p1",...,"p8"
|
|
|
+ // lat:113, [0], x, lon:22, [1], y
|
|
|
+
|
|
|
+ /**
|
|
|
+ *
|
|
|
+ * @param list 算法部 生成的文件
|
|
|
+ * @param dto
|
|
|
+ * @return
|
|
|
+ * @throws Exception
|
|
|
+ */
|
|
|
+ private static JSONArray init(List<String> list, RouteInputDto dto) throws Exception {
|
|
|
+// List<String> list = FileUtil.readFileByLines2(inputFilePath);
|
|
|
+ JSONArray maps = new JSONArray();
|
|
|
+
|
|
|
+ float startX = dto.getSource_longitude();
|
|
|
+ float startY = dto.getSource_latitude();
|
|
|
+ float startZ = dto.getSource_z();
|
|
|
+
|
|
|
+ float endX = dto.getDestination_longitude();
|
|
|
+ float endY = dto.getDestination_latitude();
|
|
|
+ float endZ = dto.getDestination_z();
|
|
|
+
|
|
|
+ Coord _start = new Coord(startX,startY,startZ);
|
|
|
+ Coord _end = new Coord(endX,endY,endZ);
|
|
|
+
|
|
|
+ float startDistance=1000f;
|
|
|
+ float endDistance = 1000f;
|
|
|
+
|
|
|
+ for(int i=0;i<list.size();++i) {
|
|
|
+ String str = list.get(i);
|
|
|
+ String[] strArray = str.trim().split(" ");
|
|
|
+ JSONObject item = new JSONObject();
|
|
|
+ //item.put("id", i);
|
|
|
+ item.put("x", strArray[0]);
|
|
|
+ item.put("y", strArray[1]);
|
|
|
+ item.put("z", strArray[2]);
|
|
|
+ item.put("weight", strArray[3]);
|
|
|
+
|
|
|
+ String linkedIds = "";
|
|
|
+ for(int j = 4;j<strArray.length;++j) {
|
|
|
+ if(Integer.valueOf(strArray[j])>0) {
|
|
|
+ linkedIds += strArray[j]+",";
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ item.put("linkedIds", linkedIds.substring(0, linkedIds.length()-1));
|
|
|
+ maps.add(item);
|
|
|
+ Coord coord = new Coord(Float.valueOf(strArray[0]),Float.valueOf(strArray[1]),Float.valueOf(strArray[2]));
|
|
|
+ float _startDistance = g_AStar.calcH(_start, coord);
|
|
|
+ if(_startDistance<startDistance) {
|
|
|
+ minStartId = i;
|
|
|
+ startDistance = _startDistance;
|
|
|
+ }
|
|
|
+ float _endDistance = g_AStar.calcH(_end, coord);
|
|
|
+ if(_endDistance<endDistance) {
|
|
|
+ minEndId = i;
|
|
|
+ endDistance = _endDistance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ JSONObject virtualStart = maps.getJSONObject(minStartId);
|
|
|
+ JSONObject virtualEnd = maps.getJSONObject(minEndId);
|
|
|
+ Coord startVirtualCoord = new Coord((float)virtualStart.getDouble("x"), (float)virtualStart.getDouble("y"),(float)virtualStart.getDouble("z"));
|
|
|
+ Coord endVirtualCoord = new Coord((float)virtualEnd.getDouble("x"), (float)virtualEnd.getDouble("y"),(float)virtualEnd.getDouble("z"));
|
|
|
+ float startH = g_AStar.calcH(startVirtualCoord, endVirtualCoord);
|
|
|
+
|
|
|
+ start = new Node(minStartId,0,startVirtualCoord, null, 0, startH);
|
|
|
+ end = new Node(minEndId,0,endVirtualCoord, null, 0, 0);
|
|
|
+
|
|
|
+ return maps;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private static JSONArray init() throws Exception {
|
|
|
+ List<String> list = FileUtil.readFileByLines2(inputFilePath);
|
|
|
+ JSONArray maps = new JSONArray();
|
|
|
+
|
|
|
+// float startX = dto.getSource_longitude();
|
|
|
+// float startY = dto.getSource_latitude();
|
|
|
+// float startZ = dto.getSource_z();
|
|
|
+//
|
|
|
+// float endX = dto.getDestination_longitude();
|
|
|
+// float endY = dto.getDestination_latitude();
|
|
|
+// float endZ = dto.getDestination_z();
|
|
|
+
|
|
|
+ Coord _start = new Coord(startX,startY,startZ);
|
|
|
+ Coord _end = new Coord(endX,endY,endZ);
|
|
|
+
|
|
|
+ float startDistance=1000f;
|
|
|
+ float endDistance = 1000f;
|
|
|
+
|
|
|
+ for(int i=0;i<list.size();++i) {
|
|
|
+ String str = list.get(i);
|
|
|
+ String[] strArray = str.trim().split(" ");
|
|
|
+ JSONObject item = new JSONObject();
|
|
|
+ //item.put("id", i);
|
|
|
+ item.put("x", strArray[0]);
|
|
|
+ item.put("y", strArray[1]);
|
|
|
+ item.put("z", strArray[2]);
|
|
|
+ item.put("weight", strArray[3]);
|
|
|
+
|
|
|
+ String linkedIds = "";
|
|
|
+ for(int j = 4;j<strArray.length;++j) {
|
|
|
+ if(Integer.valueOf(strArray[j])>0) {
|
|
|
+ linkedIds += strArray[j]+",";
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ item.put("linkedIds", linkedIds.substring(0, linkedIds.length()-1));
|
|
|
+ maps.add(item);
|
|
|
+ Coord coord = new Coord(Float.valueOf(strArray[0]),Float.valueOf(strArray[1]),Float.valueOf(strArray[2]));
|
|
|
+ float _startDistance = g_AStar.calcH(_start, coord);
|
|
|
+ if(_startDistance<startDistance) {
|
|
|
+ minStartId = i;
|
|
|
+ startDistance = _startDistance;
|
|
|
+ }
|
|
|
+ float _endDistance = g_AStar.calcH(_end, coord);
|
|
|
+ if(_endDistance<endDistance) {
|
|
|
+ minEndId = i;
|
|
|
+ endDistance = _endDistance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ JSONObject virtualStart = maps.getJSONObject(minStartId);
|
|
|
+ JSONObject virtualEnd = maps.getJSONObject(minEndId);
|
|
|
+ Coord startVirtualCoord = new Coord((float)virtualStart.getDouble("x"), (float)virtualStart.getDouble("y"),(float)virtualStart.getDouble("z"));
|
|
|
+ Coord endVirtualCoord = new Coord((float)virtualEnd.getDouble("x"), (float)virtualEnd.getDouble("y"),(float)virtualEnd.getDouble("z"));
|
|
|
+ float startH = g_AStar.calcH(startVirtualCoord, endVirtualCoord);
|
|
|
+
|
|
|
+ start = new Node(minStartId,0,startVirtualCoord, null, 0, startH);
|
|
|
+ end = new Node(minEndId,0,endVirtualCoord, null, 0, 0);
|
|
|
+
|
|
|
+ return maps;
|
|
|
+ }
|
|
|
+
|
|
|
+ private static JSONArray convertFromPath(List<Node> path) {
|
|
|
+ if(path == null||path.size() == 0) {
|
|
|
+ return null;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ JSONArray route = new JSONArray();
|
|
|
+
|
|
|
+ //起点不在path上,path的第一个点对应的是格子
|
|
|
+ JSONObject start = new JSONObject();
|
|
|
+ double[] startPosition = {startX,startY};
|
|
|
+ startPosition = TransformGPS.convert(startPosition);
|
|
|
+ start.put("longitude", startPosition[0]);
|
|
|
+ start.put("latitude", startPosition[1]);
|
|
|
+ start.put("z", startZ);
|
|
|
+ float[] location = new float[3];
|
|
|
+ location[0] = (float)startPosition[0];
|
|
|
+ location[1] = (float)startPosition[1];
|
|
|
+ location[2] = startZ;
|
|
|
+
|
|
|
+ start.put("location", location);
|
|
|
+ start.put("distance", 0);
|
|
|
+ start.put("distance_to_previous", 0);
|
|
|
+ start.put("instruction", null);
|
|
|
+ route.add(start);
|
|
|
+
|
|
|
+ float[] virtualendPosition = new float[3];
|
|
|
+
|
|
|
+ for(int i=0;i<path.size();++i) {
|
|
|
+ Node node = path.get(i);
|
|
|
+ JSONObject item = new JSONObject();
|
|
|
+ //转经纬度
|
|
|
+ double[] position = {node.coord.x,node.coord.y};
|
|
|
+ position = TransformGPS.convert(position);
|
|
|
+ item.put("longitude", position[0]);
|
|
|
+ item.put("latitude", position[1]);
|
|
|
+
|
|
|
+ item.put("z", node.coord.z);
|
|
|
+ location = new float[3];
|
|
|
+
|
|
|
+ location[0] = (float)position[0];
|
|
|
+ location[1] = (float)position[1];
|
|
|
+ location[2] = node.coord.z;
|
|
|
+ item.put("location", location);
|
|
|
+
|
|
|
+ JSONObject instruction = null;
|
|
|
+
|
|
|
+ if(i == 0) {
|
|
|
+ item.put("distance", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
|
|
|
+ item.put("distance_to_previous", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
|
|
|
+ item.put("instruction", instruction);
|
|
|
+
|
|
|
+ instruction = new JSONObject();
|
|
|
+ instruction.put("type", "source_projection_to_navgraph");
|
|
|
+ item.put("instruction", instruction);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ int j = i-1;
|
|
|
+ Node prenode = path.get(j);
|
|
|
+ JSONObject preitem = route.getJSONObject(j);
|
|
|
+ float distance_to_previous = g_AStar.calcH(node.coord,prenode.coord);
|
|
|
+ item.put("distance_to_previous", distance_to_previous);
|
|
|
+ float distance = (float)preitem.getDouble("distance")+distance_to_previous;
|
|
|
+ item.put("distance", distance);
|
|
|
+ if(i == path.size()-1) {
|
|
|
+ instruction = new JSONObject();
|
|
|
+ instruction.put("type", "destination_projection_to_navgraph");
|
|
|
+ item.put("instruction", instruction);
|
|
|
+
|
|
|
+ virtualendPosition[0] = (float)node.coord.x;
|
|
|
+ virtualendPosition[1] = (float)node.coord.y;
|
|
|
+ virtualendPosition[2] = (float)node.coord.z;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ item.put("instruction", instruction);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ route.add(item);
|
|
|
+ }
|
|
|
+
|
|
|
+ JSONObject endItem = route.getJSONObject(route.size()-1);
|
|
|
+ JSONObject end = new JSONObject();
|
|
|
+ double[] endPosition = {endX,endY};
|
|
|
+ endPosition = TransformGPS.convert(endPosition);
|
|
|
+ end.put("longitude", endPosition[0]);
|
|
|
+ end.put("latitude", endPosition[1]);
|
|
|
+ end.put("z", endZ);
|
|
|
+ location = new float[3];
|
|
|
+ location[0] = (float)endPosition[0];
|
|
|
+ location[1] = (float)endPosition[1];
|
|
|
+ location[2] = endZ;
|
|
|
+ end.put("location", location);
|
|
|
+ float enddistance = g_AStar.calcH(new Coord(virtualendPosition[0],virtualendPosition[1],virtualendPosition[2]),new Coord(endX,endY,endZ));
|
|
|
+ end.put("distance", enddistance+(float)endItem.getDouble("distance_to_previous"));
|
|
|
+ end.put("distance_to_previous", enddistance);
|
|
|
+ end.put("instruction", null);
|
|
|
+ route.add(end);
|
|
|
+
|
|
|
+ return route;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private static JSONArray convertFromPath(List<Node> path, RouteInputDto dto) {
|
|
|
+ if(path == null||path.size() == 0) {
|
|
|
+ return null;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 起始点
|
|
|
+ float startX = dto.getSource_longitude();
|
|
|
+ float startY = dto.getSource_latitude();
|
|
|
+ float startZ = dto.getSource_z();
|
|
|
+
|
|
|
+ // 终点
|
|
|
+ float endX = dto.getDestination_longitude();
|
|
|
+ float endY = dto.getDestination_latitude();
|
|
|
+ float endZ = dto.getDestination_z();
|
|
|
+
|
|
|
+ JSONArray route = new JSONArray();
|
|
|
+
|
|
|
+ //起点不在path上,path的第一个点对应的是格子
|
|
|
+ JSONObject start = new JSONObject();
|
|
|
+ double[] startPosition = {startX,startY};
|
|
|
+ startPosition = TransformGPS.convert(startPosition);
|
|
|
+ start.put("longitude", startPosition[0]);
|
|
|
+ start.put("latitude", startPosition[1]);
|
|
|
+ start.put("z", startZ);
|
|
|
+ float[] location = new float[3];
|
|
|
+ location[0] = (float)startPosition[0];
|
|
|
+ location[1] = (float)startPosition[1];
|
|
|
+ location[2] = startZ;
|
|
|
+
|
|
|
+ start.put("location", location);
|
|
|
+ start.put("distance", 0);
|
|
|
+ start.put("distance_to_previous", 0);
|
|
|
+ start.put("instruction", null);
|
|
|
+ route.add(start);
|
|
|
+
|
|
|
+ float[] endVirPosition = new float[3];
|
|
|
+ JSONObject endItem = new JSONObject();
|
|
|
+
|
|
|
+ for(int i=0;i<path.size();++i) {
|
|
|
+ Node node = path.get(i);
|
|
|
+ JSONObject item = new JSONObject();
|
|
|
+ //转经纬度
|
|
|
+ double[] position = {node.coord.x,node.coord.y};
|
|
|
+ position = TransformGPS.convert(position);
|
|
|
+ item.put("longitude", position[0]);
|
|
|
+ item.put("latitude", position[1]);
|
|
|
+
|
|
|
+ item.put("z", node.coord.z);
|
|
|
+ location = new float[3];
|
|
|
+
|
|
|
+ location[0] = (float)position[0];
|
|
|
+ location[1] = (float)position[1];
|
|
|
+ location[2] = node.coord.z;
|
|
|
+ item.put("location", location);
|
|
|
+
|
|
|
+ JSONObject instruction = null;
|
|
|
+
|
|
|
+ if(i == 0) {
|
|
|
+ item.put("distance", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
|
|
|
+ item.put("distance_to_previous", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
|
|
|
+ item.put("instruction", instruction);
|
|
|
+
|
|
|
+ instruction = new JSONObject();
|
|
|
+ instruction.put("type", "source_projection_to_navgraph");
|
|
|
+ item.put("instruction", instruction);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ int j = i-1;
|
|
|
+ Node prenode = path.get(j);
|
|
|
+ JSONObject preitem = route.getJSONObject(j);
|
|
|
+ float distance = g_AStar.calcH(node.coord,prenode.coord);
|
|
|
+ item.put("distance", distance);
|
|
|
+ float distance_to_previous = (float)preitem.getDouble("distance_to_previous")+distance;
|
|
|
+ item.put("distance_to_previous", distance_to_previous);
|
|
|
+ if(i == path.size()-1) {
|
|
|
+ instruction = new JSONObject();
|
|
|
+ instruction.put("type", "destination_projection_to_navgraph");
|
|
|
+ item.put("instruction", instruction);
|
|
|
+
|
|
|
+ endVirPosition[0] = node.coord.x;
|
|
|
+ endVirPosition[1] = node.coord.y;
|
|
|
+ endVirPosition[2] = node.coord.z;
|
|
|
+ endItem.put("distance_to_previous",distance_to_previous);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ item.put("instruction", instruction);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ route.add(item);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ JSONObject end = new JSONObject();
|
|
|
+ double[] endPosition = {endX,endY};
|
|
|
+ endPosition = TransformGPS.convert(endPosition);
|
|
|
+ end.put("longitude", endPosition[0]);
|
|
|
+ end.put("latitude", endPosition[1]);
|
|
|
+ end.put("z", endZ);
|
|
|
+ location = new float[3];
|
|
|
+ location[0] = (float)endPosition[0];
|
|
|
+ location[1] = (float)endPosition[1];
|
|
|
+ location[2] = endZ;
|
|
|
+ end.put("location", location);
|
|
|
+ float enddistance = g_AStar.calcH(new Coord(endVirPosition[0],endVirPosition[1],endVirPosition[2]),new Coord(endX,endY,endZ));
|
|
|
+ end.put("distance", enddistance+(float)endItem.getDouble("distance_to_previous"));
|
|
|
+ end.put("distance_to_previous", enddistance);
|
|
|
+ end.put("instruction", null);
|
|
|
+ route.add(end);
|
|
|
+
|
|
|
+ return route;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * 读取文件,获取文件内容
|
|
|
+ * @param inPath F:\test\project\age_laser\routeMap.txt
|
|
|
+ * @return
|
|
|
+ */
|
|
|
+// public static JSONArray createRoute(String inPath){
|
|
|
+// try {
|
|
|
+// JSONArray maps = readMap(inPath);
|
|
|
+// return maps;
|
|
|
+// } catch (Exception e) {
|
|
|
+// e.printStackTrace();
|
|
|
+// }
|
|
|
+// return null;
|
|
|
+// }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * 读取文件,获取文件内容
|
|
|
+ * F:\test\project\age_laser\routeMap.txt
|
|
|
+ *
|
|
|
+ * list 是routeMap的结果集
|
|
|
+ * @return
|
|
|
+ */
|
|
|
+ public static JSONArray getRoute(List<String> list, RouteInputDto dto){
|
|
|
+ try {
|
|
|
+ JSONArray maps = init(list, dto);
|
|
|
+ MapInfo info=new MapInfo(maps,start,end);
|
|
|
+ List<Node> path = g_AStar.start(info);
|
|
|
+ JSONArray jsonArray = convertFromPath(path, dto);
|
|
|
+ return jsonArray;
|
|
|
+ } catch (Exception e) {
|
|
|
+ e.printStackTrace();
|
|
|
+ }
|
|
|
+ return null;
|
|
|
+ }
|
|
|
+
|
|
|
+ public static void main(String[] args) {
|
|
|
+ String inputFilePath = "F:\\test\\project\\age_laser\\routeMap.txt";
|
|
|
+ try {
|
|
|
+ JSONArray maps = init();
|
|
|
+
|
|
|
+ MapInfo info=new MapInfo(maps,start,end);
|
|
|
+ List<Node> path = g_AStar.start(info);
|
|
|
+ JSONArray jsonArray = convertFromPath(path);
|
|
|
+ System.out.println(jsonArray);
|
|
|
+ }
|
|
|
+ catch(Exception e){
|
|
|
+ e.printStackTrace();
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|