We have modified the last code. Now we support the shortest path search in the case of indoor multi floors. We still use the A * algorithm to use the data field that VNode does not use in GraphAdjList as the location where we store the floor properties.
In fact, I'm lazy. Under normal circumstances, an int level attribute should be added to VNode, and data is still used as a location where the binding user wants to add any type of data. For example, when the user wants to add a description of String type to any node, it's OK to declare GraphAdjList < String >, but now our GraphAdjList can only be declared as GraphAdjList < integer > , because we use data as the floor property, the template class hh with a real name.
When users add nodes, use GraphAdjList.insertVex(E v,int index,int x,int y), v floor, the unique serial number of the index node (from 1, in line with living habits), x,y is the point coordinate, and the operation of adding edge is the same as last time.
It should be noted that in our A * code, f=g+h, the setting of heuristic function h may not be ideal. We still use the Manhattan distance of x,y, and do not consider the influence of factors such as the location and number of floors of the stairway. If the starting point and the end point are all in the center of the floor, the search direction of the stairway elevator at the edge of each floor will first expand to the center until there is no result. In this case, the efficiency may be poor, but the shortest path can still be obtained. In short, under the current design h, the starting point will be extended to the vertical point of the layer where the end point belongs. As for the design of H, you can refer to the relevant literature.
We still use TestContinuous as our test class and draw a simple data case to query the shortest path from node 4 on the first floor to node 14 on the third floor.
There are only channels 1-6, 3-8 between the first floor and the second floor, and the cost is 3, 4 respectively. There are only 10-11 passages between the second and third floors, the price is 5.
The final result is 4-5-1-6-10-11-13-14, and the total cost is 33.
Say nothing but code.
The code structure is as follows:
AStar:
package astar3D; import java.util.ArrayList; import java.util.Collections; import java.util.Comparator; import java.util.HashMap; import java.util.HashSet; import java.util.List; import java.util.Map; import java.util.Set; import astar3D.GraphAdjList.ENode; import astar3D.GraphAdjList.VNode; /** * * @author yh * @version 3.0 * */ public class AStar implements IMove { /* Open list */ Map<String, Point> openMap = new HashMap<String, Point>(); /* Closed list */ Map<String, Point> closeMap = new HashMap<String, Point>(); /* Obstacle */ Set<Point> barrier; /* Starting point */ Point startPoint; /* End */ Point endPoint; /* Node currently in use */ Point currentPoint; /* Number of cycles to prevent the target from being unreachable */ int num = 0; //Stored data structure public GraphAdjList<Integer> graphadjlist; /** * Initialize and start calculating the best path * @param point1 Starting point of user input * @param point2 End point entered by user * @param barrier List of obstacles without sequence */ @Override public Point move(Point point1, Point point2, Set<Point> barrier) { num = 0; this.barrier = barrier; this.startPoint = findNearPoint(point1); this.endPoint = findNearPoint(point2); //Reserve a place to solve the problem of the point in the obstacle //Point endPoint=new Point(x2,y2); //this.endPoint = this.getNearPoint(endPoint,endPoint); this.closeMap.put(startPoint.getKey(), startPoint); this.currentPoint = this.startPoint; this.toOpen(startPoint); return endPoint; } /** * The heuristic function one (Manhattan distance): (Math.abs(x1 - x2) + Math.abs(y1 - y2)) * Heuristic function two (Euclidean distance of square): ((X2 - X1 * (X2 - x1) + (Y2 - Y1) * (Y2 - Y1)) * Heuristic function three (Euclidean distance): (int) Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) *(y2 -y1)) * Heuristic function four (diagonal distance): Math.max(Math.abs(x1 - x2), Math.abs(y1 -y2)) * No heuristic function: 0 */ private int getGuessLength(int x1, int y1, int x2, int y2) { //return ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 -y1)); return (Math.abs(x1 - x2) + Math.abs(y1 - y2)) ; // return Math.max(Math.abs(x1 - x2), Math.abs(y1 - y2)); // return 0; } /** * Find the nearest starting point next to the point coordinate entered by the user * @param point User entered coordinate point * @return Nearest departure point */ private Point findNearPoint(Point point){ List<Integer> levelVertex = graphadjlist.getLevelVertex(point.level); if(levelVertex.size() > 0){ int index = levelVertex.get(0); int min = getGuessLength(point.x, point.y, graphadjlist.vexs[index].x, graphadjlist.vexs[index].y); int tempmin; for(int i = 1; i < levelVertex.size(); i++){ tempmin = getGuessLength(point.x, point.y, graphadjlist.vexs[levelVertex.get(i)].x, graphadjlist.vexs[levelVertex.get(i)].y); if(tempmin < min ){ min = tempmin; index = levelVertex.get(i); } } Point nearPoint = new Point( graphadjlist.vexs[index].x, graphadjlist.vexs[index].y, point.level, index); return nearPoint; } return new Point(point.x, point.y, 0, 0); } /** * Add the adjacent points of the node to the calculation * @param point */ private void toOpen(Point point) { Set<Integer> adjPoint = new HashSet<Integer>(); if(graphadjlist.vexs[point.serial].firstadj == null){ return; }else{ ENode current; current = graphadjlist.vexs[point.serial].firstadj; while(current != null){ adjPoint.add(current.adjvex); current = current.nextadj; } } for (int serial : adjPoint) { VNode<Integer> currentNode = graphadjlist.vexs[serial]; //Temporary template class GraphAdjList Only support int Type, limited by Point It is not a template class, and the floor variable type is int(currentNode.data yes int) this.addOpenPoint(new Point(currentNode.x, currentNode.y, currentNode.data, serial), graphadjlist.getEdge(currentPoint.serial, serial)); } num++; if (num <= 4000) { this.toClose(); } } /** * Add adjacent points of the node to the closed list */ private void toClose() { List<Point> list = new ArrayList<Point>(openMap.values()); Collections.sort(list, new Comparator<Point>() { @Override //Sort in ascending order, and then take out the first element. public int compare(Point o1, Point o2) { if (o1.fTotal > o2.fTotal) { return 1; } else if (o1.fTotal < o2.fTotal) { return -1; } else { return 0; } } }); if (list.size() > 0) { this.currentPoint = list.get(0); closeMap.put(this.currentPoint.getKey(), this.currentPoint); openMap.remove(this.currentPoint.getKey()); if (!currentPoint.equals(endPoint)) { this.toOpen(this.currentPoint); } else { endPoint = this.currentPoint; } } } /** * A*Core processing function * @param point currentPoint Point of connection * @param gCost Consumption from current point to this point * @return */ private void addOpenPoint(Point point,int gCost) { if (point.x < 0 || point.y < 0) { return; } String key = point.getKey(); if (!barrier.contains(point) && !point.equals(this.currentPoint)) { int hEstimate = this.getGuessLength(point.x, point.y, this.endPoint.x, this.endPoint.y); int totalGCost = this.currentPoint.gCost + gCost; int fTotal = totalGCost + hEstimate; //current point Not added to closeMap In, put openMap Medium for toClose Function comparison fTotal,And select the best point to prepare if (!closeMap.containsKey(key)) { point.hEstimate = hEstimate; point.gCost = totalGCost; point.fTotal = fTotal; Point oldPoint = openMap.get(key); //If this point has been added to openMap，See if it needs to be updated to the minimum if (oldPoint != null) { if (oldPoint.gCost > totalGCost) { oldPoint.fTotal = fTotal; oldPoint.gCost=totalGCost; oldPoint.prev = this.currentPoint; //When key Same time, back put That will cover the front. openMap.put(key, oldPoint); } } else { point.prev = this.currentPoint; openMap.put(key, point); } } else { Point oldPoint = closeMap.get(key); if (oldPoint != null) { if ((oldPoint.gCost + gCost) < this.currentPoint.gCost) { if (this.currentPoint.prev != oldPoint) { this.currentPoint.fTotal = oldPoint.fTotal + gCost; this.currentPoint.gCost = oldPoint.gCost + gCost; this.currentPoint.prev = oldPoint; } } } } } } //The following three functions haven't been modified, so we don't need to worry about them. //If the point selected by the user is in the obstacle, select the distance outside the obstacle endPoint The nearest thing to do endPoint Map<String, Point> nearOutMap; public Point getNearPoint(Point point,Point point2) { if(this.barrier.contains(point)){ nearOutMap = new HashMap<String, Point>(); this.endPoint=point; this.toNearPoint(point,point2); List<Point> nearList = new ArrayList<Point>(nearOutMap.values()); Collections.sort(nearList, new Comparator<Point>() { @Override public int compare(Point o1, Point o2) { if (o1.gCost > o2.gCost) { return 1; } else if (o1.gCost < o2.gCost) { return -1; } else { return 0; } } }); //We just used these two variables. Now the nearest point outside the obstacle has been found. Initialization preparation A* this.openMap=new HashMap<String,Point>(); this.closeMap=new HashMap<String,Point>(); if (nearList.size() > 0) { return nearList.get(0); }else{ return point; } }else{ return point; } } public void toNearPoint(Point point,Point point2) { int x = point.x; int y = point.y; this.addNearOpenPoint(new Point(x - 1, y),point2); this.addNearOpenPoint(new Point(x + 1, y),point2); this.addNearOpenPoint(new Point(x, y - 1),point2); this.addNearOpenPoint(new Point(x, y + 1),point2); this.addNearOpenPoint(new Point(x - 1, y - 1),point2); this.addNearOpenPoint(new Point(x - 1, y + 1),point2); this.addNearOpenPoint(new Point(x + 1, y - 1),point2); this.addNearOpenPoint(new Point(x + 1, y + 1),point2); if(this.nearOutMap.size()==0){ List<Point> list = new ArrayList<Point>(openMap.values()); //Sort in ascending order with the smallest in list Foremost Collections.sort(list, new Comparator<Point>() { @Override public int compare(Point o1, Point o2) { int l1 = o1.gCost; int l2 = o2.gCost; if (l1 > l2) { return 1; } else if (l1 < l2) { return -1; } else { return 0; } } }); if (list.size() > 0) { Point p = list.get(0); this.closeMap.put(p.getKey(), p); this.openMap.remove(p.getKey()); this.toNearPoint(list.get(0),point2); } } } private void addNearOpenPoint(Point point,Point point2) { String key = point.getKey(); int gCost = this.getGuessLength(point.x, point.y, point2.x,point2.y); point.gCost = gCost; if (this.barrier.contains(point)) { if (!this.openMap.containsKey(key) && !this.closeMap.containsKey(key)) { this.openMap.put(key, point); } } else { this.nearOutMap.put(key, point); } } public Map<String, Point> getOpenMap() { return openMap; } public void setOpenMap(Map<String, Point> openMap) { this.openMap = openMap; } public Map<String, Point> getCloseMap() { return closeMap; } public void setCloseMap(Map<String, Point> closeMap) { this.closeMap = closeMap; } public Set<Point> getBarrier() { return barrier; } public void setBarrier(Set<Point> barrier) { this.barrier = barrier; } public Point getEndPoint() { return endPoint; } public void setEndPoint(Point endPoint) { this.endPoint = endPoint; } public Point getStartPoint() { return startPoint; } public void setStartPoint(Point startPoint) { this.startPoint = startPoint; } }
GraphAdjList:
package astar3D; import java.lang.reflect.Array; import java.util.ArrayList; import java.util.List; public class GraphAdjList<E> implements IGraph<E> { // Vertex of linked list corresponding to table in adjacency list public static class ENode { int adjvex; // Adjacent vertex No. int weight;// Stores edge or arc related information, such as weights ENode nextadj; // Next adjacency table node public ENode(int adjvex, int weight) { this.adjvex = adjvex; this.weight = weight; } } // Vertex of table in adjacency table public static class VNode<E> { E data; // The field for storing information. This is the floor. int x; int y; ENode firstadj; // //The first node of adjacency table }; public VNode<E>[] vexs; // vertex array private int numOfVexs;// Actual number of vertices private int maxNumOfVexs;// Maximum number of vertices //private boolean[] visited;// Determine whether the vertex has been visited @SuppressWarnings("unchecked") public GraphAdjList(int maxNumOfVexs) { this.maxNumOfVexs = maxNumOfVexs; vexs = (VNode<E>[]) Array.newInstance(VNode.class, maxNumOfVexs); } // Get the number of vertices public int getNumOfVertex() { return numOfVexs; } //Get the number of vertices of a floor public int getNumOfLevelVertex(E v){ int numOfLevelVexs = 0; for(int i = 1; i < numOfVexs + 1; i++){ if(vexs[i].data.equals(v)){ numOfLevelVexs++; } } return numOfLevelVexs; } //Get the list of vertex serial numbers of the first floor public List<Integer> getLevelVertex(E v){ List<Integer> levelVertex = new ArrayList<Integer>(); for(int i = 1; i < numOfVexs + 1; i++){ if(vexs[i].data.equals(v)){ levelVertex.add(i); } } return levelVertex; } // Get the floor of the specified location node public E getLevel(int index) { if (index < 0 || index > numOfVexs) return null; return vexs[index].data; } // Insert Vertex,If you insert another index The same node covers public boolean insertVex(E v,int index,int x,int y) { if (numOfVexs >= maxNumOfVexs || index > 1000) return false; if (vexs[index] == null ){ numOfVexs++; } VNode<E> vex = new VNode<E>(); vex.data = v; vex.x = x; vex.y = y; vexs[index] = vex; return true; } // Delete Vertex public boolean deleteVex(int index) { if (index > 0 && index < numOfVexs + 1) { //delete vexs Related records in for (int i = index; i < numOfVexs; i++) { vexs[i] = vexs[i + 1]; } vexs[numOfVexs] = null; numOfVexs--; ENode current; ENode previous; //delete ENode Medium for (int i = 1; i < numOfVexs + 1; i++) { if (vexs[i].firstadj == null) continue; if (vexs[i].firstadj.adjvex == index && vexs[i].firstadj.nextadj == null) { vexs[i].firstadj = null; continue; } current = vexs[i].firstadj; while (current != null) { previous = current; current = current.nextadj; if (current != null && current.adjvex == index) { previous.nextadj = current.nextadj; break; } } } //For each ENode Medium adjvex Modify for (int i = 1; i < numOfVexs + 1; i++) { current = vexs[i].firstadj; while (current != null) { if (current.adjvex > index) current.adjvex--; current = current.nextadj; } } return true; } return false; } // Insertion edge public boolean insertEdge(int v1, int v2, int weight) { if (v1 < 0 || v2 < 0 || v1 > numOfVexs || v2 > numOfVexs) throw new ArrayIndexOutOfBoundsException(); ENode vex1 = new ENode(v2, weight); // Index is index1 No adjacent vertices for if (vexs[v1].firstadj == null) { vexs[v1].firstadj = vex1; } // Index is index1 The vertices of have adjacent vertices else { vex1.nextadj = vexs[v1].firstadj; vexs[v1].firstadj = vex1; } ENode vex2 = new ENode(v1, weight); // Index is index2 No adjacent vertices for if (vexs[v2].firstadj == null) { vexs[v2].firstadj = vex2; } // Index is index1 The vertices of have adjacent vertices else { vex2.nextadj = vexs[v2].firstadj; vexs[v2].firstadj = vex2; } return true; } // Edge deletion public boolean deleteEdge(int v1, int v2) { if (v1 < 0 || v2 < 0 || v1 > numOfVexs || v2 > numOfVexs) throw new ArrayIndexOutOfBoundsException(); // Delete index as index1 The vertex and index of are index2 Edge between vertices of ENode current = vexs[v1].firstadj; ENode previous = null; while (current != null && current.adjvex != v2) { previous = current; current = current.nextadj; } if (current != null) previous.nextadj = current.nextadj; // Delete index as index2 The vertex and index of are index1 Edge between vertices of current = vexs[v2].firstadj; while (current != null && current.adjvex != v1) { previous = current; current = current.nextadj; } if (current != null) previous.nextadj = current.nextadj; return true; } // Get edge public int getEdge(int v1, int v2) { if (v1 < 0 || v2 < 0 || v1 > numOfVexs || v2 > numOfVexs) throw new ArrayIndexOutOfBoundsException(); ENode current = vexs[v1].firstadj; while (current != null) { if (current.adjvex == v2) { return current.weight; } current = current.nextadj; } return 0; } }
IGraph:
package astar3D; import java.util.List; public interface IGraph<E> { public int getNumOfVertex();//Get the number of vertices public int getNumOfLevelVertex(E v);//Get the number of vertices of a floor public List<Integer> getLevelVertex(E v);//Get the list of vertex serial numbers of the first floor public E getLevel(int index);//Get the floor of the specified location node boolean insertVex(E v, int index, int x, int y);//Insert Vertex boolean deleteVex(int index);//Delete Vertex boolean insertEdge(int v1, int v2,int weight);//Insertion edge boolean deleteEdge(int v1, int v2);//Edge deletion int getEdge(int v1,int v2);//Search edge }
IMove:
package astar3D; import java.util.Set; /** * * @author yh * @version 3.0 * */ public interface IMove { /** * Find a suitable route from point 1 to point 2 * @param point1 Starting point of user input * @param point2 End point entered by user * @param barrier List of obstacles without sequence * @return */ Point move(Point point1, Point point2, Set<Point> barrier); }
Point:
package astar3D; public class Point { int x; int y; int gCost; int hEstimate; int fTotal; Point prev; //Floor where the point is located int level; //Serial number of the point int serial; public String getKey(){ return level + "_" + x + "_" + y; } public Point(int x, int y) { super(); this.x = x; this.y = y; } /** * * @param x * @param y * @param level floor */ public Point(int x, int y, int level){ super(); this.x = x; this.y = y; this.level = level; } /** * When the user does not input an integer, convert it to an integer and then process it * @param x * @param y * @param level */ public Point(double x, double y, int level){ super(); this.x = (int) x; this.y = (int) y; this.level = level; } /** * * @param x * @param y * @param level floor * @param serialNumber Ordinal number of the point (unique value) */ public Point(int x, int y, int level,int serialNumber) { super(); this.x = x; this.y = y; this.level = level; this.serial = serialNumber; } @Override public int hashCode() { final int prime = 31; int result = 1; result = prime * result + x; result = prime * result + y; return result; } @Override public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; Point other = (Point) obj; if (x != other.x) return false; if (y != other.y) return false; if (level != other.level) return false; return true; } }
TestContinuous:
package astar3D; import java.util.ArrayList; import java.util.HashSet; import java.util.List; import java.util.Set; import org.junit.Test; public class TestContinuous { @Test public void test2() { GraphAdjList<Integer> graphadjlist=new GraphAdjList<Integer>(1000); graphadjlist.insertVex(1, 1, 1, 1); graphadjlist.insertVex(1, 2, 2, 1); graphadjlist.insertVex(1, 3, 3, 2); graphadjlist.insertVex(1, 4, 2, 3); graphadjlist.insertVex(1, 5, 1, 3); graphadjlist.insertVex(2, 6, 1, 2); graphadjlist.insertVex(2, 7, 3, 2); graphadjlist.insertVex(2, 8, 3, 4); graphadjlist.insertVex(2, 9, 1, 4); graphadjlist.insertVex(2, 10, 2, 3); graphadjlist.insertVex(3, 11, 2, 2); graphadjlist.insertVex(3, 12, 1, 2); graphadjlist.insertVex(3, 13, 3, 2); graphadjlist.insertVex(3, 14, 2, 1); graphadjlist.insertEdge(1, 2, 10); graphadjlist.insertEdge(1, 5, 3); graphadjlist.insertEdge(2, 3, 15); graphadjlist.insertEdge(2, 4, 7); graphadjlist.insertEdge(2, 5, 13); graphadjlist.insertEdge(3, 4, 8); graphadjlist.insertEdge(4, 5, 8); graphadjlist.insertEdge(1, 6, 3); graphadjlist.insertEdge(3, 8, 4); graphadjlist.insertEdge(6, 9, 6); graphadjlist.insertEdge(9, 8, 4); graphadjlist.insertEdge(8, 7, 5); graphadjlist.insertEdge(7, 6, 2); graphadjlist.insertEdge(6, 10, 3); graphadjlist.insertEdge(9, 10, 15); graphadjlist.insertEdge(7, 10, 1); graphadjlist.insertEdge(10, 11, 5); graphadjlist.insertEdge(11, 12, 5); graphadjlist.insertEdge(12, 14, 8); graphadjlist.insertEdge(11, 13, 9); graphadjlist.insertEdge(13, 14, 2); Set<Point> barrier = new HashSet<Point>(); //barrier.add(new Point(1, 3, 1)); AStar aStar = new AStar(); aStar.graphadjlist = graphadjlist; Point startPoint = new Point(2.2, 3.1, 1); Point endPoint = new Point(2, 1, 3); endPoint = aStar.move(startPoint, endPoint, barrier); List<Point> list = new ArrayList<Point>(); list = TestContinuous.get(endPoint, list); for (Point point : list) { System.out.println(point.serial); } System.out.println(endPoint.fTotal); } //Build path collection public static List<Point> get(Point p, List<Point> list) { if (p != null) { list.add(p); } Point pp = p.prev; if (pp != null) { TestContinuous.get(pp, list); } else { return list; } return list; } }
If you want to realize the shortest three-dimensional path search like (x,y,z) format for every point, you can modify the code in two-dimensional case (the last one written).
Post our main reference:
https://blog.csdn.net/h348592532/article/details/44421753