|
@@ -70,13 +70,22 @@ public class GetRoute_1
|
|
|
// public static String inputFilePath = "F:\\2021\\navvis\\���Ի���\\test3(v2.7.3)\\map.txt";
|
|
|
private static String inputFilePath = "F:\\test\\project\\age_laser\\routeMap.txt";
|
|
|
|
|
|
- private static float startX = 5.358192084229412f;
|
|
|
- private static float startY = -7.905951689748807f;
|
|
|
- private static float startZ = -1.3145928255248511f;
|
|
|
-
|
|
|
- private static float endX = -2.143694831230416f;
|
|
|
- private static float endY = -3.3754012098200965f;
|
|
|
- private static float endZ = -1.1803865408990568f;
|
|
|
+// private static float startX = 5.358192084229412f;
|
|
|
+// private static float startY = -7.905951689748807f;
|
|
|
+// private static float startZ = -1.3145928255248511f;
|
|
|
+//
|
|
|
+// private static float endX = -2.143694831230416f;
|
|
|
+// private static float endY = -3.3754012098200965f;
|
|
|
+// private static float endZ = -1.1803865408990568f;
|
|
|
+
|
|
|
+ private static Double startX = 5.358192084229412;
|
|
|
+ private static Double startY = -7.905951689748807;
|
|
|
+ private static Double startZ = -1.3145928255248511;
|
|
|
+
|
|
|
+ private static Double endX = -2.143694831230416;
|
|
|
+ private static Double endY = -3.3754012098200965;
|
|
|
+ private static Double endZ = -1.1803865408990568;
|
|
|
+
|
|
|
private static int minStartId = -1;
|
|
|
private static int minEndId = -1;
|
|
|
|
|
@@ -88,20 +97,20 @@ public class GetRoute_1
|
|
|
// 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();
|
|
|
+ Double startX = dto.getSource_longitude();
|
|
|
+ Double startY = dto.getSource_latitude();
|
|
|
+ Double startZ = dto.getSource_z();
|
|
|
|
|
|
- float endX = dto.getDestination_longitude();
|
|
|
- float endY = dto.getDestination_latitude();
|
|
|
- float endZ = dto.getDestination_z();
|
|
|
+ Double endX = dto.getDestination_longitude();
|
|
|
+ Double endY = dto.getDestination_latitude();
|
|
|
+ Double endZ = dto.getDestination_z();
|
|
|
|
|
|
|
|
|
Coord _start = new Coord(startX,startY,startZ);
|
|
|
Coord _end = new Coord(endX,endY,endZ);
|
|
|
|
|
|
- float startDistance=1000f;
|
|
|
- float endDistance = 1000f;
|
|
|
+ Double startDistance=1000.0;
|
|
|
+ Double endDistance = 1000.0;
|
|
|
|
|
|
for(int i=0;i<list.size();++i) {
|
|
|
String str = list.get(i);
|
|
@@ -123,13 +132,13 @@ public class GetRoute_1
|
|
|
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);
|
|
|
+ Coord coord = new Coord(Double.valueOf(strArray[0]),Double.valueOf(strArray[1]),Double.valueOf(strArray[2]));
|
|
|
+ Double _startDistance = g_AStar.calcH(_start, coord);
|
|
|
if(_startDistance<startDistance) {
|
|
|
minStartId = i;
|
|
|
startDistance = _startDistance;
|
|
|
}
|
|
|
- float _endDistance = g_AStar.calcH(_end, coord);
|
|
|
+ Double _endDistance = g_AStar.calcH(_end, coord);
|
|
|
if(_endDistance<endDistance) {
|
|
|
minEndId = i;
|
|
|
endDistance = _endDistance;
|
|
@@ -138,12 +147,12 @@ public class GetRoute_1
|
|
|
|
|
|
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);
|
|
|
+ Coord startVirtualCoord = new Coord(virtualStart.getDouble("x"), virtualStart.getDouble("y"),virtualStart.getDouble("z"));
|
|
|
+ Coord endVirtualCoord = new Coord(virtualEnd.getDouble("x"), virtualEnd.getDouble("y"),virtualEnd.getDouble("z"));
|
|
|
+ Double startH = g_AStar.calcH(startVirtualCoord, endVirtualCoord);
|
|
|
|
|
|
- start = new Node(minStartId,0,startVirtualCoord, null, 0, startH);
|
|
|
- end = new Node(minEndId,0,endVirtualCoord, null, 0, 0);
|
|
|
+ start = new Node(minStartId,0.0,startVirtualCoord, null, 0.0, startH);
|
|
|
+ end = new Node(minEndId,0.0,endVirtualCoord, null, 0.0, 0.0);
|
|
|
|
|
|
return maps;
|
|
|
}
|
|
@@ -154,14 +163,14 @@ public class GetRoute_1
|
|
|
}
|
|
|
|
|
|
// 起始点
|
|
|
- float startX = dto.getSource_longitude();
|
|
|
- float startY = dto.getSource_latitude();
|
|
|
- float startZ = dto.getSource_z();
|
|
|
+ Double startX = dto.getSource_longitude();
|
|
|
+ Double startY = dto.getSource_latitude();
|
|
|
+ Double startZ = dto.getSource_z();
|
|
|
|
|
|
// 终点
|
|
|
- float endX = dto.getDestination_longitude();
|
|
|
- float endY = dto.getDestination_latitude();
|
|
|
- float endZ = dto.getDestination_z();
|
|
|
+ Double endX = dto.getDestination_longitude();
|
|
|
+ Double endY = dto.getDestination_latitude();
|
|
|
+ Double endZ = dto.getDestination_z();
|
|
|
|
|
|
|
|
|
List<Node> path = new ArrayList<Node>();
|
|
@@ -182,12 +191,12 @@ public class GetRoute_1
|
|
|
start.put("longitude", startPosition[0]);
|
|
|
start.put("latitude", startPosition[1]);
|
|
|
start.put("z", startZ);
|
|
|
- float[] location = new float[3];
|
|
|
+ Double[] location = new Double[3];
|
|
|
// location[0] = startX;
|
|
|
// location[1] = startY;
|
|
|
|
|
|
- location[0] = (float)startPosition[0];
|
|
|
- location[1] = (float)startPosition[1];
|
|
|
+ location[0] = startPosition[0];
|
|
|
+ location[1] = startPosition[1];
|
|
|
location[2] = startZ;
|
|
|
|
|
|
start.put("location", location);
|
|
@@ -196,7 +205,7 @@ public class GetRoute_1
|
|
|
start.put("instruction", null);
|
|
|
route.add(start);
|
|
|
|
|
|
- float[] virtualEndPosition = new float[3];
|
|
|
+ Double[] virtualEndPosition = new Double[3];
|
|
|
for(int i=0;i<path.size();++i) {
|
|
|
Node node = path.get(i);
|
|
|
JSONObject item = new JSONObject();
|
|
@@ -210,12 +219,12 @@ public class GetRoute_1
|
|
|
// item.put("longitude", node.coord.x);
|
|
|
// item.put("latitude", node.coord.y);
|
|
|
item.put("z", node.coord.z);
|
|
|
- location = new float[3];
|
|
|
+ location = new Double[3];
|
|
|
// location[0] = node.coord.x;
|
|
|
// location[1] = node.coord.y;
|
|
|
|
|
|
- location[0] = (float)position[0];
|
|
|
- location[1] = (float)position[1];
|
|
|
+ location[0] = position[0];
|
|
|
+ location[1] = position[1];
|
|
|
|
|
|
location[2] = node.coord.z;
|
|
|
item.put("location", location);
|
|
@@ -235,9 +244,9 @@ public class GetRoute_1
|
|
|
else {
|
|
|
Node prenode = path.get(i-1);
|
|
|
JSONObject preitem = route.getJSONObject(i);
|
|
|
- float distance_to_previous = g_AStar.calcH(node.coord,prenode.coord);
|
|
|
+ Double 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;
|
|
|
+ Double distance = preitem.getDouble("distance")+distance_to_previous;
|
|
|
item.put("distance", distance);
|
|
|
if(i == path.size()-1) {
|
|
|
instruction = new JSONObject();
|
|
@@ -266,12 +275,12 @@ public class GetRoute_1
|
|
|
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 = new Double[3];
|
|
|
+ location[0] = endPosition[0];
|
|
|
+ location[1] = 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));
|
|
|
+ Double 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"));
|
|
|
end.put("distance_to_previous", enddistance);
|
|
|
end.put("instruction", null);
|
|
@@ -287,9 +296,9 @@ public class GetRoute_1
|
|
|
|
|
|
Coord _start = new Coord(startX,startY,startZ);
|
|
|
Coord _end = new Coord(endX,endY,endZ);
|
|
|
-
|
|
|
- float startDistance=1000f;
|
|
|
- float endDistance = 1000f;
|
|
|
+
|
|
|
+ Double startDistance=1000.0;
|
|
|
+ Double endDistance = 1000.0;
|
|
|
|
|
|
for(int i=0;i<list.size();++i) {
|
|
|
String str = list.get(i);
|
|
@@ -311,13 +320,13 @@ public class GetRoute_1
|
|
|
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);
|
|
|
+ Coord coord = new Coord(Double.valueOf(strArray[0]),Double.valueOf(strArray[1]),Double.valueOf(strArray[2]));
|
|
|
+ Double _startDistance = g_AStar.calcH(_start, coord);
|
|
|
if(_startDistance<startDistance) {
|
|
|
minStartId = i;
|
|
|
startDistance = _startDistance;
|
|
|
}
|
|
|
- float _endDistance = g_AStar.calcH(_end, coord);
|
|
|
+ Double _endDistance = g_AStar.calcH(_end, coord);
|
|
|
if(_endDistance<endDistance) {
|
|
|
minEndId = i;
|
|
|
endDistance = _endDistance;
|
|
@@ -326,12 +335,12 @@ public class GetRoute_1
|
|
|
|
|
|
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);
|
|
|
+ Coord startVirtualCoord = new Coord(virtualStart.getDouble("x"), virtualStart.getDouble("y"),virtualStart.getDouble("z"));
|
|
|
+ Coord endVirtualCoord = new Coord(virtualEnd.getDouble("x"), virtualEnd.getDouble("y"),virtualEnd.getDouble("z"));
|
|
|
+ Double startH = g_AStar.calcH(startVirtualCoord, endVirtualCoord);
|
|
|
|
|
|
- start = new Node(minStartId,0,startVirtualCoord, null, 0, startH);
|
|
|
- end = new Node(minEndId,0,endVirtualCoord, null, 0, 0);
|
|
|
+ start = new Node(minStartId,0.0,startVirtualCoord, null, 0.0, startH);
|
|
|
+ end = new Node(minEndId,0.0,endVirtualCoord, null, 0.0, 0.0);
|
|
|
|
|
|
return maps;
|
|
|
}
|
|
@@ -354,7 +363,7 @@ public class GetRoute_1
|
|
|
start.put("longitude", startX);
|
|
|
start.put("latitude", startY);
|
|
|
start.put("z", startZ);
|
|
|
- float[] location = new float[3];
|
|
|
+ Double[] location = new Double[3];
|
|
|
location[0] = startX;
|
|
|
location[1] = startY;
|
|
|
location[2] = startZ;
|
|
@@ -370,7 +379,7 @@ public class GetRoute_1
|
|
|
item.put("longitude", node.coord.x);
|
|
|
item.put("latitude", node.coord.y);
|
|
|
item.put("z", node.coord.z);
|
|
|
- location = new float[3];
|
|
|
+ location = new Double[3];
|
|
|
location[0] = node.coord.x;
|
|
|
location[1] = node.coord.y;
|
|
|
location[2] = node.coord.z;
|
|
@@ -391,9 +400,9 @@ public class GetRoute_1
|
|
|
else {
|
|
|
Node prenode = path.get(i-1);
|
|
|
JSONObject preitem = route.getJSONObject(i);
|
|
|
- float distance_to_previous = g_AStar.calcH(node.coord,prenode.coord);
|
|
|
+ Double 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;
|
|
|
+ Double distance = preitem.getDouble("distance")+distance_to_previous;
|
|
|
item.put("distance", distance);
|
|
|
if(i == path.size()-1) {
|
|
|
instruction = new JSONObject();
|
|
@@ -414,13 +423,13 @@ public class GetRoute_1
|
|
|
end.put("longitude", endX);
|
|
|
end.put("latitude", endY);
|
|
|
end.put("z", endZ);
|
|
|
- location = new float[3];
|
|
|
+ location = new Double[3];
|
|
|
location[0] = endX;
|
|
|
location[1] = endY;
|
|
|
location[2] = endZ;
|
|
|
end.put("location", location);
|
|
|
- float enddistance = g_AStar.calcH(new Coord((float)endItem.getDouble("longitude"),(float)endItem.getDouble("latitude"),(float)endItem.getDouble("z")),new Coord(endX,endY,endZ));
|
|
|
- end.put("distance", enddistance+(float)endItem.getDouble("distance_to_previous"));
|
|
|
+ Double enddistance = g_AStar.calcH(new Coord(endItem.getDouble("longitude"),endItem.getDouble("latitude"),endItem.getDouble("z")),new Coord(endX,endY,endZ));
|
|
|
+ end.put("distance", enddistance+endItem.getDouble("distance_to_previous"));
|
|
|
end.put("distance_to_previous", enddistance);
|
|
|
end.put("instruction", null);
|
|
|
route.add(end);
|