├── .gitignore
├── .classpath
├── .project
├── README.md
├── LICENSE
└── source
    └── projects
        └── multipath
            ├── advanced
                ├── Path.java
                ├── Vertex.java
                ├── Problem.java
                └── Graph.java
            └── ILP
                ├── PuzzleSolver.java
                ├── PathFinder.java
                ├── MultiagentGraphSolverGurobiTimePuzzle.java
                ├── MultiagentGraphSolverGurobiDistance.java
                ├── HeuristicPathPlanner.java
                ├── MultiagentGraphSolverGurobiTime.java
                ├── MultiagentGraphSolverGurobiTotalTime.java
                ├── MultiagentGraphSolverGurobiTimeR.java
                └── Main.java


/.gitignore:
--------------------------------------------------------------------------------
1 | /bin/
2 | 


--------------------------------------------------------------------------------
/.classpath:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <classpath>
3 | 	<classpathentry kind="src" path="source"/>
4 | 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/J2SE-1.5"/>
5 | 	<classpathentry kind="lib" path="/shared/lib/gurobi.jar"/>
6 | 	<classpathentry kind="output" path="bin"/>
7 | </classpath>
8 | 


--------------------------------------------------------------------------------
/.project:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <projectDescription>
 3 | 	<name>omrppg-journal</name>
 4 | 	<comment></comment>
 5 | 	<projects>
 6 | 	</projects>
 7 | 	<buildSpec>
 8 | 		<buildCommand>
 9 | 			<name>org.eclipse.jdt.core.javabuilder</name>
10 | 			<arguments>
11 | 			</arguments>
12 | 		</buildCommand>
13 | 	</buildSpec>
14 | 	<natures>
15 | 		<nature>org.eclipse.jdt.core.javanature</nature>
16 | 	</natures>
17 | </projectDescription>
18 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ### Optimal Multi-Robot Path Planning on Graphs: Complete Algorithms and Effective Heuristics. 
 2 | 
 3 | ![mrmp-tor-13](https://user-images.githubusercontent.com/23622170/124762308-c26f4c00-df00-11eb-8fa0-704966a57d4d.png)
 4 | 
 5 | Source code used in the following work
 6 | 
 7 | J. Yu and S. M. LaValle. Optimal Multi-Robot Path Planning on Graphs: Complete Algorithms and Effective Heuristics. IEEE Transactions on Robotics, 32(5), page(s): 1163 - 1177, 2016.
 8 | 
 9 | ### Requires 
10 | 
11 | Gurobi (6.5+): http://www.gurobi.com/ 
12 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | All components of this library are licensed under the BSD 3-Clause 
 2 | License.
 3 | 
 4 | Copyright (c) 2015-, Rutgers Algorithmic Robotics and Control Group 
 5 | (https://arc.cs.rutgers.edu). All rights reserved.
 6 | 
 7 | Redistribution and use in source and binary forms, with or without
 8 | modification, are permitted provided that the following conditions are
 9 | met:
10 | 	
11 | * Redistributions of source code must retain the above copyright notice,
12 |   this list of conditions and the following disclaimer.  
13 | 
14 | * Redistributions in binary form must reproduce the above copyright 
15 |   notice, this list of conditions and the following disclaimer in the 
16 |   documentation and/or other materials provided with the distribution. 
17 | 
18 | * Neither the name of the copyright holder nor the names of its 
19 |   contributors may be used to endorse or promote products derived from 
20 |   this software without specific prior written permission. 
21 | 
22 | 	
23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
26 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
27 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
28 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
29 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
30 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
31 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 | 


--------------------------------------------------------------------------------
/source/projects/multipath/advanced/Path.java:
--------------------------------------------------------------------------------
 1 | package projects.multipath.advanced;
 2 | 
 3 | import java.awt.Font;
 4 | import java.awt.Graphics2D;
 5 | import java.util.ArrayList;
 6 | import java.util.List;
 7 | 
 8 | public class Path {
 9 | 
10 | 	public int id = 0;
11 | 	public List<Integer> vertexList = new ArrayList<Integer>();
12 | 
13 | 	public void addVertex(Integer v){
14 | 		vertexList.add(v);
15 | 	}
16 | 	
17 | 	public void insertVertex(Integer v){
18 | 		vertexList.add(0, v);
19 | 	}
20 | 	
21 | 	public void printPath(Graph g){
22 | 		for(int i = 0; i < vertexList.size(); i ++){
23 | 			g.idVertexMap.get(vertexList.get(i)).printVertex();
24 | 			System.out.print("|");
25 | 		}
26 | 		System.out.println();
27 | 	}
28 | 
29 | 	public void drawPaths(Graphics2D g2d, Graph g, int scale, int number) {
30 | 		int currentEdge = 0;
31 | 
32 | 		// Draw edges
33 | 		g2d.setPaint(new java.awt.Color(0x202020));
34 | 		while(currentEdge < vertexList.size() - 1){
35 | 			Vertex startVertex = g.idVertexMap.get(vertexList.get(currentEdge));
36 | 			Vertex endVertex = g.idVertexMap.get(vertexList.get(currentEdge + 1));
37 | 			int sx = (int)(startVertex.getX());
38 | 			int sy = (int)(startVertex.getY());
39 | 			int ex = (int)(endVertex.getX());
40 | 			int ey = (int)(endVertex.getY());
41 | 			g2d.drawLine((sx) * scale, (sy)*scale, (ex)*scale, (ey)*scale);
42 | 			currentEdge ++;
43 | 		}
44 | 
45 | 	}
46 | 
47 | 	public void drawRobots(Graphics2D g2d, Graph g, int scale, int number) {
48 | 		// Draw beginning location as a dot
49 | 		Vertex firstVertex = g.idVertexMap.get(vertexList.get(0));
50 | 		g2d.setPaint(new java.awt.Color(0x6495ed));
51 | 		//g2d.drawOval((int)(firstVertex.getX()) * scale - scale/2 + 1, (int)(firstVertex.getY()) * scale - scale/2 + 1, scale - 2, scale - 2);
52 | 		g2d.fillOval((int)(firstVertex.getX()) * scale - scale/2 + 1, (int)(firstVertex.getY()) * scale - scale/2 + 1, scale - 2, scale - 2);
53 | 		
54 | 		Font f = new Font(g2d.getFont().getName(), Font.BOLD, 5);
55 | 		g2d.setPaint(new java.awt.Color(0xF0F0F0));
56 | 		g2d.setFont(f);
57 | 		String s = "" + number;
58 | 		g2d.drawString(s, (int)(firstVertex.getX()*scale - scale/3) + (number>9?0:2),(int)((firstVertex.getY())*scale + 2));
59 | 
60 | 	}
61 | }
62 | 
63 | 


--------------------------------------------------------------------------------
/source/projects/multipath/advanced/Vertex.java:
--------------------------------------------------------------------------------
 1 | package projects.multipath.advanced;
 2 | 
 3 | import java.awt.Font;
 4 | import java.awt.Graphics2D;
 5 | import java.awt.geom.Point2D;
 6 | 
 7 | public class Vertex {
 8 | 	
 9 | 	public int id;
10 | 	protected Point2D point = null;
11 | 	public int usedInPath = 0;
12 | 
13 | 	public static final int STYLE_START = 1; 
14 | 	public static final int STYLE_END = 2; 
15 | 	
16 | 	public Vertex(Point2D point, int id){
17 | 		this.point = point;
18 | 		this.id = id;
19 | 	}
20 | 	
21 | 	public Vertex(Point2D point) {
22 | 		this.point = point;
23 | 	}
24 | 
25 | 	public Vertex(int id){
26 | 		this.id = id;
27 | 	}
28 | 
29 | 	public double getX(){
30 | 		return point.getX();
31 | 	}
32 | 	
33 | 	public double getY(){
34 | 		return point.getY();
35 | 	}
36 | 	
37 | 	public int getIX(){
38 | 		return (int)(getX());
39 | 	}
40 | 	
41 | 	public int getIY(){
42 | 		return (int)(getY());
43 | 	}
44 | 
45 | 	public void print(){
46 | 		System.out.print("" + id);
47 | 	}
48 | 	
49 | 	public void printVertex(){
50 | 		print();
51 | 		if(point != null){
52 | 			System.out.print("(" + getIX()+"," + getIY() + ")");
53 | 			// System.out.print("" + getIX()+"," + getIY() + "");
54 | 		}
55 | 	}
56 | 	
57 | 	public void printVertex(StringBuffer buffer){
58 | 		if(point != null){
59 | 			buffer.append("(" + getIX()+"," + getIY() + ")");
60 | 		}
61 | 	}
62 | 
63 | 	public void draw(Graphics2D g2d, int scale, int style, int number) {
64 | 		g2d.setPaint(new java.awt.Color(0xF0F0F0));
65 | 		g2d.fillOval((int)(this.point.getX()*scale - scale/2),(int)(this.point.getY()*scale - scale/2), scale, scale);
66 | 
67 | 		switch(style){
68 | 		case STYLE_START: 
69 | 			g2d.setPaint(new java.awt.Color(0xD04040));
70 | 			break;
71 | 		case STYLE_END: 
72 | 			g2d.setPaint(new java.awt.Color(0x4040D0));
73 | 			break;
74 | 			default:;
75 | 		}
76 | 
77 | 		g2d.drawOval((int)(this.point.getX()*scale - scale/2),(int)(this.point.getY()*scale - scale/2), scale, scale);
78 | 		
79 | 		Font f = new Font(g2d.getFont().getName(), Font.BOLD, 5);
80 | 		g2d.setFont(f);
81 | 		String s = "" + number;
82 | 		g2d.drawString(s, (int)(this.point.getX()*scale - scale/3) + (number>9?0:2),(int)(this.point.getY()*scale + 2));
83 | 	}
84 | 
85 | }
86 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/PuzzleSolver.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import projects.multipath.advanced.Graph;
  4 | import projects.multipath.advanced.Path;
  5 | import projects.multipath.advanced.Problem;
  6 | import gurobi.GRB;
  7 | import gurobi.GRBCallback;
  8 | import gurobi.GRBException;
  9 | 
 10 | public class PuzzleSolver extends GRBCallback{
 11 | 	
 12 | 	protected boolean optimizationDone = false;
 13 | 	protected int goodSolution = 0;
 14 | 
 15 | 	Graph graph = null;
 16 | 	int[] starts = null, goals = null;
 17 | 	PuzzleSolver callback = null;
 18 | 	int[][] sol1 = null, sol2 = null;
 19 | 	boolean flag1 = false, flag2 = false;
 20 | 	private boolean reachableVertices[][][] = null;
 21 | 	int startTimeSteps = 0;
 22 | 	boolean failedAttempt = false;
 23 | 	boolean allThreadFinished = false;
 24 | 	
 25 | 	public PuzzleSolver(Graph graph, int starts[], int goals[]){
 26 | 		this.goals = goals;
 27 | 		this.graph = graph;
 28 | 		this.starts = starts;
 29 | 	}
 30 | 	
 31 | 	protected int[][] solve(double timeLimit){
 32 | 		long time = System.currentTimeMillis();
 33 | 		long minutes = 0;
 34 | 		callback = this;
 35 | 
 36 | 		// Get minimal time steps to start
 37 | 		// Check minimal distances necessary for solving the problem
 38 | 		Path paths[] = PathFinder.findShortestPaths(graph, starts, goals);
 39 | 		for(int i = 0; i < paths.length; i ++){
 40 | 			if(paths[i].vertexList.size() - 1 > startTimeSteps){
 41 | 				startTimeSteps = paths[i].vertexList.size() - 1;
 42 | 			}
 43 | 		}
 44 | 
 45 | 		
 46 | 		while(true){
 47 | 			updateReachableSets(graph, starts, goals, startTimeSteps);
 48 | //			System.out.println("Working with T = " + startTimeSteps);
 49 | 			Thread firstThread = createThread(1);
 50 | 			Thread secondThread = createThread(2);
 51 | 			firstThread.start();
 52 | 			secondThread.start();
 53 | 			
 54 | 			while(!optimizationDone){
 55 | 				try {
 56 | 					Thread.sleep(1000);
 57 | 					// Check time limit
 58 | 					if(timeLimit > 0 && (System.currentTimeMillis() - time)/1000. > timeLimit){
 59 | 						optimizationDone = true; 
 60 | 						return null;
 61 | 					}
 62 | 					
 63 | 					if(failedAttempt){
 64 | 						optimizationDone = true;
 65 | 						while(!allThreadFinished){
 66 | 							// Wait for other threads to finish
 67 | 							try {
 68 | 								Thread.sleep(100);
 69 | 							} catch (InterruptedException e) {
 70 | 								e.printStackTrace();
 71 | 							}
 72 | 						}
 73 | 						allThreadFinished = false;
 74 | 						failedAttempt = false;
 75 | 						break;
 76 | 					}
 77 | 					
 78 | 					// Print status
 79 | 					long min = (System.currentTimeMillis() - time)/60000;
 80 | 					if(min > minutes){
 81 | 						minutes = min;
 82 | 						System.out.print(".");
 83 | 					}
 84 | 				} catch (InterruptedException e) {
 85 | 					e.printStackTrace();
 86 | 				}
 87 | 			}
 88 | 			
 89 | 			while(!flag1 || !flag2){
 90 | 				try {
 91 | 					Thread.sleep(100);
 92 | 				} catch (InterruptedException e) {
 93 | 					e.printStackTrace();
 94 | 				}
 95 | 			}
 96 | 			flag1 = flag2 = false;
 97 | 			
 98 | 			if(sol1 == null && sol2 == null){
 99 | 				optimizationDone = false;
100 | 				startTimeSteps ++;
101 | 			}
102 | 			else{
103 | 				System.out.println();
104 | 				int[][] sol = goodSolution == 1? sol1 : sol2;
105 | 				return sol;
106 | 			}
107 | 		}
108 | 	}
109 | 
110 | 	private Thread createThread(final int instance){
111 | 		return new Thread(
112 | 				new Runnable(){
113 | 					@Override
114 | 					public void run(){
115 | 						MultiagentGraphSolverGurobiTimePuzzle solver = new MultiagentGraphSolverGurobiTimePuzzle();
116 | 						solver.instance = instance;
117 | 						try {
118 | 							int[][] sol = solver.solve(graph, starts, goals, instance == 1 ? false : true, callback, startTimeSteps, reachableVertices);
119 | 							if(solver.instance == 1){
120 | 								sol1 = sol;
121 | 								flag1 = true;
122 | 							}
123 | 							else{
124 | 								sol2 = sol;
125 | 								flag2 = true;
126 | 							}
127 | 						} catch (GRBException e) {
128 | 							e.printStackTrace();
129 | 						}
130 | 					};
131 | 				});
132 | 	}
133 | 	
134 | 	private void updateReachableSets(Graph graph, int starts[], int goals[], int startTimeSteps){
135 | 		/**
136 | 		 *  Preparation. First compute whether a vertex is reachable by an agent at a time step.
137 | 		 *  For each agent with start x_i and goal x_g, a vertex v at time t is reachable if  
138 | 		 *  dist(x_i, v) <=  t and dist(v, x_g) <= T - t.   
139 | 		 */
140 | 		reachableVertices = new boolean[starts.length][graph.vertices.length][startTimeSteps + 1];
141 | 		for(int a = 0; a < starts.length; a ++){
142 | 			// Compute the distances from start/goal to all vertices of the graph   
143 | 			int[] distFromStart = PathFinder.getDistancesToAllVertices(graph, starts[a], null); 
144 | 			int[] distFromGoal = PathFinder.getDistancesToAllVertices(graph, goals[a], null);
145 | 			
146 | 			// Assign reachability values
147 | 			for(int t = 0; t <= startTimeSteps; t ++){
148 | 				for(int v = 0; v < graph.vertices.length; v ++){
149 | 					if(distFromStart[v] <= t && distFromGoal[v] <= startTimeSteps - t){
150 | 						reachableVertices[a][v][t] = true;
151 | 					}
152 | 					else{
153 | 						reachableVertices[a][v][t] = false;
154 | 					}
155 | 				}
156 | 			}
157 | 		}
158 | 		
159 | 	}
160 | 	
161 | 	
162 | 	@Override
163 | 	protected void callback() {
164 | 		if (where == GRB.CB_MESSAGE) {
165 | 			if(optimizationDone == true){
166 | 				allThreadFinished = true;
167 | 				abort();
168 | 			}
169 | //			else{
170 | //				System.out.println("\nGot new message... \n");
171 | //			}
172 | 		} 
173 | 
174 | 	}
175 | 
176 | 
177 | 	public static int[][] solve(Problem p, double timeLimit){
178 | 		// Solve the problem
179 | 		int paths[][] = null;
180 | 		if(p.sg[0].length > 16){
181 | 			// 25 Puzzle is best solved using a mixture of methods. 
182 | 			PuzzleSolver solver = new PuzzleSolver(p.graph, p.sg[0], p.sg[1]);
183 | 			paths = solver.solve(timeLimit);
184 | 		}
185 | 		else{
186 | 			PathPlanner ms = new PathPlanner();
187 | 			paths = ms.planPathsAdvanced(p.graph, p.sg[0], p.sg[1], false, 0, true, timeLimit);
188 | 		}
189 | 		return paths;
190 | 	}
191 | 	
192 | 
193 | }
194 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/PathFinder.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import java.util.HashMap;
  4 | import java.util.HashSet;
  5 | import java.util.LinkedList;
  6 | import java.util.List;
  7 | import java.util.Map;
  8 | import java.util.Set;
  9 | import java.util.SortedMap;
 10 | import java.util.TreeMap;
 11 | import java.util.Vector;
 12 | 
 13 | import projects.multipath.advanced.Graph;
 14 | import projects.multipath.advanced.Path;
 15 | 
 16 | public class PathFinder {
 17 | 
 18 | 	public static boolean bHeuristic = false;
 19 | 
 20 | 	/**
 21 | 	 * Compute distance of one vertex to all other vertices of the graph
 22 | 	 * 
 23 | 	 * @param g
 24 | 	 * @param start
 25 | 	 * @return
 26 | 	 */
 27 | 	public static int[] getDistancesToAllVertices(Graph g, int start,
 28 | 			int[] inputDistances) {
 29 | 		Set<Integer> visitedVertexIds = new HashSet<Integer>();
 30 | 		List<Integer> queue = new LinkedList<Integer>();
 31 | 		queue.add(start);
 32 | 		int[] distances;
 33 | 		if (inputDistances == null) {
 34 | 			distances = new int[g.vertices.length];
 35 | 		} else {
 36 | 			distances = inputDistances;
 37 | 		}
 38 | 		distances[start] = 0;
 39 | 		while (queue.size() > 0) {
 40 | 			int vertex = queue.remove(0);
 41 | 			Integer[] adjacentVertices = g.adjacencySetMap.get(vertex).toArray(
 42 | 					new Integer[0]);
 43 | 			for (int i = 0; i < adjacentVertices.length; i++) {
 44 | 				if (!visitedVertexIds.contains(adjacentVertices[i])) {
 45 | 					visitedVertexIds.add(adjacentVertices[i]);
 46 | 					queue.add(adjacentVertices[i]);
 47 | 					// This vertex has not been visited, set initial distance
 48 | 					if (adjacentVertices[i] != start)
 49 | 						distances[adjacentVertices[i]] = distances[vertex] + 1;
 50 | 				} else {
 51 | 					// Update distance if shorter
 52 | 					if (distances[adjacentVertices[i]] > distances[vertex] + 1) {
 53 | 						distances[adjacentVertices[i]] = distances[vertex] + 1;
 54 | 					}
 55 | 				}
 56 | 			}
 57 | 		}
 58 | 		return distances;
 59 | 	}
 60 | 	
 61 | 	protected static Set<Integer> findGoalsInDistanceRange(Graph g, int start, int dMin, int dMax){
 62 | 		Set<Integer> vSet = new HashSet<Integer>();
 63 | 		int distance = 1;
 64 | 		Set<Integer> visitedSet = new HashSet<Integer>();
 65 | 		Set<Integer> currentSet = new HashSet<Integer>();
 66 | 		Set<Integer> toBeVisitedSet = new HashSet<Integer>();
 67 | 		currentSet.add(start);
 68 | 		visitedSet.add(start);
 69 | 		while(distance <= dMax && visitedSet.size() < g.vertices.length){
 70 | 			Integer vIds[] = currentSet.toArray(new Integer[0]);
 71 | 			for(int i = 0; i < vIds.length; i ++){
 72 | 				int vId = vIds[i];
 73 | 				Set<Integer> nbrs = g.adjacencySetMap.get(vId);
 74 | 				if (nbrs != null) {
 75 | 					Integer[] neighborIds = nbrs.toArray(new Integer[0]);
 76 | 					for (int n = 0; n < neighborIds.length; n++) {
 77 | 						if (neighborIds[n] != vId) {
 78 | 							if(!visitedSet.contains(neighborIds[n])){
 79 | 								toBeVisitedSet.add(neighborIds[n]);
 80 | 								if(distance >= dMin){
 81 | 									vSet.add(neighborIds[n]);
 82 | 								}
 83 | 							}
 84 | 						}
 85 | 					}
 86 | 				}
 87 | 			}
 88 | 			distance ++;
 89 | 			currentSet = toBeVisitedSet; 
 90 | 			toBeVisitedSet =  new HashSet<Integer>();
 91 | 			visitedSet.addAll(currentSet);
 92 | 		}
 93 | 		return vSet;
 94 | 	}
 95 | 
 96 | 	/**
 97 | 	 * A* search for all naive paths
 98 | 	 * 
 99 | 	 * @param g
100 | 	 * @param start
101 | 	 * @param goal
102 | 	 * @return
103 | 	 */
104 | 	protected static Path[] findShortestPaths(Graph g, int[] start, int[] goal) {
105 | //		for (Vertex v : g.vertices) {
106 | //			v.usedInPath = 0;
107 | //		}
108 | 		Path[] paths = new Path[start.length];
109 | 		for (int i = 0; i < paths.length; i++) {
110 | 			paths[i] = findOneShortestPath(g, start[i], goal[i], null);
111 | 		}
112 | 		return paths;
113 | 	}
114 | 
115 | 	/**
116 | 	 * A* search
117 | 	 * 
118 | 	 * @param g
119 | 	 * @param start
120 | 	 * @param goal
121 | 	 * @return
122 | 	 */
123 | 	protected static Path findOneShortestPath(Graph g, int start, int goal,
124 | 			Set<Integer> alreadyVisitedVertices) {
125 | 		// Check whether start == goal
126 | 		if (start == goal) {
127 | 			Path p = new Path();
128 | 			p.addVertex(start);
129 | 			return p;
130 | 		}
131 | 
132 | 		// House keeping
133 | 		Set<Integer> visitedVertices = null;
134 | 		if (alreadyVisitedVertices != null) {
135 | 			visitedVertices = alreadyVisitedVertices;
136 | 		} else {
137 | 			visitedVertices = new HashSet<Integer>();
138 | 		}
139 | 		SortedMap<Integer, List<Integer>> queueMap = new TreeMap<Integer, List<Integer>>();
140 | 		Map<Integer, Integer> parentMap = new HashMap<Integer, Integer>();
141 | 
142 | 		// Add root
143 | 		addToQueue(queueMap, g, start, goal);
144 | 
145 | 		boolean pathFound = false;
146 | 		// BFS graph using heuristic
147 | 		while (!queueMap.isEmpty()) {
148 | 			int vId = deQueue(queueMap);
149 | 			Set<Integer> is = g.adjacencySetMap.get(vId);
150 | 			if (is != null) {
151 | //				SortedMap<Integer, Integer> nMap = new TreeMap<Integer, Integer>();
152 | //				for (int nVId : is) {
153 | //					if (nVId != vId) {
154 | //						nMap.put(g.vertices[nVId].usedInPath*1000000 + (int)(Math.random()*10)*10000 + nVId, nVId);
155 | //					}
156 | //				}
157 | //
158 | //				Integer[] nVIds = nMap.values().toArray(new Integer[0]);
159 | //				for(int nVId: nVIds){
160 | //					if (!visitedVertices.contains(nVId)) {
161 | //						parentMap.put(nVId, vId);
162 | //						if (nVId == goal) {
163 | //							queueMap.clear();
164 | //							pathFound = true;
165 | //							break;
166 | //						}
167 | //						addToQueue(queueMap, g, nVId, goal);
168 | //						visitedVertices.add(nVId);
169 | //					}
170 | //				}
171 | 				Integer[] neighborIds = is.toArray(new Integer[0]);
172 | 				Vector<Integer> nVec = new Vector<Integer>();
173 | 				for (int i = 0; i < neighborIds.length; i++) {
174 | 					if (neighborIds[i] != vId) {
175 | 						nVec.add(neighborIds[i]);
176 | 					}
177 | 				}
178 | 				while (nVec.size() > 0) {
179 | 					int index = (int) (nVec.size() * Math.random());
180 | 					int nid = nVec.get(index);
181 | 					nVec.remove(index);
182 | 					if (!visitedVertices.contains(nid)) {
183 | 						parentMap.put(nid, vId);
184 | 						if (nid == goal) {
185 | 							queueMap.clear();
186 | 							pathFound = true;
187 | 							break;
188 | 						}
189 | 						addToQueue(queueMap, g, nid, goal);
190 | 						visitedVertices.add(nid);
191 | 					}
192 | 				}
193 | 			}
194 | 		}
195 | 
196 | 		if (pathFound) {
197 | 			// Recover path
198 | 			Path p = new Path();
199 | 			p.insertVertex(goal);
200 | //			g.vertices[goal].usedInPath ++;
201 | 			int lastVertex = goal;
202 | 			while (true) {
203 | 				Integer parent = parentMap.get(lastVertex);
204 | 				if (parent != null) {
205 | 					p.insertVertex(parent);
206 | //					g.vertices[parent].usedInPath ++;
207 | 					lastVertex = parent;
208 | 					if (lastVertex == start) {
209 | 						break;
210 | 					}
211 | 				}
212 | 			}
213 | 			return p;
214 | 		}
215 | 
216 | //		if (pathFound) {
217 | //			// Recover path
218 | //			Path p = new Path();
219 | //			p.insertVertex(goal);
220 | //			int lastVertex = goal;
221 | //			while (true) {
222 | //				Integer parent = parentMap.get(lastVertex);
223 | //				if (parent != null) {
224 | //					p.insertVertex(parent);
225 | //					lastVertex = parent;
226 | //					if (lastVertex == start) {
227 | //						break;
228 | //					}
229 | //				}
230 | //			}
231 | //			return p;
232 | //		}
233 | 		return null;
234 | 	}
235 | 
236 | 	/**
237 | 	 * Add to queue
238 | 	 * 
239 | 	 * @param queueMap
240 | 	 * @param g
241 | 	 * @param vId
242 | 	 * @param target
243 | 	 */
244 | 	private static void addToQueue(SortedMap<Integer, List<Integer>> queueMap,
245 | 			Graph g, int vId, int target) {
246 | 		int dist = 0; // g.getHeuristicDist(vId, target);
247 | 		List<Integer> list = queueMap.get(dist);
248 | 		if (list == null) {
249 | 			list = new LinkedList<Integer>();
250 | 			queueMap.put(dist, list);
251 | 		}
252 | //		list.add(vId);
253 | 		if (!bHeuristic) {
254 | 			list.add(vId);
255 | 		} else {
256 | 			list.add((int) (Math.random() * list.size()), vId);
257 | 		}
258 | 	}
259 | 
260 | 	/**
261 | 	 * Dequeue
262 | 	 * 
263 | 	 * @param queueMap
264 | 	 * @return
265 | 	 */
266 | 	private static int deQueue(SortedMap<Integer, List<Integer>> queueMap) {
267 | 		int minKey = queueMap.firstKey();
268 | 		List<Integer> list = queueMap.get(queueMap.firstKey());
269 | 		int ret = list.remove(0);
270 | 		if (list.size() == 0) {
271 | 			queueMap.remove(minKey);
272 | 		}
273 | 		return ret;
274 | 	}
275 | 
276 | 	/**
277 | 	 * 
278 | 	 * @param paths
279 | 	 * @return
280 | 	 */
281 | 	public static int getTotalDistance(int[][] paths){
282 | 		int dist = 0;
283 | 		for(int a = 0; a < paths.length; a++){
284 | 			for(int i = 0; i < paths[a].length - 1; i ++){
285 | 				if(paths[a][i] != paths[a][i+1]){
286 | 					dist++;
287 | 				}
288 | 			}
289 | 		}
290 | 		return dist;
291 | 	}
292 | 
293 | 	/**
294 | 	 * 
295 | 	 * @param hp
296 | 	 * @return
297 | 	 */
298 | 	public static int getTotalDistance(Path[] hp){
299 | 		int dist = 0;
300 | 		for(int a = 0; a < hp.length; a++){
301 | 			dist += (hp[a].vertexList.size() - 1);
302 | 		}
303 | 		return dist;
304 | 	}
305 | 	
306 | 	public static int getMakespanLowerBound(Graph g, int start[], int goal[]){
307 | 		Path[] hp = PathFinder.findShortestPaths(g, start, goal);
308 | 		int makespanLb = 0;
309 | 		for(int i = 0; i < hp.length; i ++){
310 | 			if(hp[i].vertexList.size() > makespanLb){
311 | 				makespanLb = hp[i].vertexList.size();
312 | 			}
313 | 		}
314 | 		return makespanLb;
315 | 		
316 | 	}
317 | 
318 | 	public static int getTotalTimeLowerBound(Graph g, int start[], int goal[]){
319 | 		Path[] hp = PathFinder.findShortestPaths(g, start, goal);
320 | 		int ttLB = 0;
321 | 		for(int i = 0; i < hp.length; i ++){
322 | 			ttLB += (hp[i].vertexList.size() - 1);
323 | 		}
324 | 		return ttLB;
325 | 		
326 | 	}
327 | }
328 | 
329 | 


--------------------------------------------------------------------------------
/source/projects/multipath/advanced/Problem.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.advanced;
  2 | 
  3 | import java.io.File;
  4 | import java.io.FileInputStream;
  5 | import java.io.FileNotFoundException;
  6 | import java.io.FileOutputStream;
  7 | import java.io.IOException;
  8 | import java.io.ObjectInputStream;
  9 | import java.io.ObjectOutputStream;
 10 | import java.io.PrintWriter;
 11 | import java.io.Serializable;
 12 | import java.util.Map;
 13 | import java.util.Set;
 14 | 
 15 | public class Problem implements Serializable {
 16 | 	private static String FILE_FOLDER = "D:\\temp\\data\\dmrpp\\"; 
 17 | 	
 18 | 	private static final long serialVersionUID = 1L;
 19 | 	
 20 | 	public Graph graph = null;
 21 | 	public int sg[][] = null;
 22 | 	
 23 | 	public  Map<Integer, Integer> vidNewIdMap = null;
 24 | 	public  Map<Integer, Integer> newIdVidMap = null;
 25 | 
 26 | 	public Problem() {}
 27 | 	
 28 | 	public Problem(Graph graph, int[][] sg) {
 29 | 		super();
 30 | 		this.graph = graph;
 31 | 		this.sg = sg;
 32 | 	}
 33 | 	
 34 | 	
 35 | 	/**
 36 | 	 * Write the graph in plain text to a file. The file consists the follwoing lines
 37 | 	 * 1. Vertices in the format vid:x:y, separated by space. Note that x, y may be empty
 38 | 	 * 2. The set of edges in v1:v2 format, separated by space
 39 | 	 * 3. The start & goal locations in the format svid:gvid, separated by space
 40 | 	 * @param fileName
 41 | 	 */
 42 | 	private void writeToFile(String fileName){
 43 | 		try{
 44 | 			// Create directory structure if needed
 45 | 			File file = new File(fileName);
 46 | 			file.getParentFile().mkdirs();
 47 | 			
 48 | 			// Open printwriter 
 49 | 			PrintWriter pw = new PrintWriter(file);
 50 | 			StringBuffer sbuf = new StringBuffer(); 
 51 | 			
 52 | 			// Write out the vertices
 53 | 			for(int i = 0; i <graph.vertices.length; i ++){
 54 | 				if(i > 0)pw.print(" ");
 55 | 				pw.print(graph.vertices[i].id + ":" + graph.vertices[i].getIX()+ ":" + graph.vertices[i].getIY());
 56 | 				
 57 | 				// Append edges to buffer
 58 | 				Integer[] nbrs = graph.adjacencySetMap.get(graph.vertices[i].id).toArray(new Integer[0]);
 59 | 				for(int j = 0; j < nbrs.length; j ++){
 60 | 					if(sbuf.length() > 0)sbuf.append(" ");
 61 | 					sbuf.append(graph.vertices[i].id+":"+nbrs[j]);
 62 | 				}
 63 | 			}
 64 | 			pw.println();
 65 | 			
 66 | 			// Write the edges
 67 | 			pw.println(sbuf);
 68 | 			
 69 | 			// Write the starts
 70 | 			for(int i = 0; i < sg[0].length; i ++){
 71 | 				if(i > 0){pw.print(" ");}
 72 | 				pw.print(sg[0][i]+":"+sg[1][i]);
 73 | 			}
 74 | 			pw.flush();
 75 | 			pw.close();
 76 | 		}
 77 | 		catch(IOException e){}
 78 | 	}
 79 | 	
 80 | 	
 81 | 	public static void writeToFile(Problem p, String fileName){
 82 | 		ObjectOutputStream out;
 83 | 		try {
 84 | 			out = new ObjectOutputStream(new FileOutputStream(fileName));
 85 | 			out.writeObject(p.graph);
 86 | 			out.writeObject(p.sg);
 87 | 			out.close();
 88 | 		} catch (FileNotFoundException e) {
 89 | 			// TODO Auto-generated catch block
 90 | 			e.printStackTrace();
 91 | 		} catch (IOException e) {
 92 | 			// TODO Auto-generated catch block
 93 | 			e.printStackTrace();
 94 | 		}
 95 | 	}
 96 | 	
 97 | 	public static Problem readFromFile(String fileName){
 98 | 		ObjectInputStream in;
 99 | 		Problem p = new Problem();
100 | 		try {
101 | 			in = new ObjectInputStream(new FileInputStream(fileName));
102 | 			p.graph = (Graph)(in.readObject());
103 | 			p.sg = (int[][])(in.readObject());
104 | 			in.close();
105 | 		} catch (ClassNotFoundException e) {
106 | 			// TODO Auto-generated catch block
107 | 			e.printStackTrace();
108 | 		} catch (FileNotFoundException e) {
109 | 			// TODO Auto-generated catch block
110 | 			e.printStackTrace();
111 | 		} catch (IOException e) {
112 | 			// TODO Auto-generated catch block
113 | 			e.printStackTrace();
114 | 		}
115 | 		return p;
116 | 	}
117 | 	
118 | 	/**
119 | 	 * Create a col x row grid problem with fracObs obstacles and n agents
120 | 	 * @param col
121 | 	 * @param row
122 | 	 * @param fracObs
123 | 	 * @param n
124 | 	 * @return
125 | 	 */
126 | 	public static Problem createGridProblem(int col, int row, double fracObs, int n){
127 | 		Problem p = new Problem();
128 | 		p.graph = Graph.createGeneric2DGridGraphWithHoles(col, row, fracObs);
129 | 		p.sg = Graph.getRandomStartGoal(p.graph, n);
130 | 		p.graph = Graph.convertGraph(p.graph, p.sg[0], p.sg[1]);
131 | 		return p;
132 | 	}
133 | 
134 | 	/**
135 | 	 * Create a col x row grid problem with fracObs obstacles and n agents
136 | 	 * @param col
137 | 	 * @param row
138 | 	 * @param fracObs
139 | 	 * @param n
140 | 	 * @return
141 | 	 */
142 | 	public static Problem createGridProblem8Connected(int col, int row, double fracObs, int n){
143 | 		Problem p = new Problem();
144 | 		p.graph = Graph.createGeneric2DGridGraphWithHoles8Connected(col, row, fracObs);
145 | 		p.sg = Graph.getRandomStartGoal(p.graph, n);
146 | 		p.graph = Graph.convertGraph(p.graph, p.sg[0], p.sg[1]);
147 | 		return p;
148 | 	}
149 | 
150 | 	/**
151 | 	 * Create a n^-puzzle
152 | 	 * @param n
153 | 	 * @return
154 | 	 */
155 | 	public static Problem createN2Puzzle(int n){
156 | 		Problem p = new Problem();
157 | 		p.graph = Graph.create2DGridGraph(n, n, true);
158 | 		p.sg = Graph.getRandomStartGoalMany(p.graph, n*n);
159 | 		return p;
160 | 	}
161 | 	
162 | 	/**
163 | 	 * Create a n^-puzzle
164 | 	 * @param n
165 | 	 * @return
166 | 	 */
167 | 	public static Problem createN2M1Puzzle(int n){
168 | 		Problem p = new Problem();
169 | 		p.graph = Graph.create2DGridGraph(n, n, true);
170 | 		p.sg = Graph.getRandomStartGoalMany(p.graph, n*n-1);
171 | 		return p;
172 | 	}
173 | 	
174 | 	public static void main(String argv[]){
175 | 		if(argv.length > 0) FILE_FOLDER = argv[0];
176 | 		// Allow the files to close properly
177 | 		try {
178 | 			createN2Puzzles();
179 | 			createCrowdedHardProblems();
180 | 			create32x32Problems();
181 | 			create32x32Problems8Connected();
182 | 			create24x18PerformanceTestingProblems();
183 | 			Thread.sleep(5000);
184 | 		} catch (InterruptedException e) {
185 | 			// TODO Auto-generated catch block
186 | 			e.printStackTrace();
187 | 		}
188 | 	}
189 | 	
190 | 	
191 | 	protected static void createN2Puzzles(){
192 | 		for(int i = 0; i < 100; i++){
193 | 			for(int n = 3; n < 6; n ++){
194 | 				Problem p = Problem.createN2Puzzle(n);
195 | 				p.writeToFile(FILE_FOLDER + "\\n2puzzle\\" + (n*n) + "-puzzle-" + (1001 + i) + ".txt");
196 | 				Problem.writeToFile(p, FILE_FOLDER + "\\n2puzzle\\" + (n*n) + "-puzzle-" + (1001 + i) + ".dat");
197 | 			}
198 | 		}
199 | 		
200 | 	}
201 | 	
202 | 	protected static void createCrowdedHardProblems(){
203 | 		for(int i = 0; i < 10; i ++){
204 | 			for(int a = 10; a <= 60; a = a + 10){
205 | 				Problem p = Problem.createGridProblem(8, 8, 0, a);
206 | 				p.writeToFile(FILE_FOLDER + "\\8x8-grid\\" + a + "-agts-" + (1001 + i) + ".txt");
207 | 				Problem.writeToFile(p, FILE_FOLDER + "\\8x8-grid\\" + a + "-agts-" + (1001 + i) + ".dat");
208 | 			}
209 | 		}
210 | 		for(int i = 0; i < 10; i ++){
211 | 			for(int a = 10; a <= 250; a = a + 10){
212 | 				Problem p = Problem.createGridProblem(16, 16, 0, a);
213 | 				p.writeToFile(FILE_FOLDER + "\\16x16-grid\\" + a + "-agts-" + (1001 + i) + ".txt");
214 | 				Problem.writeToFile(p, FILE_FOLDER + "\\16x16-grid\\" + a + "-agts-" + (1001 + i) + ".dat");
215 | 			}
216 | 		}
217 | 	}
218 | 
219 | 	
220 | 	protected static void create32x32Problems(){
221 | 		for(int i = 0; i < 10; i ++){
222 | 			for(int n = 10; n <= 200; n=n+10){
223 | 				Problem p = Problem.createGridProblem(32, 32, 0.2, n);
224 | 				p.writeToFile(FILE_FOLDER + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".txt");
225 | 				Problem.writeToFile(p, FILE_FOLDER + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
226 | 			}
227 | 		}
228 | 	}
229 | 
230 | 	protected static void create32x32Problems8Connected(){
231 | 		for(int i = 0; i < 10; i ++){
232 | 			for(int n = 10; n <= 400; n=n+10){
233 | 				Problem p = Problem.createGridProblem8Connected(32, 32, 0.2, n);
234 | 				p.writeToFile(FILE_FOLDER + "\\32x32-grid-8c\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".txt");
235 | 				Problem.writeToFile(p, FILE_FOLDER + "\\32x32-grid-8c\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
236 | 			}
237 | 		}
238 | 	}
239 | 
240 | 	protected static void create24x18PerformanceTestingProblems(){
241 | 		for(int obs = 0; obs < 35; obs=obs+5){
242 | 			for(int n = 10; n <= 300; n=n+10){
243 | 				for(int i = 0; i < 10; i ++){
244 | 					Problem p = Problem.createGridProblem(24, 18, obs/100., n);
245 | 					p.writeToFile(FILE_FOLDER + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".txt");
246 | 					Problem.writeToFile(p, FILE_FOLDER + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
247 | 				}
248 | 			}
249 | 		}
250 | 	}
251 | 	
252 | 	
253 | 	
254 | 	
255 | 	
256 | 	protected static void createCrowdedHardProblems8Connected(){
257 | 		for(int i = 0; i < 10; i ++){
258 | 			for(int a = 10; a < 61; a = a + 10){
259 | 				Problem p = Problem.createGridProblem8Connected(8, 8, 0, a);
260 | 				Problem.writeToFile(p, FILE_FOLDER + "8x8-grid-8c\\" + a + "-agts-" + (1001 + i) + ".txt");
261 | 			}
262 | 		}
263 | 		for(int i = 0; i < 10; i ++){
264 | 			for(int a = 10; a < 251; a = a + 10){
265 | 				Problem p = Problem.createGridProblem8Connected(16, 16, 0, a);
266 | 				Problem.writeToFile(p, FILE_FOLDER + "16x16-grid-8c\\" + a + "-agts-" + (1001 + i) + ".txt");
267 | 			}
268 | 		}
269 | 	}
270 | 
271 | 	protected static Problem getA9Puzzle(){
272 | 		Graph g = new Graph();
273 | 		g.addVertex(0, new int[]{0, 1, 3});
274 | 		g.addVertex(1, new int[]{0, 1, 2, 4});
275 | 		g.addVertex(2, new int[]{1, 2, 5});
276 | 		g.addVertex(3, new int[]{0, 3, 4, 6});
277 | 		g.addVertex(4, new int[]{1, 3, 4, 5, 7});
278 | 		g.addVertex(5, new int[]{2, 4, 5, 8});
279 | 		g.addVertex(6, new int[]{3, 6, 7});
280 | 		g.addVertex(7, new int[]{4, 6, 7, 8});
281 | 		g.addVertex(8, new int[]{5, 7, 8});
282 | 		g.finishBuildingGraph();
283 | 		int sg[][] = new int[][]{{3, 0, 2, 8, 1, 4, 7, 5, 6},{0, 1, 2, 3, 4, 5, 6, 7, 8}};
284 | 		
285 | 		Problem p = new Problem();
286 | 		p.graph = g;
287 | 		p.sg = sg;
288 | 		return p;
289 | 	}
290 | 	
291 | 	public static Problem getLongStraightWithOneGarageProblem(){
292 | 		Graph g = new Graph();
293 | 		g.addVertex(0, new int[]{0, 1});
294 | 		g.addVertex(1, new int[]{0, 1, 2});
295 | 		g.addVertex(2, new int[]{1, 2, 3});
296 | 		g.addVertex(3, new int[]{2, 3, 4});
297 | 		g.addVertex(4, new int[]{3, 4, 5, 9});
298 | 		g.addVertex(5, new int[]{4, 5, 6});
299 | 		g.addVertex(6, new int[]{5, 6, 7});
300 | 		g.addVertex(7, new int[]{6, 7, 8});
301 | 		g.addVertex(8, new int[]{7, 8});
302 | 		g.addVertex(9, new int[]{4, 9});
303 | 		g.finishBuildingGraph();
304 | 		int sg[][] = new int[][]{{0, 8},{8, 0}};
305 | 		
306 | 		Problem p = new Problem();
307 | 		p.graph = g;
308 | 		p.sg = sg;
309 | 		return p;
310 | 	}
311 | }
312 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/MultiagentGraphSolverGurobiTimePuzzle.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import gurobi.GRB;
  4 | import gurobi.GRBEnv;
  5 | import gurobi.GRBException;
  6 | import gurobi.GRBLinExpr;
  7 | import gurobi.GRBModel;
  8 | import gurobi.GRBVar;
  9 | 
 10 | import java.util.HashSet;
 11 | import java.util.Set;
 12 | import java.util.SortedMap;
 13 | import java.util.TreeMap;
 14 | import java.util.Vector;
 15 | 
 16 | import projects.multipath.advanced.Graph;
 17 | 
 18 | public class MultiagentGraphSolverGurobiTimePuzzle{
 19 | 
 20 | 	private SortedMap<Long, GRBVar> edgeVarMap = new TreeMap<Long, GRBVar>();
 21 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 22 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 23 | 	private SortedMap<Long, Vector<GRBVar>> avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 24 | 	private SortedMap<Long, Vector<GRBVar>> avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 25 | 	private SortedMap<Long, Vector<GRBVar>> edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 26 | 	protected int instance = 0;
 27 | 	protected boolean interrupted = false;
 28 | 	
 29 | 	protected int[][] solve(Graph graph, int starts[], int goals[], boolean setOptimal, PuzzleSolver callback, int startTimeSteps, boolean[][][] reachableVertices) throws GRBException{
 30 | 		// Create LP solver and solve 
 31 | 		GRBEnv env = new GRBEnv();
 32 | 		// env.set(GRB.DoubleParam.Heuristics, 0.2);
 33 | 		// env.set(GRB.DoubleParam.IntFeasTol, 0.01);
 34 | 		// env.set(GRB.IntParam.Threads, 6);
 35 | 		env.set(GRB.IntParam.OutputFlag, 1);
 36 | 		env.set(GRB.IntParam.LogToConsole, 0);
 37 | 		env.set(GRB.StringParam.LogFile, "C:\\Temp\\25puzzle-instance-" + instance);
 38 | 		// env.set(GRB.IntParam.Presolve, 2);
 39 | 		// env.set(GRB.IntParam.Cuts, 1);
 40 | 		GRBModel model = prepareModel(env, graph, starts, goals, true, setOptimal, startTimeSteps, reachableVertices);
 41 | 		int[][] paths = null;
 42 | 		model.setCallback(callback);
 43 | 		model.optimize();
 44 | 		model.setCallback(null);
 45 | 		if(model.get(GRB.IntAttr.SolCount) > 0 && (int) (model.get(GRB.DoubleAttr.ObjVal)) == starts.length){
 46 | 			paths = retrievePaths(model, graph, starts, startTimeSteps);
 47 | 			callback.goodSolution = instance;
 48 | 			callback.optimizationDone = true;
 49 | 		}
 50 | 		else{
 51 | 			if(model.get(GRB.IntAttr.Status) != GRB.INTERRUPTED) callback.failedAttempt = true;
 52 | 		}
 53 | 		model.dispose();
 54 | 		env.dispose();
 55 | 		return paths;
 56 | 	}
 57 | 	
 58 | 	private long getEdgeId(int a, int v1, int v2, int t) {
 59 | 		return a * 1000000000000L + v1 * 100000000 + v2 * 10000 + t;
 60 | 	}
 61 | 
 62 | 	private long getVertexTimeId(int v, int t) {
 63 | 		return v * 10000 + t;
 64 | 	}
 65 | 
 66 | 	private long getAgentVertexTimeId(int a, int v, int t) {
 67 | 		return a * 1000000000000L + v * 10000 + t;
 68 | 	}
 69 | 
 70 | 	private long getEdgeTimeId(int v1, int v2, int t) {
 71 | 		if(v1 > v2){
 72 | 			int temp = v1; 
 73 | 			v1 = v2;
 74 | 			v2 = temp;
 75 | 		}
 76 | 		return v1 * 1000000000 + v2* 10000 + t;
 77 | 	}
 78 | 
 79 | 	private int[][] retrievePaths(GRBModel model, Graph graph, int starts[], int timeSteps) throws GRBException{
 80 | 		int paths[][] = new int[starts.length][timeSteps + 1];
 81 | 		for(int a = 0; a < starts.length; a ++){
 82 | 			paths[a][0] = starts[a];
 83 | 			for(int t = 1; t <= timeSteps; t ++){
 84 | 				Integer nvs[] = graph.adjacencySetMap.get(paths[a][t-1]).toArray(new Integer[0]);
 85 | 				for(int nv = 0; nv < nvs.length; nv ++){
 86 | 					if(isEdgeVarSetToTrue(model, a, paths[a][t-1], nvs[nv], t-1)){
 87 | 						paths[a][t] = nvs[nv];
 88 | 						break;
 89 | 					}
 90 | 				}
 91 | 			}
 92 | 		}
 93 | 		return paths;
 94 | 	}
 95 | 	
 96 | 	private boolean isEdgeVarSetToTrue(GRBModel model, int a, int v1, int v2, int t) throws GRBException{
 97 | 		long eId = getEdgeId(a, v1, v2, t);
 98 | 		GRBVar var = edgeVarMap.get(eId);
 99 | 		if(var != null && var.get(GRB.DoubleAttr.X) == 1.0){
100 | 			return true;
101 | 		}
102 | 		return false;
103 | 	}
104 | 	
105 | 	private GRBModel prepareModel(GRBEnv env, Graph graph, int starts[], int goals[], boolean integer, boolean setOptimal, int timeSteps, boolean[][][] reachableVertices) throws GRBException{
106 | 		// Reset global storage
107 | 		edgeVarMap = new TreeMap<Long, GRBVar>();
108 | 		vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
109 | 		vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
110 | 		avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
111 | 		avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
112 | 		edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
113 | 		
114 | 		// Setup model
115 | 		GRBModel model = new GRBModel(env);
116 | 
117 | 		// Set up variables for all edges, agent by agent
118 | 		for(int a = 0; a < starts.length; a++){
119 | 			Set<Integer> rVs = new HashSet<Integer>();
120 | 			rVs.add(starts[a]);
121 | 			for(int t = 1; t <= timeSteps; t++){
122 | 				Integer[] rvs = rVs.toArray(new Integer[0]);
123 | 				for(int rv = 0; rv < rvs.length; rv ++){
124 | 					int curV = rvs[rv];
125 | 					Integer[] adjVs = graph.adjacencySetMap.get(curV).toArray(new Integer[0]);
126 | 					for(int i = 0; i < adjVs.length; i ++){
127 | 						int nextV = adjVs[i];
128 | 						if(reachableVertices[a][nextV][t]){
129 | 							rVs.add(nextV);
130 | 							GRBVar x = model.addVar(0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
131 | 							long edgeId = getEdgeId(a, curV, nextV, t - 1);
132 | 							edgeVarMap.put(edgeId, x);
133 | 
134 | 							// Store edges indexed by vertices
135 | 							storeVarForVertices(x, curV, nextV, t - 1, t);
136 | 							storeVarForAVT(x, a, curV, nextV, t - 1, t);
137 | 
138 | 							// Store edges indexed by edges, if the start/goal vertice are different
139 | 							if(curV != nextV){
140 | 								storeVarForEdges(x, curV, nextV, t - 1);
141 | 							}
142 | 						}
143 | 					}
144 | 
145 | 					// Remove current vertex if it's no longer reachable
146 | 					if(!reachableVertices[a][curV][t]){
147 | 						rVs.remove(curV);
148 | 					}
149 | 				}
150 | 			}
151 | 		}
152 | 		model.update();
153 | 		
154 | 		// Special case for last t, only need to setup single edges between respective start/goal
155 | 		GRBLinExpr objExpr = new GRBLinExpr();
156 | 		for(int a = 0; a < starts.length; a ++){
157 | 			long edgeId = getEdgeId(a, goals[a], starts[a], timeSteps);
158 | 			GRBVar x = model.addVar(setOptimal?1.0:0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
159 | 			model.update();
160 | 
161 | 			// Store edges indexed by vertices
162 | 			storeVarForVertices(x, goals[a], starts[a], timeSteps, 0);
163 | 			storeVarForAVT(x, a, goals[a], starts[a], timeSteps, 0);
164 | 			
165 | 			// Setup the objective exprssion 
166 | 			objExpr.addTerm(1, x);
167 | 			edgeVarMap.put(edgeId, x);
168 | 		}
169 | 		model.setObjective(objExpr, GRB.MAXIMIZE);
170 | 		
171 | 
172 | 		// Set up constraints for edges going into a vertex
173 | 		for(int t = 0; t < timeSteps; t++){
174 | 
175 | 			// Set up variables for all edges and expression 
176 | 			for(int v = 0; v < graph.vertices.length; v++){
177 | 				GRBLinExpr expr = null;
178 | 				long vtId = getVertexTimeId(v, t + 1);
179 | 				Vector<GRBVar> inVarVec = vertexTimeInVarVectorMap.get(vtId);
180 | 				if(inVarVec != null){
181 | 					if(expr == null) expr = new GRBLinExpr();
182 | 					for(int i = 0; i < inVarVec.size(); i ++){
183 | 						expr.addTerm(1., inVarVec.get(i));
184 | 					}
185 | 				}
186 | 				if(expr != null){
187 | 					if(expr.size() > 1){
188 | 						model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
189 | 					}
190 | 					expr = null;
191 | 				}
192 | 			}
193 | 		}
194 | 		
195 | 		// Set up constraints for vertices
196 | 		for(int a = 0; a < starts.length; a ++){
197 | 			for(int t = 0; t <= timeSteps; t++){
198 | 				// Set up variables for all edges and expression 
199 | 				for(int v = 0; v < graph.vertices.length; v++){
200 | 					GRBLinExpr expr = null;
201 | 					long vtId = getAgentVertexTimeId(a, v, t);
202 | 					Vector<GRBVar> inVarVec = avtInVarVectorMap.get(vtId);
203 | 					if(inVarVec != null){
204 | 						expr = new GRBLinExpr();
205 | 						for(int i = 0; i < inVarVec.size(); i ++){
206 | 							expr.addTerm(-1., inVarVec.get(i));
207 | 						}
208 | 					}
209 | 					Vector<GRBVar> outVarVec = avtOutVarVectorMap.get(vtId);
210 | 					if(outVarVec != null){
211 | 						if(expr == null) expr = new GRBLinExpr();
212 | 						for(int i = 0; i < outVarVec.size(); i ++){
213 | 							expr.addTerm(1., outVarVec.get(i));
214 | 						}
215 | 					}
216 | 					if(expr != null){
217 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
218 | 						expr = null;
219 | 					}
220 | 				}
221 | 			}
222 | 		}
223 | 		
224 | 		// Setup constraint for single edges
225 | 		Long[] edgeTimeKeys = edgeTimeVarVectorMap.keySet().toArray(new Long[0]);
226 | 		for(int e = 0; e < edgeTimeKeys.length; e ++){
227 | 			GRBLinExpr expr = null;
228 | 			Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(edgeTimeKeys[e]);
229 | 			if(varVec != null){
230 | 				if(expr == null) expr = new GRBLinExpr();
231 | 				for(int i = 0; i < varVec.size(); i ++){
232 | 					expr.addTerm(1., varVec.get(i));
233 | 				}
234 | 			}
235 | 			if(expr != null && expr.size() > 1){
236 | 				model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
237 | 			}
238 | 			expr = null;
239 | 		}
240 | 		model.update();
241 | 		
242 | 		return model;
243 | 	}
244 | 	
245 | 	private void storeVarForAVT(GRBVar var, int a, int v1, int v2, int t1, int t2){
246 | 		// Add edge var to vertex time map, outgoing first
247 | 		long vtId = getAgentVertexTimeId(a, v1, t1);
248 | 		Vector<GRBVar> varVec = avtOutVarVectorMap.get(vtId);
249 | 		if(varVec == null){
250 | 			varVec = new Vector<GRBVar>();
251 | 			avtOutVarVectorMap.put(vtId, varVec);
252 | 		}
253 | 		varVec.add(var);
254 | 		
255 | 		// Incoming
256 | 		vtId = getAgentVertexTimeId(a, v2, t2);
257 | 		varVec = avtInVarVectorMap.get(vtId);
258 | 		if(varVec == null){
259 | 			varVec = new Vector<GRBVar>();
260 | 			avtInVarVectorMap.put(vtId, varVec);
261 | 		}
262 | 		varVec.add(var);
263 | 	}
264 | 
265 | 	/**
266 | 	 * Store edge var for head and tail vertices
267 | 	 * @param var
268 | 	 * @param v1
269 | 	 * @param v2
270 | 	 * @param t1
271 | 	 * @param t2
272 | 	 */
273 | 	private void storeVarForVertices(GRBVar var, int v1, int v2, int t1, int t2){
274 | 		// Add edge var to vertex time map, outgoing first
275 | 		long vtId = getVertexTimeId(v1, t1);
276 | 		Vector<GRBVar> varVec = vertexTimeOutVarVectorMap.get(vtId);
277 | 		if(varVec == null){
278 | 			varVec = new Vector<GRBVar>();
279 | 			vertexTimeOutVarVectorMap.put(vtId, varVec);
280 | 		}
281 | 		varVec.add(var);
282 | 		
283 | 		// Incoming
284 | 		vtId = getVertexTimeId(v2, t2);
285 | 		varVec = vertexTimeInVarVectorMap.get(vtId);
286 | 		if(varVec == null){
287 | 			varVec = new Vector<GRBVar>();
288 | 			vertexTimeInVarVectorMap.put(vtId, varVec);
289 | 		}
290 | 		varVec.add(var);
291 | 	}
292 | 
293 | 	/**
294 | 	 * Store edge var for one edge in the original graph with different time stamps
295 | 	 * @param var
296 | 	 * @param v1
297 | 	 * @param v2
298 | 	 * @param t
299 | 	 */
300 | 	private void storeVarForEdges(GRBVar var, int v1, int v2, int t){
301 | 		// Add edge var to edge time map
302 | 		long etId = getEdgeTimeId(v1, v2, t);
303 | 		Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(etId);
304 | 		if(varVec == null){
305 | 			varVec = new Vector<GRBVar>();
306 | 			edgeTimeVarVectorMap.put(etId, varVec);
307 | 		}
308 | 		varVec.add(var);
309 | 	}
310 | 	
311 | 
312 | }
313 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/MultiagentGraphSolverGurobiDistance.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import gurobi.GRB;
  4 | import gurobi.GRBCallback;
  5 | import gurobi.GRBEnv;
  6 | import gurobi.GRBException;
  7 | import gurobi.GRBLinExpr;
  8 | import gurobi.GRBModel;
  9 | import gurobi.GRBVar;
 10 | 
 11 | import java.util.HashSet;
 12 | import java.util.Set;
 13 | import java.util.SortedMap;
 14 | import java.util.TreeMap;
 15 | import java.util.Vector;
 16 | 
 17 | import projects.multipath.advanced.Graph;
 18 | 
 19 | public class MultiagentGraphSolverGurobiDistance extends GRBCallback{
 20 | 
 21 | 	public static boolean bPrintPaths = false;
 22 | 	public static boolean bDebugInfo = true;
 23 | 	public static boolean bReturnAnySolution = false;
 24 | 	
 25 | 	private SortedMap<Long, GRBVar> edgeVarMap = new TreeMap<Long, GRBVar>();
 26 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 27 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 28 | 	private SortedMap<Long, Vector<GRBVar>> avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 29 | 	private SortedMap<Long, Vector<GRBVar>> avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 30 | 	private SortedMap<Long, Vector<GRBVar>> edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 31 | 	private boolean reachableVertices[][][] = null;
 32 | 	private GRBModel currentModel = null;
 33 | 	private int expectedBound = 0;
 34 | 			
 35 | 	protected int[][] solve(Graph graph, int starts[], int goals[], double gap, int expansion, double timeLimit) throws GRBException{
 36 | 
 37 | 		long startTime = System.currentTimeMillis(); 
 38 | 		updateReachableSets(graph, starts, goals, expansion);
 39 | 		
 40 | 		// Create LP solver
 41 | 		GRBEnv env = new GRBEnv();
 42 | 		env.set(GRB.IntParam.OutputFlag, 0);
 43 | 		env.set(GRB.DoubleParam.MIPGap, gap);
 44 | 
 45 | 		// Solve the ILP
 46 | 		int ps[][] = null;
 47 | 		// timeLimit = timeLimit - (System.currentTimeMillis() - startTime)/1000.0;
 48 | 		if(bDebugInfo){
 49 | 			System.out.print("Trying T = " + expansion + " for the integer model " + ((timeLimit != -1)?("with max time limit " + timeLimit):"") + ", gap=" + gap + " ...");
 50 | 		}
 51 | 		GRBModel model = prepareModel(env, graph, starts, goals, true, expansion, timeLimit);
 52 | 		expectedBound = starts.length;
 53 | 		model.optimize();
 54 | 		double objVal = 0;
 55 | 		if(model.get(GRB.IntAttr.SolCount) > 0){
 56 | 			objVal = (model.get(GRB.DoubleAttr.ObjVal));
 57 | 			double bnd = model.get(GRB.DoubleAttr.ObjBound);
 58 | 			double agap = 1 - bnd/objVal;
 59 | 			
 60 | 			if(agap <= gap || bReturnAnySolution){
 61 | 				if(bDebugInfo){
 62 | 					System.out.println(" done.\n");
 63 | 					System.out.println("Solution with optimal distance " + objVal + " found in " + (System.currentTimeMillis() - startTime)/1000.0 + " seconds.");
 64 | 				}
 65 | 				ps = retrievePaths(model, graph, starts, expansion);
 66 | 			}
 67 | 		}
 68 | 		if(bDebugInfo && ps == null){
 69 | 			System.out.println(" time out.");
 70 | 		}
 71 | 		
 72 | 		model.dispose();
 73 | 		env.dispose();
 74 | 		
 75 | 		return ps;
 76 | 	}
 77 | 	
 78 | 	private void updateReachableSets(Graph graph, int starts[], int goals[], int startTimeSteps){
 79 | 		/**
 80 | 		 *  Preparation. First compute whether a vertex is reachable by an agent at a time step.
 81 | 		 *  For each agent with start x_i and goal x_g, a vertex v at time t is reachable if  
 82 | 		 *  dist(x_i, v) <=  t and dist(v, x_g) <= T - t.   
 83 | 		 */
 84 | 		reachableVertices = new boolean[starts.length][graph.vertices.length][startTimeSteps + 1];
 85 | 		for(int a = 0; a < starts.length; a ++){
 86 | 			// Compute the distances from start/goal to all vertices of the graph   
 87 | 			int[] distFromStart = PathFinder.getDistancesToAllVertices(graph, starts[a], null); 
 88 | 			int[] distFromGoal = PathFinder.getDistancesToAllVertices(graph, goals[a], null);
 89 | 			
 90 | 			// Assign reachability values
 91 | 			for(int t = 0; t <= startTimeSteps; t ++){
 92 | 				for(int v = 0; v < graph.vertices.length; v ++){
 93 | 					if(distFromStart[v] <= t && distFromGoal[v] <= startTimeSteps - t){
 94 | 						reachableVertices[a][v][t] = true;
 95 | 					}
 96 | 					else{
 97 | 						reachableVertices[a][v][t] = false;
 98 | 					}
 99 | 				}
100 | 			}
101 | 		}
102 | 		
103 | 	}
104 | 	
105 | 	private long getEdgeId(int a, int v1, int v2, int t) {
106 | 		return a * 1000000000000L + v1 * 100000000 + v2 * 10000 + t;
107 | 	}
108 | 
109 | 	private long getVertexTimeId(int v, int t) {
110 | 		return v * 10000 + t;
111 | 	}
112 | 
113 | 	private long getAgentVertexTimeId(int a, int v, int t) {
114 | 		return a * 1000000000000L + v * 10000 + t;
115 | 	}
116 | 
117 | 	private long getEdgeTimeId(int v1, int v2, int t) {
118 | 		if(v1 > v2){
119 | 			int temp = v1; 
120 | 			v1 = v2;
121 | 			v2 = temp;
122 | 		}
123 | 		return v1 * 1000000000 + v2* 10000 + t;
124 | 	}
125 | 
126 | 	private int[][] retrievePaths(GRBModel model, Graph graph, int starts[], int timeSteps) throws GRBException{
127 | 		int paths[][] = new int[starts.length][timeSteps + 1];
128 | 		for(int a = 0; a < starts.length; a ++){
129 | 			if(bPrintPaths){
130 | 				System.out.print("Agent " + a + ": 0:");
131 | 				graph.vertices[starts[a]].printVertex();
132 | 			}
133 | 			paths[a][0] = starts[a];
134 | 			for(int t = 1; t <= timeSteps; t ++){
135 | 				Integer nvs[] = graph.adjacencySetMap.get(paths[a][t-1]).toArray(new Integer[0]);
136 | 				for(int nv = 0; nv < nvs.length; nv ++){
137 | 					if(isEdgeVarSetToTrue(model, a, paths[a][t-1], nvs[nv], t-1)){
138 | 						paths[a][t] = nvs[nv];
139 | 						break;
140 | 					}
141 | 				}
142 | 				if(bPrintPaths){
143 | 					System.out.print(" " + t + ":");
144 | 					graph.vertices[paths[a][t]].printVertex();
145 | 				}
146 | 			}
147 | 			if(bPrintPaths)System.out.println();
148 | 		}
149 | 		return paths;
150 | 	}
151 | 	
152 | 	private boolean isEdgeVarSetToTrue(GRBModel model, int a, int v1, int v2, int t) throws GRBException{
153 | 		long eId = getEdgeId(a, v1, v2, t);
154 | 		GRBVar var = edgeVarMap.get(eId);
155 | 		if(var != null && var.get(GRB.DoubleAttr.X) == 1.0){
156 | 			return true;
157 | 		}
158 | 		return false;
159 | 	}
160 | 	
161 | 	private GRBModel prepareModel(GRBEnv env, Graph graph, int starts[], int goals[], boolean integer, int timeSteps, double timeLimit) throws GRBException{
162 | 		// Reset global storage
163 | 		edgeVarMap = new TreeMap<Long, GRBVar>();
164 | 		vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
165 | 		vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
166 | 		avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
167 | 		avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
168 | 		edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
169 | 		
170 | 		// Suppress output if we are solving the relaxation
171 | 		if(bDebugInfo){env.set(GRB.IntParam.OutputFlag, 0);}
172 | 		else{env.set(GRB.IntParam.OutputFlag, 0);}
173 | 
174 | 		// Time limit?
175 | 		if(timeLimit > 0)
176 | 		{ 
177 | 			env.set(GRB.DoubleParam.TimeLimit, timeLimit); 
178 | 		}
179 | 		else{
180 | 			env.set(GRB.DoubleParam.TimeLimit, 10000000000.0);
181 | 		}
182 | 		
183 | 		// Setup model
184 | 		GRBModel model = new GRBModel(env);
185 | 		currentModel = model;
186 | 
187 | 		GRBLinExpr objExpr = new GRBLinExpr();
188 | 
189 | 		// Set up variables for all edges, agent by agent
190 | 		for(int a = 0; a < starts.length; a++){
191 | 			Set<Integer> rVs = new HashSet<Integer>();
192 | 			rVs.add(starts[a]);
193 | 			for(int t = 1; t <= timeSteps; t++){
194 | 				Integer[] rvs = rVs.toArray(new Integer[0]);
195 | 				for(int rv = 0; rv < rvs.length; rv ++){
196 | 					int curV = rvs[rv];
197 | 					Integer[] adjVs = graph.adjacencySetMap.get(curV).toArray(new Integer[0]);
198 | 					for(int i = 0; i < adjVs.length; i ++){
199 | 						int nextV = adjVs[i];
200 | 						if(reachableVertices[a][nextV][t]){
201 | 							rVs.add(nextV);
202 | 							GRBVar x = model.addVar(0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
203 | 							if(curV != nextV){
204 | 								objExpr.addTerm(1.0, x);
205 | 							}
206 | 							long edgeId = getEdgeId(a, curV, nextV, t - 1);
207 | 							edgeVarMap.put(edgeId, x);
208 | 
209 | 							// Store edges indexed by vertices
210 | 							storeVarForVertices(x, curV, nextV, t - 1, t);
211 | 							storeVarForAVT(x, a, curV, nextV, t - 1, t);
212 | 
213 | 							// Store edges indexed by edges, if the start/goal vertice are different
214 | 							if(curV != nextV){
215 | 								storeVarForEdges(x, curV, nextV, t - 1);
216 | 							}
217 | 						}
218 | 					}
219 | 
220 | 					// Remove current vertex if it's no longer reachable
221 | 					if(!reachableVertices[a][curV][t]){
222 | 						rVs.remove(curV);
223 | 					}
224 | 				}
225 | 			}
226 | 		}
227 | 		model.update();
228 | 		model.setObjective(objExpr, GRB.MINIMIZE);
229 | 		
230 | 		// Special case for last t, only need to setup single edges between respective start/goal
231 | 		for(int a = 0; a < starts.length; a ++){
232 | 			long edgeId = getEdgeId(a, goals[a], starts[a], timeSteps);
233 | 			GRBVar x = model.addVar(integer?1.0:0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
234 | 			model.update();
235 | 
236 | 			// Store edges indexed by vertices
237 | 			storeVarForVertices(x, goals[a], starts[a], timeSteps, 0);
238 | 			storeVarForAVT(x, a, goals[a], starts[a], timeSteps, 0);
239 | 			
240 | 			// Setup the objective exprssion 
241 | 			edgeVarMap.put(edgeId, x);
242 | 		}
243 | 		
244 | 
245 | 		// Set up constraints for edges going into a vertex
246 | 		for(int t = 0; t < timeSteps; t++){
247 | 
248 | 			// Set up variables for all edges and expression 
249 | 			for(int v = 0; v < graph.vertices.length; v++){
250 | 				GRBLinExpr expr = null;
251 | 				long vtId = getVertexTimeId(v, t + 1);
252 | 				Vector<GRBVar> inVarVec = vertexTimeInVarVectorMap.get(vtId);
253 | 				if(inVarVec != null){
254 | 					if(expr == null) expr = new GRBLinExpr();
255 | 					for(int i = 0; i < inVarVec.size(); i ++){
256 | 						expr.addTerm(1., inVarVec.get(i));
257 | 					}
258 | 				}
259 | 				if(expr != null){
260 | 					if(expr.size() > 1){
261 | 						model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
262 | 					}
263 | 					expr = null;
264 | 				}
265 | 			}
266 | 		}
267 | 		
268 | 		// Set up constraints for vertices
269 | 		for(int a = 0; a < starts.length; a ++){
270 | 			for(int t = 0; t <= timeSteps; t++){
271 | 				// Set up variables for all edges and expression 
272 | 				for(int v = 0; v < graph.vertices.length; v++){
273 | 					GRBLinExpr expr = null;
274 | 					long vtId = getAgentVertexTimeId(a, v, t);
275 | 					Vector<GRBVar> inVarVec = avtInVarVectorMap.get(vtId);
276 | 					if(inVarVec != null){
277 | 						expr = new GRBLinExpr();
278 | 						for(int i = 0; i < inVarVec.size(); i ++){
279 | 							expr.addTerm(-1., inVarVec.get(i));
280 | 						}
281 | 					}
282 | 					Vector<GRBVar> outVarVec = avtOutVarVectorMap.get(vtId);
283 | 					if(outVarVec != null){
284 | 						if(expr == null) expr = new GRBLinExpr();
285 | 						for(int i = 0; i < outVarVec.size(); i ++){
286 | 							expr.addTerm(1., outVarVec.get(i));
287 | 						}
288 | 					}
289 | 					if(expr != null){
290 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
291 | 						expr = null;
292 | 					}
293 | 				}
294 | 			}
295 | 		}
296 | 		
297 | 		// Setup constraint for single edges
298 | 		Long[] edgeTimeKeys = edgeTimeVarVectorMap.keySet().toArray(new Long[0]);
299 | 		for(int e = 0; e < edgeTimeKeys.length; e ++){
300 | 			GRBLinExpr expr = null;
301 | 			Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(edgeTimeKeys[e]);
302 | 			if(varVec != null){
303 | 				if(expr == null) expr = new GRBLinExpr();
304 | 				for(int i = 0; i < varVec.size(); i ++){
305 | 					expr.addTerm(1., varVec.get(i));
306 | 				}
307 | 			}
308 | 			if(expr != null && expr.size() > 1){
309 | 				model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
310 | 			}
311 | 			expr = null;
312 | 		}
313 | 		model.update();
314 | 		
315 | 		return model;
316 | 	}
317 | 	
318 | 	private void storeVarForAVT(GRBVar var, int a, int v1, int v2, int t1, int t2){
319 | 		// Add edge var to vertex time map, outgoing first
320 | 		long vtId = getAgentVertexTimeId(a, v1, t1);
321 | 		Vector<GRBVar> varVec = avtOutVarVectorMap.get(vtId);
322 | 		if(varVec == null){
323 | 			varVec = new Vector<GRBVar>();
324 | 			avtOutVarVectorMap.put(vtId, varVec);
325 | 		}
326 | 		varVec.add(var);
327 | 		
328 | 		// Incoming
329 | 		vtId = getAgentVertexTimeId(a, v2, t2);
330 | 		varVec = avtInVarVectorMap.get(vtId);
331 | 		if(varVec == null){
332 | 			varVec = new Vector<GRBVar>();
333 | 			avtInVarVectorMap.put(vtId, varVec);
334 | 		}
335 | 		varVec.add(var);
336 | 	}
337 | 
338 | 	/**
339 | 	 * Store edge var for head and tail vertices
340 | 	 * @param var
341 | 	 * @param v1
342 | 	 * @param v2
343 | 	 * @param t1
344 | 	 * @param t2
345 | 	 */
346 | 	private void storeVarForVertices(GRBVar var, int v1, int v2, int t1, int t2){
347 | 		// Add edge var to vertex time map, outgoing first
348 | 		long vtId = getVertexTimeId(v1, t1);
349 | 		Vector<GRBVar> varVec = vertexTimeOutVarVectorMap.get(vtId);
350 | 		if(varVec == null){
351 | 			varVec = new Vector<GRBVar>();
352 | 			vertexTimeOutVarVectorMap.put(vtId, varVec);
353 | 		}
354 | 		varVec.add(var);
355 | 		
356 | 		// Incoming
357 | 		vtId = getVertexTimeId(v2, t2);
358 | 		varVec = vertexTimeInVarVectorMap.get(vtId);
359 | 		if(varVec == null){
360 | 			varVec = new Vector<GRBVar>();
361 | 			vertexTimeInVarVectorMap.put(vtId, varVec);
362 | 		}
363 | 		varVec.add(var);
364 | 	}
365 | 
366 | 	/**
367 | 	 * Store edge var for one edge in the original graph with different time stamps
368 | 	 * @param var
369 | 	 * @param v1
370 | 	 * @param v2
371 | 	 * @param t
372 | 	 */
373 | 	private void storeVarForEdges(GRBVar var, int v1, int v2, int t){
374 | 		// Add edge var to edge time map
375 | 		long etId = getEdgeTimeId(v1, v2, t);
376 | 		Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(etId);
377 | 		if(varVec == null){
378 | 			varVec = new Vector<GRBVar>();
379 | 			edgeTimeVarVectorMap.put(etId, varVec);
380 | 		}
381 | 		varVec.add(var);
382 | 	}
383 | 	
384 | 	@Override
385 | 	protected void callback() {
386 | 		try{
387 | 			if (where == GRB.CB_MIP) {
388 | 				if(expectedBound > getDoubleInfo(GRB.Callback.MIP_OBJBND)){
389 | 					currentModel.terminate();
390 | 				}
391 | 			}
392 | 		} catch (GRBException e) {
393 | 			System.out.println("Error code: " + e.getErrorCode() + ". "
394 | 					+ e.getMessage());
395 | 			e.printStackTrace();
396 | 		}
397 | 
398 | 	}
399 | 
400 | 
401 | }
402 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/HeuristicPathPlanner.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import java.util.HashMap;
  4 | import java.util.HashSet;
  5 | import java.util.Map;
  6 | import java.util.Set;
  7 | import java.util.SortedMap;
  8 | import java.util.TreeMap;
  9 | import java.util.Vector;
 10 | 
 11 | import projects.multipath.advanced.Graph;
 12 | import projects.multipath.advanced.Path;
 13 | import projects.multipath.advanced.Problem;
 14 | import projects.multipath.advanced.Vertex;
 15 | 
 16 | /**
 17 |  * Note that there is a 10000 vertex limitation in the current implementation. 
 18 |  * @author Jingjin Yu
 19 |  *
 20 |  */
 21 | public class HeuristicPathPlanner extends PathPlanner{
 22 | 	
 23 | 	public static void main(String argv[]){
 24 | 		HeuristicPathPlanner pp = new HeuristicPathPlanner();
 25 | 		Problem p = Problem.getLongStraightWithOneGarageProblem();
 26 | 		printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
 27 | 		int start[] = p.sg[0].clone();
 28 | 		int goal[] = p.sg[1].clone();
 29 | 		int[][] paths = pp.planHeuristicPaths(p.graph, p.sg[0], p.sg[1]);
 30 | 		if(paths != null){
 31 | 			printPaths(p.graph, paths);
 32 | 			if(isPathSetValid(paths, p.graph, start, goal)){
 33 | 				System.out.println("Path set is valid.");
 34 | 			}
 35 | 		}
 36 | 	}
 37 | 	
 38 | 	public static void printPaths(Graph g, int [][] paths){
 39 | 		for(int a = 0; a < paths.length; a ++){
 40 | 			System.out.print("Agent " + a + ":");
 41 | 			for(int t = 0; t < paths[0].length; t ++){
 42 | 				System.out.print(" " + t + ":");
 43 | 				g.vertices[paths[a][t]].printVertex();
 44 | 			}
 45 | 			System.out.println();
 46 | 		}
 47 | 	}
 48 | 	
 49 | 	@Override
 50 | 	public int[][] planHeuristicPaths(Graph graph, int start[], int goal[]){
 51 | 		distanceToGoals = new int[start.length][graph.vertices.length];
 52 | 		for(int a = 0; a < goal.length; a ++){
 53 | 			PathFinder.getDistancesToAllVertices(graph, goal[a], distanceToGoals[a]);
 54 | 		}
 55 | 		
 56 | 		int[][] ps = null; 
 57 | 		for(int i = 0; i < 3; i ++){
 58 | 			System.out.println("Attempting with " + (1.5 + (5 + i * 4)*start.length/120.) + " seconds...");
 59 | 			int s[] = start.clone();
 60 | 			int g[] = goal.clone();
 61 | 			ps = planHeuristicPath(graph, s, g, 1.5 + (5 + i * 4)*start.length/120.);
 62 | 			if(ps != null){
 63 | 				 int count = numberOfGoalsReached(ps, goal);
 64 | 				 if(count == goal.length){
 65 | 					 return ps;
 66 | 				 }
 67 | 			}
 68 | 		}
 69 | 		return ps;
 70 | 	}
 71 | 
 72 | 	public static int numberOfGoalsReached(int[][] paths, int goal[]){
 73 | 		int count = 0;
 74 | 		for(int i = 0; i < goal.length; i ++){
 75 | 			if(paths[i][paths[i].length-1] == goal[i]){
 76 | 				count ++;
 77 | 			}
 78 | 		}
 79 | 		return count;
 80 | 	}
 81 | 	
 82 | 	@Override
 83 | 	public int[][] planHeuristicPath(Graph g, int s[], int goal[], double timeLimit){
 84 | 		long time = System.currentTimeMillis(); 
 85 | 		
 86 | 		MultiagentGraphSolverGurobiTime.bPrintPaths = false;
 87 | 		MultiagentGraphSolverGurobiTime.bDebugInfo = false;
 88 | 		int numAgents = s.length;
 89 | 		int start[] = s.clone();
 90 | 
 91 | 		// Print start/goal
 92 | 		// printStartAndGoals(g, start, goal);
 93 | 		
 94 | 		Path[] pathArray = new Path[numAgents];
 95 | 		for(int a = 0; a < numAgents; a ++){
 96 | 			pathArray[a] = new Path();
 97 | 			pathArray[a].addVertex(start[a]);
 98 | 		}
 99 | 		
100 | 		int progressCountStalled = 0;
101 | 		while(true){
102 | 			int beginLength = pathArray[0].vertexList.size();
103 | 			// printStartAndGoals(g, start, goal);
104 | 			
105 | 			// Grab paths and convert to array
106 | 			int[][] paths = parsePaths(PathFinder.findShortestPaths(g, start, goal));
107 | 			if(paths == null || paths[0].length == 1) return parsePaths(pathArray);
108 | 			
109 | 			// Move agents as much as we can
110 | 			int ct = 0;
111 | 			while(ct < paths[0].length && existsEdgeCollision(paths, ct) == null && existsVertexCollision(paths, ct) == null){
112 | 				// No meet or head on, move agents
113 | 				ct ++;
114 | 				moveAgentsIndependentlyOneStep(pathArray, paths, ct);
115 | 				
116 | 				// Are we done?
117 | 				if(ct == paths[0].length - 1) return parsePaths(pathArray);
118 | 			}
119 | 			
120 | 			// printPaths(g, parsePaths(pathArray));
121 | 			
122 | 			// Not done, resolve local conflicts. 
123 | 			Vector<Problem> subProbVec = new Vector<Problem>();
124 | 			
125 | 			// Detecting two agents exchanging locations
126 | 			Set<Long> eSet = new HashSet<Long>();
127 | 			for(int a = 0; a < numAgents; a ++){
128 | 				int v1 = paths[a][ct];
129 | 				int v2 = paths[a][ct + 1];
130 | 				long eId = v1 > v2 ? v1*10000 + v2 : v2*10000 + v1;
131 | 				if(eSet.contains(eId)){
132 | 					// Create sub problem
133 | 					subProbVec.add(createSubProblem(g, paths, ct, new Integer[]{v1, v2}, goal, 2));
134 | //					subProbVec.insertElementAt(createSubProblem(g, paths, ct, new Integer[]{v1, v2}, goal, 2),
135 | //							(int)(subProbVec.size()*Math.random()));
136 | 				}
137 | 				else{
138 | 					eSet.add(eId);
139 | 				}
140 | 			}
141 | 			
142 | 			// Detect two conflicting vertices and grow a little distance from the area
143 | 			Set<Integer> vSet = new HashSet<Integer>();
144 | 			Set<Integer> processedVSet = new HashSet<Integer>();
145 | 			for(int a = 0; a < numAgents; a ++){
146 | 				int vId = paths[a][ct + 1];
147 | 				if(vSet.contains(vId) && !processedVSet.contains(vId)){
148 | 					// Get conflicting vIds
149 | 					Vector<Integer> cVIdSet = new Vector<Integer>();
150 | 					for(int aa = 0; aa < numAgents; aa++){
151 | 						if(paths[aa][ct + 1] == vId){
152 | 							cVIdSet.add(paths[aa][ct]);
153 | 						}
154 | 					}
155 | 					
156 | 					// Create sub problem
157 | 					subProbVec.add(createSubProblem(g, paths, ct, cVIdSet.toArray(new Integer[0]), goal, 2));
158 | //					subProbVec.insertElementAt(createSubProblem(g, paths, ct, cVIdSet.toArray(new Integer[0]), goal, 2)
159 | //							,(int)(subProbVec.size()*Math.random()));
160 | 					processedVSet.add(vId);
161 | 				}
162 | 				else{
163 | 					vSet.add(vId);
164 | 				}
165 | 			}
166 | 			
167 | 			// Merge subproblems
168 | 			// subProbVec = mergeSubProblems(subProbVec);
169 | 			
170 | 			// Solve problems that do not interset with previously solved problems
171 | 			boolean progress = false;
172 | 			int toAdd = 0;
173 | 			int startLength = pathArray[0].vertexList.size();
174 | 			Set<Integer> usedVertexSet = new HashSet<Integer>();
175 | 			for(int pi = 0; pi < subProbVec.size(); pi ++){
176 | 				// Do we still have time?
177 | 				double timeLeft = timeLimit - (System.currentTimeMillis() - time)/1000.;
178 | 				if(timeLeft < 0 ){
179 | 					return parsePaths(pathArray);
180 | 				}
181 | 
182 | 				Problem p = subProbVec.get(pi);
183 | 				// Check whether vertices are already used
184 | 				boolean newProb = true;
185 | 				for(int i = 0; i < p.graph.vertices.length; i ++){
186 | 					if(usedVertexSet.contains(p.newIdVidMap.get(p.graph.vertices[i].id))){
187 | 						newProb = false;
188 | 						break;
189 | 					}
190 | 				}
191 | 				if(!newProb)continue;
192 | 				
193 | 				// Solve 
194 | 				int[][] subPs = planPathsAdvanced(p.graph, p.sg[0], p.sg[1], true, 0, true, timeLeft);
195 | 				
196 | 				// Append to existing path set
197 | 				if(subPs != null){
198 | 					Map<Integer, Integer> startVIdAgtMap = new HashMap<Integer, Integer>();
199 | 					for(int i = 0; i < p.sg[0].length; i ++){
200 | 						startVIdAgtMap.put(p.sg[0][i], i);
201 | 					}
202 | 					
203 | 					for(int a = 0; a < numAgents; a++){
204 | 						int vId = paths[a][ct];
205 | 						// Is this agent part of the subproblem?
206 | 						if(p.vidNewIdMap.get(vId) != null){
207 | 							int newVId = p.vidNewIdMap.get(vId);
208 | 							// Yes, need to update path for this agent
209 | 							int subIndex = startVIdAgtMap.get(newVId);
210 | 							for(int t = 1; t < subPs[0].length; t ++){
211 | 								pathArray[a].addVertex(p.newIdVidMap.get(subPs[subIndex][t]));
212 | 							}
213 | 						}
214 | 					}
215 | 					// printStartAndGoals(g, start, goal);
216 | 					progress = true;
217 | 					
218 | 					// Update used vertex set
219 | 					for(int i = 0; i < p.graph.vertices.length; i ++){
220 | 						usedVertexSet.add(p.newIdVidMap.get(p.graph.vertices[i].id));
221 | 					}
222 | 					
223 | 					// Update maxLength if needed
224 | 					if(toAdd < subPs[0].length - 1){
225 | 						toAdd = subPs[0].length - 1;
226 | 					}
227 | 				}
228 | 				
229 | 			}
230 | 			
231 | 			// If we make progress, pad all paths to the same length
232 | 			if(progress){
233 | 				int expLength = startLength + toAdd;
234 | 				for(int a = 0; a < numAgents; a ++){
235 | 					int length = pathArray[a].vertexList.size();
236 | 					if(length < expLength){
237 | 						int vId = pathArray[a].vertexList.get(pathArray[a].vertexList.size() - 1);
238 | 						for(int i = 0; i < expLength - length; i ++){
239 | 							pathArray[a].addVertex(vId);
240 | 						}
241 | 					}
242 | 				}
243 | 			}
244 | 			
245 | 			if(pathArray[0].vertexList.size() == beginLength){
246 | 				progressCountStalled ++; 
247 | 				if(progressCountStalled == 5){
248 | 					return parsePaths(pathArray);
249 | 				}
250 | 			}
251 | 
252 | 			// printPaths(g, parsePaths(pathArray));
253 | 			
254 | 			for(int a = 0; a < numAgents; a++ )
255 | 				start[a] = pathArray[a].vertexList.get(pathArray[a].vertexList.size() - 1);
256 | 
257 | 		}
258 | 	}
259 | 	
260 | 	private int[] existsEdgeCollision(int[][] paths, int t){
261 | 		int numAgents = paths.length;
262 | 		Map<Long, Integer> eAMap = new HashMap<Long, Integer>();
263 | 
264 | 		for(int a = 0; a < numAgents; a ++){
265 | 			int v1 = paths[a][t];
266 | 			int v2 = paths[a][t+1];
267 | 			long eId = v1 > v2 ? v1*10000 + v2 : v2*10000 + v1;
268 | 			if(eAMap.get(eId) != null){
269 | 				return new int[]{eAMap.get(eId), a};
270 | 			}
271 | 			else
272 | 			{
273 | 				eAMap.put(eId, a);
274 | 			}
275 | 		}
276 | 		return null;
277 | 	}
278 | 		
279 | 	private int[] existsVertexCollision(int[][] paths, int t){
280 | 		int numAgents = paths.length;
281 | 		Map<Integer, Integer> vAMap = new HashMap<Integer, Integer>();
282 | 		for(int a = 0; a < numAgents; a ++){
283 | 			int vId = paths[a][t+1];
284 | 			if(vAMap.get(vId) != null){
285 | 				return new int[]{vAMap.get(vId), a};
286 | 			}
287 | 			else{
288 | 				vAMap.put(vId, a);
289 | 			}
290 | 		}
291 | 		return null;
292 | 	}
293 | 	
294 | 	/**
295 | 	 * Move agents at most one step 
296 | 	 */
297 | 	private void moveAgentsIndependentlyOneStep(Path[] pathArray, int[][]paths, int t){
298 | 		int numAgent = paths.length;
299 | 		for(int a = 0; a < numAgent; a ++){
300 | 			pathArray[a].addVertex(paths[a][t]);
301 | 		}
302 | 	}
303 | 
304 | 	/**
305 | 	 * Create a subproblem around the conflicting vertices
306 | 	 * @param g
307 | 	 * @param paths
308 | 	 * @param ct
309 | 	 * @param svs
310 | 	 * @param goal
311 | 	 * @param distance
312 | 	 * @return
313 | 	 */
314 | 	private Problem createSubProblem(Graph g, int[][] paths, int ct, Integer svs[], int goal[], int distance){
315 | 		int numAgents = paths.length;
316 |  
317 | 		// Gather vertices that are of distance two from conflicting vertices
318 | 		Set<Integer> involved = new HashSet<Integer>();
319 | 		Set<Integer> unOccupied = new HashSet<Integer>();
320 | 		for(int a = 0; a < svs.length; a ++){
321 | 			involved.add(svs[a]);
322 | 			unOccupied.add(svs[a]);
323 | 			
324 | 			// Distance 1
325 | 			Integer[] adjVs = g.adjacencySetMap.get(svs[a]).toArray(new Integer[0]);
326 | 			for(int i = 0; i < adjVs.length; i ++){
327 | 				involved.add(adjVs[i]);
328 | 				// Distance 2
329 | 				if(distance > 1){
330 | 					Integer[] adj2Vs = g.adjacencySetMap.get(adjVs[i]).toArray(new Integer[0]);
331 | 					for(int j = 0; j < adj2Vs.length; j ++){
332 | 						involved.add(adj2Vs[j]);
333 | 						// Distance 3
334 | 						if(distance > 2 || Math.random() > 0.75){
335 | 							Integer[] adj3Vs = g.adjacencySetMap.get(adj2Vs[j]).toArray(new Integer[0]);
336 | 							for(int k = 0; k < adj3Vs.length; k ++){
337 | 								involved.add(adj3Vs[k]);
338 | 								// Distance 4
339 | //								if(distance > 3 || Math.random() > 0.999){
340 | //									Integer[] adj4Vs = g.adjacencySetMap.get(adj3Vs[k]).toArray(new Integer[0]);
341 | //									for(int l = 0; l < adj4Vs.length; l ++){
342 | //										involved.add(adj4Vs[l]);
343 | //									}
344 | //								}
345 | 							}
346 | 						}
347 | 					}
348 | 				}
349 | 			}
350 | 		}
351 | 		
352 | 		// We have all the vertices, create graph
353 | 		Graph subG = new Graph();
354 | 		Integer[] vs = involved.toArray(new Integer[0]); 
355 | 		Map<Integer, Integer> vidNewIdMap = new HashMap<Integer, Integer>();
356 | 		Map<Integer, Integer> newIdVidMap = new HashMap<Integer, Integer>();
357 | 		
358 | 		// Add vertices
359 | 		for(int i = 0; i < vs.length; i ++){
360 | 			vidNewIdMap.put(vs[i], i);
361 | 			newIdVidMap.put(i, vs[i]);
362 | 			Vertex v = new Vertex(i);
363 | 			subG.verticeSet.add(v);
364 | 			subG.idVertexMap.put(v.id, v);
365 | 			subG.adjacencySetMap.put(v.id, new HashSet<Integer>());
366 | 			subG.adjacencySetMap.get(v.id).add(v.id);
367 | 		}
368 | 		
369 | 		// Build up the rest of the graph
370 | 		for(int i = 0; i < vs.length; i ++){
371 | 			Integer adjs[] = g.adjacencySetMap.get(vs[i]).toArray(new Integer[0]);
372 | 			for(int nbr = 0; nbr < adjs.length; nbr++){
373 | 				Integer idInt = vidNewIdMap.get(adjs[nbr]);
374 | 				if(idInt != null){
375 | 					subG.adjacencySetMap.get(i).add(idInt.intValue());
376 | 				}
377 | 			}
378 | 		}
379 | 		subG.finishBuildingGraph();
380 | 		
381 | 		// Start and goal locations. First locate all agents starts in subG
382 | 		Vector<Integer> newStart = new Vector<Integer>();
383 | 		Vector<Integer> newGoal = new Vector<Integer>();
384 | 
385 | 		unOccupied.addAll(involved);
386 | 		
387 | 		for(int a = 0; a < numAgents; a ++){
388 | 			int vId = paths[a][ct];
389 | 			if(vidNewIdMap.get(vId) != null){
390 | 				// Start is determined
391 | 				newStart.add(vidNewIdMap.get(vId));
392 | 				
393 | 				// Assign goal 
394 | 				SortedMap<Integer,Integer> dVMap = new TreeMap<Integer, Integer>();
395 | 				for(Integer vId2: unOccupied){
396 | 					dVMap.put(distanceToGoals[a][vId2], vId2);
397 | 				}
398 | 				int goalId = dVMap.get(dVMap.firstKey());
399 | 				newGoal.add(vidNewIdMap.get(goalId));
400 | 				unOccupied.remove(goalId);
401 | 			}
402 | 		}
403 | 		
404 | 		int sg[][] = new int[2][newStart.size()];
405 | 		for(int i = 0; i < newStart.size(); i ++){
406 | 			sg[0][i] = newStart.get(i);
407 | 			sg[1][i] = newGoal.get(i);
408 | 		}
409 | 		
410 | 		Problem p = new Problem();
411 | 		p.graph = subG;
412 | 		p.sg = sg;
413 | 		p.vidNewIdMap = vidNewIdMap;
414 | 		p.newIdVidMap = newIdVidMap;
415 | 		
416 | 		return p;
417 | 	}
418 | 	
419 | }
420 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/MultiagentGraphSolverGurobiTime.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import gurobi.GRB;
  4 | import gurobi.GRBCallback;
  5 | import gurobi.GRBEnv;
  6 | import gurobi.GRBException;
  7 | import gurobi.GRBLinExpr;
  8 | import gurobi.GRBModel;
  9 | import gurobi.GRBVar;
 10 | 
 11 | import java.util.HashSet;
 12 | import java.util.Set;
 13 | import java.util.SortedMap;
 14 | import java.util.TreeMap;
 15 | import java.util.Vector;
 16 | 
 17 | import projects.multipath.advanced.Graph;
 18 | import projects.multipath.advanced.Path;
 19 | 
 20 | public class MultiagentGraphSolverGurobiTime extends GRBCallback{
 21 | 
 22 | 	public static boolean bPrintPaths = true;
 23 | 	public static boolean bDebugInfo = true;
 24 | 	
 25 | 	private SortedMap<Long, GRBVar> edgeVarMap = new TreeMap<Long, GRBVar>();
 26 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 27 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 28 | 	private SortedMap<Long, Vector<GRBVar>> avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 29 | 	private SortedMap<Long, Vector<GRBVar>> avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 30 | 	private SortedMap<Long, Vector<GRBVar>> edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 31 | 	private boolean reachableVertices[][][] = null;
 32 | 	private GRBModel currentModel = null;
 33 | 	private int expectedBound = 0;
 34 | 			
 35 | 	protected int[][] solve(Graph graph, int starts[], int goals[], boolean solveRelaxedFirst, boolean setOptimal, int extraSteps, boolean hasTimeLimit, double timeLimit) throws GRBException{
 36 | 		long startTime = System.currentTimeMillis(); 
 37 | 		
 38 | 		// Check minimal distances necessary for solving the problem 
 39 | 		int startTimeSteps = 0;
 40 | 		Path paths[] = PathFinder.findShortestPaths(graph, starts, goals);
 41 | 		for(int i = 0; i < paths.length; i ++){
 42 | 			if(paths[i].vertexList.size() - 1 > startTimeSteps){
 43 | 				startTimeSteps = paths[i].vertexList.size() - 1;
 44 | 			}
 45 | 		}
 46 | 		
 47 | 		// Create LP solver
 48 | 		GRBEnv env = new GRBEnv();
 49 | 		env.set(GRB.IntParam.OutputFlag, 0);
 50 | //		env.set(GRB.DoubleParam.Heuristics, 0.2);
 51 | //		env.set(GRB.IntParam.Threads, 3);
 52 | 		// env.set(GRB.IntParam.Presolve, 2);
 53 | 		
 54 | 		// Solve the LP relaxation
 55 | 		startTimeSteps += extraSteps;
 56 | 		if(bDebugInfo)System.out.println("\nUsing " + extraSteps + " extra steps.");
 57 | 		updateReachableSets(graph, starts, goals, startTimeSteps);
 58 | 		if(solveRelaxedFirst && extraSteps == 0){
 59 | 			System.out.println("\nSolving relaxed model first. ");
 60 | 			while(true){
 61 | 				timeLimit = timeLimit - (System.currentTimeMillis() - startTime)/1000.0;
 62 | 				System.out.print("Trying T = " + startTimeSteps + (hasTimeLimit?(" with max time limit " + timeLimit):"") + " ... ");
 63 | 				GRBModel model = prepareModel(env, graph, starts, goals, false, false, startTimeSteps, hasTimeLimit ? timeLimit : -1);
 64 | 				model.relax();
 65 | 				model.optimize();
 66 | 				double bestBet = model.get(GRB.DoubleAttr.ObjVal);
 67 | 				model.dispose();
 68 | 				if(bestBet + 0.01 > starts.length){
 69 | 					if(hasTimeLimit && timeLimit - (System.currentTimeMillis() - startTime)/1000.0 < 0){
 70 | 						System.out.println("relaxed model not feasible, time limit reached.");
 71 | 						env.dispose();
 72 | 						return null;
 73 | 					}
 74 | 					System.out.println("relaxed model feasible.");
 75 | 					break;
 76 | 				}
 77 | 				else if(hasTimeLimit && timeLimit - (System.currentTimeMillis() - startTime)/1000.0 < 0){
 78 | 					System.out.println("relaxed model not feasible, time limit reached.");
 79 | 					env.dispose();
 80 | 					return null;
 81 | 				}
 82 | 				System.out.println("relaxed model not feasible, increase T...");
 83 | 				startTimeSteps += 1;
 84 | 				updateReachableSets(graph, starts, goals, startTimeSteps);
 85 | 			}
 86 | 		}
 87 | 		
 88 | 		// Solve the ILP
 89 | 		int val = 0; 
 90 | 		int ps[][] = null;
 91 | 		while(val != starts.length){
 92 | 			timeLimit = timeLimit - (System.currentTimeMillis() - startTime)/1000.0;
 93 | 			if(bDebugInfo){
 94 | 				System.out.print("Trying T = " + startTimeSteps + " for the integer model " + (hasTimeLimit?("with max time limit " + timeLimit):"") + "...");
 95 | 			}
 96 | 			GRBModel model = prepareModel(env, graph, starts, goals, true, setOptimal, startTimeSteps, hasTimeLimit ? timeLimit : -1);
 97 | 			expectedBound = starts.length;
 98 | //			model.setCallback(this);
 99 | 			model.optimize();
100 | //			model.setCallback(null);
101 | 			if(model.get(GRB.IntAttr.SolCount) > 0){
102 | 				val = (int) (model.get(GRB.DoubleAttr.ObjVal));
103 | 			}
104 | 			if(val == starts.length){
105 | 				if(bDebugInfo){
106 | 					System.out.println(" done.\n");
107 | 					System.out.println("Solution with optimal makespan found in " + (System.currentTimeMillis() - startTime)/1000.0 + " seconds.");
108 | 				}
109 | 				ps = retrievePaths(model, graph, starts, startTimeSteps);
110 | 			}
111 | 			else{
112 | 				if(hasTimeLimit && (timeLimit - (System.currentTimeMillis() - startTime)/1000.0) < 0){
113 | 					if(bDebugInfo){
114 | 						System.out.println(" time out.");
115 | 					}
116 | 					model.dispose();
117 | 					env.dispose(); 
118 | 					return null;
119 | 				}
120 | 				else{
121 | 					if(bDebugInfo){
122 | 						System.out.println(" integer model infeasible.");
123 | 					}
124 | 				}
125 | 			}
126 | 			startTimeSteps ++;
127 | 			updateReachableSets(graph, starts, goals, startTimeSteps);
128 | 			model.dispose();
129 | 		}
130 | 		env.dispose();
131 | 		
132 | 		return ps;
133 | 	}
134 | 	
135 | 	private void updateReachableSets(Graph graph, int starts[], int goals[], int startTimeSteps){
136 | 		/**
137 | 		 *  Preparation. First compute whether a vertex is reachable by an agent at a time step.
138 | 		 *  For each agent with start x_i and goal x_g, a vertex v at time t is reachable if  
139 | 		 *  dist(x_i, v) <=  t and dist(v, x_g) <= T - t.   
140 | 		 */
141 | 		reachableVertices = new boolean[starts.length][graph.vertices.length][startTimeSteps + 1];
142 | 		for(int a = 0; a < starts.length; a ++){
143 | 			// Compute the distances from start/goal to all vertices of the graph   
144 | 			int[] distFromStart = PathFinder.getDistancesToAllVertices(graph, starts[a], null); 
145 | 			int[] distFromGoal = PathFinder.getDistancesToAllVertices(graph, goals[a], null);
146 | 			
147 | 			// Assign reachability values
148 | 			for(int t = 0; t <= startTimeSteps; t ++){
149 | 				for(int v = 0; v < graph.vertices.length; v ++){
150 | 					if(distFromStart[v] <= t && distFromGoal[v] <= startTimeSteps - t){
151 | 						reachableVertices[a][v][t] = true;
152 | 					}
153 | 					else{
154 | 						reachableVertices[a][v][t] = false;
155 | 					}
156 | 				}
157 | 			}
158 | 		}
159 | 		
160 | 	}
161 | 	
162 | 	private long getEdgeId(int a, int v1, int v2, int t) {
163 | 		return a * 1000000000000L + v1 * 100000000 + v2 * 10000 + t;
164 | 	}
165 | 
166 | 	private long getVertexTimeId(int v, int t) {
167 | 		return v * 10000 + t;
168 | 	}
169 | 
170 | 	private long getAgentVertexTimeId(int a, int v, int t) {
171 | 		return a * 1000000000000L + v * 10000 + t;
172 | 	}
173 | 
174 | 	private long getEdgeTimeId(int v1, int v2, int t) {
175 | 		if(v1 > v2){
176 | 			int temp = v1; 
177 | 			v1 = v2;
178 | 			v2 = temp;
179 | 		}
180 | 		return v1 * 1000000000 + v2* 10000 + t;
181 | 	}
182 | 
183 | 	private int[][] retrievePaths(GRBModel model, Graph graph, int starts[], int timeSteps) throws GRBException{
184 | 		int paths[][] = new int[starts.length][timeSteps + 1];
185 | 		for(int a = 0; a < starts.length; a ++){
186 | 			if(bPrintPaths){
187 | 				System.out.print("Agent " + a + ": 0:");
188 | 				graph.vertices[starts[a]].printVertex();
189 | 			}
190 | 			paths[a][0] = starts[a];
191 | 			for(int t = 1; t <= timeSteps; t ++){
192 | 				Integer nvs[] = graph.adjacencySetMap.get(paths[a][t-1]).toArray(new Integer[0]);
193 | 				for(int nv = 0; nv < nvs.length; nv ++){
194 | 					if(isEdgeVarSetToTrue(model, a, paths[a][t-1], nvs[nv], t-1)){
195 | 						paths[a][t] = nvs[nv];
196 | 						break;
197 | 					}
198 | 				}
199 | 				if(bPrintPaths){
200 | 					System.out.print(" " + t + ":");
201 | 					graph.vertices[paths[a][t]].printVertex();
202 | 				}
203 | 			}
204 | 			if(bPrintPaths)System.out.println();
205 | 		}
206 | 		return paths;
207 | 	}
208 | 	
209 | 	private boolean isEdgeVarSetToTrue(GRBModel model, int a, int v1, int v2, int t) throws GRBException{
210 | 		long eId = getEdgeId(a, v1, v2, t);
211 | 		GRBVar var = edgeVarMap.get(eId);
212 | 		if(var != null && var.get(GRB.DoubleAttr.X) == 1.0){
213 | 			return true;
214 | 		}
215 | 		return false;
216 | 	}
217 | 	
218 | 	private GRBModel prepareModel(GRBEnv env, Graph graph, int starts[], int goals[], boolean integer, boolean setOptimal, int timeSteps, double timeLimit) throws GRBException{
219 | 		// Reset global storage
220 | 		edgeVarMap = new TreeMap<Long, GRBVar>();
221 | 		vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
222 | 		vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
223 | 		avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
224 | 		avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
225 | 		edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
226 | 		
227 | 		// Suppress output if we are solving the relaxation
228 | 		if(bDebugInfo){env.set(GRB.IntParam.OutputFlag, 1);}
229 | 		else{env.set(GRB.IntParam.OutputFlag, 0);}
230 | 
231 | 		// Time limit?
232 | 		if(timeLimit > 0)
233 | 		{ 
234 | 			env.set(GRB.DoubleParam.TimeLimit, timeLimit); 
235 | 		}
236 | 		else{
237 | 			env.set(GRB.DoubleParam.TimeLimit, 10000000000.0);
238 | 		}
239 | 		
240 | 		// Setup model
241 | 		GRBModel model = new GRBModel(env);
242 | 		currentModel = model;
243 | 
244 | 		// Set up variables for all edges, agent by agent
245 | 		for(int a = 0; a < starts.length; a++){
246 | 			Set<Integer> rVs = new HashSet<Integer>();
247 | 			rVs.add(starts[a]);
248 | 			for(int t = 1; t <= timeSteps; t++){
249 | 				Integer[] rvs = rVs.toArray(new Integer[0]);
250 | 				for(int rv = 0; rv < rvs.length; rv ++){
251 | 					int curV = rvs[rv];
252 | 					Integer[] adjVs = graph.adjacencySetMap.get(curV).toArray(new Integer[0]);
253 | 					for(int i = 0; i < adjVs.length; i ++){
254 | 						int nextV = adjVs[i];
255 | 						if(reachableVertices[a][nextV][t]){
256 | 							rVs.add(nextV);
257 | 							GRBVar x = model.addVar(0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
258 | 							long edgeId = getEdgeId(a, curV, nextV, t - 1);
259 | 							edgeVarMap.put(edgeId, x);
260 | 
261 | 							// Store edges indexed by vertices
262 | 							storeVarForVertices(x, curV, nextV, t - 1, t);
263 | 							storeVarForAVT(x, a, curV, nextV, t - 1, t);
264 | 
265 | 							// Store edges indexed by edges, if the start/goal vertice are different
266 | 							if(curV != nextV){
267 | 								storeVarForEdges(x, curV, nextV, t - 1);
268 | 							}
269 | 						}
270 | 					}
271 | 
272 | 					// Remove current vertex if it's no longer reachable
273 | 					if(!reachableVertices[a][curV][t]){
274 | 						rVs.remove(curV);
275 | 					}
276 | 				}
277 | 			}
278 | 		}
279 | 		model.update();
280 | 		
281 | 		// Special case for last t, only need to setup single edges between respective start/goal
282 | 		GRBLinExpr objExpr = new GRBLinExpr();
283 | 		for(int a = 0; a < starts.length; a ++){
284 | 			long edgeId = getEdgeId(a, goals[a], starts[a], timeSteps);
285 | 			GRBVar x = model.addVar(setOptimal?1.0:0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
286 | 			model.update();
287 | 
288 | 			// Store edges indexed by vertices
289 | 			storeVarForVertices(x, goals[a], starts[a], timeSteps, 0);
290 | 			storeVarForAVT(x, a, goals[a], starts[a], timeSteps, 0);
291 | 			
292 | 			// Setup the objective exprssion 
293 | 			objExpr.addTerm(1, x);
294 | 			edgeVarMap.put(edgeId, x);
295 | 		}
296 | 		model.setObjective(objExpr, GRB.MAXIMIZE);
297 | 		
298 | 
299 | 		// Set up constraints for edges going into a vertex
300 | 		for(int t = 0; t < timeSteps; t++){
301 | 
302 | 			// Set up variables for all edges and expression 
303 | 			for(int v = 0; v < graph.vertices.length; v++){
304 | 				GRBLinExpr expr = null;
305 | 				long vtId = getVertexTimeId(v, t + 1);
306 | 				Vector<GRBVar> inVarVec = vertexTimeInVarVectorMap.get(vtId);
307 | 				if(inVarVec != null){
308 | 					if(expr == null) expr = new GRBLinExpr();
309 | 					for(int i = 0; i < inVarVec.size(); i ++){
310 | 						expr.addTerm(1., inVarVec.get(i));
311 | 					}
312 | 				}
313 | 				if(expr != null){
314 | 					if(expr.size() > 1){
315 | 						model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
316 | 					}
317 | 					expr = null;
318 | 				}
319 | 			}
320 | 		}
321 | 		
322 | 		// Set up constraints for vertices
323 | 		for(int a = 0; a < starts.length; a ++){
324 | 			for(int t = 0; t <= timeSteps; t++){
325 | 				// Set up variables for all edges and expression 
326 | 				for(int v = 0; v < graph.vertices.length; v++){
327 | 					GRBLinExpr expr = null;
328 | 					long vtId = getAgentVertexTimeId(a, v, t);
329 | 					Vector<GRBVar> inVarVec = avtInVarVectorMap.get(vtId);
330 | 					if(inVarVec != null){
331 | 						expr = new GRBLinExpr();
332 | 						for(int i = 0; i < inVarVec.size(); i ++){
333 | 							expr.addTerm(-1., inVarVec.get(i));
334 | 						}
335 | 					}
336 | 					Vector<GRBVar> outVarVec = avtOutVarVectorMap.get(vtId);
337 | 					if(outVarVec != null){
338 | 						if(expr == null) expr = new GRBLinExpr();
339 | 						for(int i = 0; i < outVarVec.size(); i ++){
340 | 							expr.addTerm(1., outVarVec.get(i));
341 | 						}
342 | 					}
343 | 					if(expr != null){
344 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
345 | 						expr = null;
346 | 					}
347 | 				}
348 | 			}
349 | 		}
350 | 		
351 | 		// Setup constraint for single edges
352 | 		Long[] edgeTimeKeys = edgeTimeVarVectorMap.keySet().toArray(new Long[0]);
353 | 		for(int e = 0; e < edgeTimeKeys.length; e ++){
354 | 			GRBLinExpr expr = null;
355 | 			Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(edgeTimeKeys[e]);
356 | 			if(varVec != null){
357 | 				if(expr == null) expr = new GRBLinExpr();
358 | 				for(int i = 0; i < varVec.size(); i ++){
359 | 					expr.addTerm(1., varVec.get(i));
360 | 				}
361 | 			}
362 | 			if(expr != null && expr.size() > 1){
363 | 				model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
364 | 			}
365 | 			expr = null;
366 | 		}
367 | 		model.update();
368 | 		
369 | 		return model;
370 | 	}
371 | 	
372 | 	private void storeVarForAVT(GRBVar var, int a, int v1, int v2, int t1, int t2){
373 | 		// Add edge var to vertex time map, outgoing first
374 | 		long vtId = getAgentVertexTimeId(a, v1, t1);
375 | 		Vector<GRBVar> varVec = avtOutVarVectorMap.get(vtId);
376 | 		if(varVec == null){
377 | 			varVec = new Vector<GRBVar>();
378 | 			avtOutVarVectorMap.put(vtId, varVec);
379 | 		}
380 | 		varVec.add(var);
381 | 		
382 | 		// Incoming
383 | 		vtId = getAgentVertexTimeId(a, v2, t2);
384 | 		varVec = avtInVarVectorMap.get(vtId);
385 | 		if(varVec == null){
386 | 			varVec = new Vector<GRBVar>();
387 | 			avtInVarVectorMap.put(vtId, varVec);
388 | 		}
389 | 		varVec.add(var);
390 | 	}
391 | 
392 | 	/**
393 | 	 * Store edge var for head and tail vertices
394 | 	 * @param var
395 | 	 * @param v1
396 | 	 * @param v2
397 | 	 * @param t1
398 | 	 * @param t2
399 | 	 */
400 | 	private void storeVarForVertices(GRBVar var, int v1, int v2, int t1, int t2){
401 | 		// Add edge var to vertex time map, outgoing first
402 | 		long vtId = getVertexTimeId(v1, t1);
403 | 		Vector<GRBVar> varVec = vertexTimeOutVarVectorMap.get(vtId);
404 | 		if(varVec == null){
405 | 			varVec = new Vector<GRBVar>();
406 | 			vertexTimeOutVarVectorMap.put(vtId, varVec);
407 | 		}
408 | 		varVec.add(var);
409 | 		
410 | 		// Incoming
411 | 		vtId = getVertexTimeId(v2, t2);
412 | 		varVec = vertexTimeInVarVectorMap.get(vtId);
413 | 		if(varVec == null){
414 | 			varVec = new Vector<GRBVar>();
415 | 			vertexTimeInVarVectorMap.put(vtId, varVec);
416 | 		}
417 | 		varVec.add(var);
418 | 	}
419 | 
420 | 	/**
421 | 	 * Store edge var for one edge in the original graph with different time stamps
422 | 	 * @param var
423 | 	 * @param v1
424 | 	 * @param v2
425 | 	 * @param t
426 | 	 */
427 | 	private void storeVarForEdges(GRBVar var, int v1, int v2, int t){
428 | 		// Add edge var to edge time map
429 | 		long etId = getEdgeTimeId(v1, v2, t);
430 | 		Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(etId);
431 | 		if(varVec == null){
432 | 			varVec = new Vector<GRBVar>();
433 | 			edgeTimeVarVectorMap.put(etId, varVec);
434 | 		}
435 | 		varVec.add(var);
436 | 	}
437 | 	
438 | 	@Override
439 | 	protected void callback() {
440 | 		try{
441 | 			if (where == GRB.CB_MIP) {
442 | 				if(expectedBound > getDoubleInfo(GRB.Callback.MIP_OBJBND)){
443 | 					currentModel.terminate();
444 | 				}
445 | 			}
446 | 		} catch (GRBException e) {
447 | 			System.out.println("Error code: " + e.getErrorCode() + ". "
448 | 					+ e.getMessage());
449 | 			e.printStackTrace();
450 | 		}
451 | 
452 | 	}
453 | 
454 | 
455 | }
456 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/MultiagentGraphSolverGurobiTotalTime.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import gurobi.GRB;
  4 | import gurobi.GRBCallback;
  5 | import gurobi.GRBEnv;
  6 | import gurobi.GRBException;
  7 | import gurobi.GRBLinExpr;
  8 | import gurobi.GRBModel;
  9 | import gurobi.GRBVar;
 10 | 
 11 | import java.util.HashSet;
 12 | import java.util.Set;
 13 | import java.util.SortedMap;
 14 | import java.util.TreeMap;
 15 | import java.util.Vector;
 16 | 
 17 | import projects.multipath.advanced.Graph;
 18 | import projects.multipath.advanced.Path;
 19 | 
 20 | public class MultiagentGraphSolverGurobiTotalTime extends GRBCallback{
 21 | 
 22 | 	public static boolean bPrintPaths = true;
 23 | 	public static boolean bDebugInfo = true;
 24 | 	public static boolean bReturnAnySolution = false;
 25 | 	
 26 | 	private SortedMap<Long, GRBVar> edgeVarMap = new TreeMap<Long, GRBVar>();
 27 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 28 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 29 | 	private SortedMap<Long, Vector<GRBVar>> avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 30 | 	private SortedMap<Long, Vector<GRBVar>> avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 31 | 	private SortedMap<Long, Vector<GRBVar>> edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 32 | 	private boolean reachableVertices[][][] = null;
 33 | 	private GRBModel currentModel = null;
 34 | 	private int expectedBound = 0;
 35 | 			
 36 | 	// Variables for time till end
 37 | 	private GRBVar[][] yVars = null;
 38 | 	private GRBVar[] yTVars = null;
 39 | 	
 40 | 	
 41 | 	protected int[][] solve(Graph graph, int starts[], int goals[], int opt[], double gap, int timeSteps, boolean hasTimeLimit, double timeLimit) throws GRBException{
 42 | 		long startTime = System.currentTimeMillis(); 
 43 | 		
 44 | 		// Check minimal distances necessary for solving the problem 
 45 | 		int optLB = 0;
 46 | 		Path paths[] = PathFinder.findShortestPaths(graph, starts, goals);
 47 | 		for(int i = 0; i < paths.length; i ++){
 48 | 			optLB += (paths[i].vertexList.size() - 1);
 49 | 		}
 50 | 		
 51 | 		// Create LP solver
 52 | 		GRBEnv env = new GRBEnv();
 53 | 		env.set(GRB.IntParam.OutputFlag, 0);
 54 | 		env.set(GRB.DoubleParam.MIPGap, gap);
 55 | 
 56 | 		if(bDebugInfo)System.out.println("\nUsing " + timeSteps + " time expansion horizon.");
 57 | 
 58 | 		// Update reachable set
 59 | 		updateReachableSets(graph, starts, goals, timeSteps);
 60 | 		
 61 | 		// Solve the ILP, this should always be feasible
 62 | 		int val = 0; 
 63 | 		int ps[][] = null;
 64 | 		if(bDebugInfo){
 65 | 			System.out.print("Trying T = " + timeSteps + " for the integer model " + (hasTimeLimit?("with max time limit " + timeLimit):"") + "...");
 66 | 		}
 67 | 		GRBModel model = prepareModel(env, graph, starts, goals, true, timeSteps, hasTimeLimit ? timeLimit : -1);
 68 | 		
 69 | 		expectedBound = starts.length;
 70 | 		System.out.println(" let's do this...");
 71 | 
 72 | 		model.optimize();
 73 | 		
 74 | 		// We should always have a solution here, unless we ran out of time
 75 | 		if(model.get(GRB.IntAttr.SolCount) > 0){
 76 | 			val = (int) (model.get(GRB.DoubleAttr.ObjVal));
 77 | 			double bnd = model.get(GRB.DoubleAttr.ObjBound);
 78 | 			double agap = bnd/val - 1;
 79 | 			
 80 | 			if(agap <= gap || bReturnAnySolution){
 81 | 				val = starts.length*timeSteps - val;
 82 | 				opt[0] = val;
 83 | 				
 84 | 				// Check actual gap
 85 | 				
 86 | 				if(bDebugInfo){
 87 | 					System.out.println(" done.\n");
 88 | 					System.out.println("Solution with a total of " + val + " time steps found in " + (System.currentTimeMillis() - startTime)/1000.0 + " seconds.");
 89 | 					System.out.println("Lower bound is " + optLB);
 90 | 				}
 91 | 				ps = retrievePaths(model, graph, starts, timeSteps);
 92 | 			}
 93 | 		}
 94 | 		if(ps == null && bDebugInfo){
 95 | 			System.out.println(" time out.");
 96 | 		}
 97 | 		
 98 | 		
 99 | 		model.dispose();
100 | 		env.dispose();
101 | 		
102 | 		return ps;
103 | 	}
104 | 	
105 | 	private void updateReachableSets(Graph graph, int starts[], int goals[], int startTimeSteps){
106 | 		/**
107 | 		 *  Preparation. First compute whether a vertex is reachable by an agent at a time step.
108 | 		 *  For each agent with start x_i and goal x_g, a vertex v at time t is reachable if  
109 | 		 *  dist(x_i, v) <=  t and dist(v, x_g) <= T - t.   
110 | 		 */
111 | 		reachableVertices = new boolean[starts.length][graph.vertices.length][startTimeSteps + 1];
112 | 		for(int a = 0; a < starts.length; a ++){
113 | 			// Compute the distances from start/goal to all vertices of the graph   
114 | 			int[] distFromStart = PathFinder.getDistancesToAllVertices(graph, starts[a], null); 
115 | 			int[] distFromGoal = PathFinder.getDistancesToAllVertices(graph, goals[a], null);
116 | 			
117 | 			// Assign reachability values
118 | 			for(int t = 0; t <= startTimeSteps; t ++){
119 | 				for(int v = 0; v < graph.vertices.length; v ++){
120 | 					if(distFromStart[v] <= t && distFromGoal[v] <= startTimeSteps - t){
121 | 						reachableVertices[a][v][t] = true;
122 | 					}
123 | 					else{
124 | 						reachableVertices[a][v][t] = false;
125 | 					}
126 | 				}
127 | 			}
128 | 		}
129 | 		
130 | 	}
131 | 	
132 | 	private long getEdgeId(int a, int v1, int v2, int t) {
133 | 		return a * 1000000000000L + v1 * 100000000 + v2 * 10000 + t;
134 | 	}
135 | 
136 | 	private long getVertexTimeId(int v, int t) {
137 | 		return v * 10000 + t;
138 | 	}
139 | 
140 | 	private long getAgentVertexTimeId(int a, int v, int t) {
141 | 		return a * 1000000000000L + v * 10000 + t;
142 | 	}
143 | 
144 | 	private long getEdgeTimeId(int v1, int v2, int t) {
145 | 		if(v1 > v2){
146 | 			int temp = v1; 
147 | 			v1 = v2;
148 | 			v2 = temp;
149 | 		}
150 | 		return v1 * 1000000000 + v2* 10000 + t;
151 | 	}
152 | 
153 | 	private int[][] retrievePaths(GRBModel model, Graph graph, int starts[], int timeSteps) throws GRBException{
154 | 		int paths[][] = new int[starts.length][timeSteps + 1];
155 | 		for(int a = 0; a < starts.length; a ++){
156 | 			if(bPrintPaths){
157 | 				System.out.print("Agent " + a + ": 0:");
158 | 				graph.vertices[starts[a]].printVertex();
159 | 			}
160 | 			paths[a][0] = starts[a];
161 | 			for(int t = 1; t <= timeSteps; t ++){
162 | 				Integer nvs[] = graph.adjacencySetMap.get(paths[a][t-1]).toArray(new Integer[0]);
163 | 				for(int nv = 0; nv < nvs.length; nv ++){
164 | 					if(isEdgeVarSetToTrue(model, a, paths[a][t-1], nvs[nv], t-1)){
165 | 						paths[a][t] = nvs[nv];
166 | 						break;
167 | 					}
168 | 				}
169 | 				if(bPrintPaths){
170 | 					System.out.print(" " + t + ":");
171 | 					graph.vertices[paths[a][t]].printVertex();
172 | 				}
173 | 			}
174 | 			if(bPrintPaths)System.out.println();
175 | 		}
176 | 		return paths;
177 | 	}
178 | 	
179 | 	private boolean isEdgeVarSetToTrue(GRBModel model, int a, int v1, int v2, int t) throws GRBException{
180 | 		long eId = getEdgeId(a, v1, v2, t);
181 | 		GRBVar var = edgeVarMap.get(eId);
182 | 		if(var != null && var.get(GRB.DoubleAttr.X) == 1.0){
183 | 			return true;
184 | 		}
185 | 		return false;
186 | 	}
187 | 	
188 | 	private GRBModel prepareModel(GRBEnv env, Graph graph, int starts[], int goals[], boolean integer, int timeSteps, double timeLimit) throws GRBException{
189 | 		// Reset global storage
190 | 		edgeVarMap = new TreeMap<Long, GRBVar>();
191 | 		vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
192 | 		vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
193 | 		avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
194 | 		avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
195 | 		edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
196 | 		
197 | 		yVars = new GRBVar[goals.length][timeSteps];
198 | 		yTVars = new GRBVar[goals.length];
199 | 
200 | 		// Suppress output if we are solving the relaxation
201 | 		if(bDebugInfo){env.set(GRB.IntParam.OutputFlag, 1);}
202 | 		else{env.set(GRB.IntParam.OutputFlag, 0);}
203 | 		
204 | 		// env.set(GRB.IntParam.OutputFlag, 1);
205 | 
206 | 		// Time limit?
207 | 		if(timeLimit > 0)
208 | 		{ 
209 | 			env.set(GRB.DoubleParam.TimeLimit, timeLimit); 
210 | 		}
211 | 		else{
212 | 			env.set(GRB.DoubleParam.TimeLimit, 10000000000.0);
213 | 		}
214 | 		
215 | 		// Setup model
216 | 		GRBModel model = new GRBModel(env);
217 | 		currentModel = model;
218 | 
219 | 		// Set up variables for all edges, agent by agent
220 | 		for(int a = 0; a < starts.length; a++){
221 | 			Set<Integer> rVs = new HashSet<Integer>();
222 | 			rVs.add(starts[a]);
223 | 			for(int t = 1; t <= timeSteps; t++){
224 | 				Integer[] rvs = rVs.toArray(new Integer[0]);
225 | 				for(int rv = 0; rv < rvs.length; rv ++){
226 | 					int curV = rvs[rv];
227 | 					Integer[] adjVs = graph.adjacencySetMap.get(curV).toArray(new Integer[0]);
228 | 					for(int i = 0; i < adjVs.length; i ++){
229 | 						int nextV = adjVs[i];
230 | 						if(reachableVertices[a][nextV][t]){
231 | 							rVs.add(nextV);
232 | 							GRBVar x = model.addVar(0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
233 | 							long edgeId = getEdgeId(a, curV, nextV, t - 1);
234 | 							edgeVarMap.put(edgeId, x);
235 | 
236 | 							// Store edges indexed by vertices
237 | 							storeVarForVertices(x, curV, nextV, t - 1, t);
238 | 							storeVarForAVT(x, a, curV, nextV, t - 1, t);
239 | 
240 | 							// Store edges indexed by edges, if the start/goal vertice are different
241 | 							if(curV != nextV){
242 | 								storeVarForEdges(x, curV, nextV, t - 1);
243 | 							}
244 | 						}
245 | 					}
246 | 
247 | 					// Remove current vertex if it's no longer reachable
248 | 					if(!reachableVertices[a][curV][t]){
249 | 						rVs.remove(curV);
250 | 					}
251 | 				}
252 | 			}
253 | 		}
254 | 		model.update();
255 | 
256 | 		GRBVar xx[] = new GRBVar[starts.length];
257 | 		for(int a = 0; a < starts.length; a ++){
258 | 			xx[a] = model.addVar(1.0, 1.0, 0.0, GRB.BINARY, null);
259 | 		}
260 | 		model.update();
261 | 
262 | 		// Special case for last t, only need to setup single edges between respective start/goal
263 | 		for(int a = 0; a < starts.length; a ++){
264 | 			long edgeId = getEdgeId(a, goals[a], starts[a], timeSteps);
265 | 
266 | 			// Store edges indexed by vertices
267 | 			storeVarForVertices(xx[a], goals[a], starts[a], timeSteps, 0);
268 | 			storeVarForAVT(xx[a], a, goals[a], starts[a], timeSteps, 0);
269 | 			
270 | 			// Setup the objective exprssion 
271 | 			edgeVarMap.put(edgeId, xx[a]);
272 | 		}
273 | 		
274 | 		// For total time
275 | 		for(int a = 0; a < starts.length; a ++){
276 | 			yTVars[a] =  model.addVar(0.0, timeSteps, 0.0, GRB.INTEGER, null);
277 | 			for(int t = timeSteps - 1; t >= 0; t--){
278 | 				yVars[a][t] = model.addVar(0.0, 1.0, 0.0, GRB.BINARY, null);
279 | 			}
280 | 		}
281 | 		model.update();
282 | 		
283 | 		GRBLinExpr objExpr = new GRBLinExpr();
284 | 		for(int a = 0; a < starts.length; a ++){
285 | 			GRBLinExpr exprX = new GRBLinExpr();
286 | 			exprX.addTerm(1, yTVars[a]);
287 | 			objExpr.addTerm(1, yTVars[a]);
288 | 			
289 | 			for(int t = timeSteps - 1; t >= 0; t--){
290 | 				long edgeId = getEdgeId(a, goals[a], goals[a], t);
291 | 				if(edgeVarMap.containsKey(edgeId)){
292 | 					GRBVar x= edgeVarMap.get(edgeId);
293 | 					
294 | 					// Constraints on yVars[a][t]
295 | 					if(t == timeSteps - 1){
296 | 						GRBLinExpr expr = new GRBLinExpr();
297 | 						expr.addTerm(1, yVars[a][t]);
298 | 						expr.addTerm(-1, x);
299 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
300 | 					}
301 | 					else{
302 | 						GRBLinExpr expr = new GRBLinExpr();
303 | 						expr.addTerm(1, yVars[a][t]);
304 | 						expr.addTerm(-1, yVars[a][t + 1]);
305 | 						expr.addTerm(-1, x);
306 | 						model.addConstr(expr, GRB.GREATER_EQUAL, -1, null);
307 | 						
308 | 						expr = new GRBLinExpr();
309 | 						expr.addTerm(1, yVars[a][t]);
310 | 						expr.addTerm(-1, yVars[a][t + 1]);
311 | 						model.addConstr(expr, GRB.LESS_EQUAL, 0, null);
312 | 
313 | 						expr = new GRBLinExpr();
314 | 						expr.addTerm(1, yVars[a][t]);
315 | 						expr.addTerm(-1, x);
316 | 						model.addConstr(expr, GRB.LESS_EQUAL, 0, null);
317 | 					}
318 | 					exprX.addTerm(-1, yVars[a][t]);
319 | 					
320 | 				}
321 | 			}
322 | 			model.addConstr(exprX, GRB.EQUAL, 0, null);
323 | 		}
324 | 		model.update();
325 | 		model.setObjective(objExpr, GRB.MAXIMIZE);
326 | 	
327 | 		
328 | //		GRBLinExpr objExpr = new GRBLinExpr();
329 | //		objExpr.addTerm(1, x);
330 | //		model.setObjective(objExpr, GRB.MAXIMIZE);
331 | 		
332 | 
333 | 		// Set up constraints for edges going into a vertex
334 | 		for(int t = 0; t < timeSteps; t++){
335 | 
336 | 			// Set up variables for all edges and expression 
337 | 			for(int v = 0; v < graph.vertices.length; v++){
338 | 				GRBLinExpr expr = null;
339 | 				long vtId = getVertexTimeId(v, t + 1);
340 | 				Vector<GRBVar> inVarVec = vertexTimeInVarVectorMap.get(vtId);
341 | 				if(inVarVec != null){
342 | 					if(expr == null) expr = new GRBLinExpr();
343 | 					for(int i = 0; i < inVarVec.size(); i ++){
344 | 						expr.addTerm(1., inVarVec.get(i));
345 | 					}
346 | 				}
347 | 				if(expr != null){
348 | 					if(expr.size() > 1){
349 | 						model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
350 | 					}
351 | 					expr = null;
352 | 				}
353 | 			}
354 | 		}
355 | 		
356 | 		// Set up constraints for vertices
357 | 		for(int a = 0; a < starts.length; a ++){
358 | 			for(int t = 0; t <= timeSteps; t++){
359 | 				// Set up variables for all edges and expression 
360 | 				for(int v = 0; v < graph.vertices.length; v++){
361 | 					GRBLinExpr expr = null;
362 | 					long vtId = getAgentVertexTimeId(a, v, t);
363 | 					Vector<GRBVar> inVarVec = avtInVarVectorMap.get(vtId);
364 | 					if(inVarVec != null){
365 | 						expr = new GRBLinExpr();
366 | 						for(int i = 0; i < inVarVec.size(); i ++){
367 | 							expr.addTerm(-1., inVarVec.get(i));
368 | 						}
369 | 					}
370 | 					Vector<GRBVar> outVarVec = avtOutVarVectorMap.get(vtId);
371 | 					if(outVarVec != null){
372 | 						if(expr == null) expr = new GRBLinExpr();
373 | 						for(int i = 0; i < outVarVec.size(); i ++){
374 | 							expr.addTerm(1., outVarVec.get(i));
375 | 						}
376 | 					}
377 | 					if(expr != null){
378 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
379 | 						expr = null;
380 | 					}
381 | 				}
382 | 			}
383 | 		}
384 | 		
385 | 		// Setup constraint for single edges
386 | 		Long[] edgeTimeKeys = edgeTimeVarVectorMap.keySet().toArray(new Long[0]);
387 | 		for(int e = 0; e < edgeTimeKeys.length; e ++){
388 | 			GRBLinExpr expr = null;
389 | 			Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(edgeTimeKeys[e]);
390 | 			if(varVec != null){
391 | 				if(expr == null) expr = new GRBLinExpr();
392 | 				for(int i = 0; i < varVec.size(); i ++){
393 | 					expr.addTerm(1., varVec.get(i));
394 | 				}
395 | 			}
396 | 			if(expr != null && expr.size() > 1){
397 | 				model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
398 | 			}
399 | 			expr = null;
400 | 		}
401 | 		model.update();
402 | 		
403 | 		return model;
404 | 	}
405 | 	
406 | 	private void storeVarForAVT(GRBVar var, int a, int v1, int v2, int t1, int t2){
407 | 		// Add edge var to vertex time map, outgoing first
408 | 		long vtId = getAgentVertexTimeId(a, v1, t1);
409 | 		Vector<GRBVar> varVec = avtOutVarVectorMap.get(vtId);
410 | 		if(varVec == null){
411 | 			varVec = new Vector<GRBVar>();
412 | 			avtOutVarVectorMap.put(vtId, varVec);
413 | 		}
414 | 		varVec.add(var);
415 | 		
416 | 		// Incoming
417 | 		vtId = getAgentVertexTimeId(a, v2, t2);
418 | 		varVec = avtInVarVectorMap.get(vtId);
419 | 		if(varVec == null){
420 | 			varVec = new Vector<GRBVar>();
421 | 			avtInVarVectorMap.put(vtId, varVec);
422 | 		}
423 | 		varVec.add(var);
424 | 	}
425 | 
426 | 	/**
427 | 	 * Store edge var for head and tail vertices
428 | 	 * @param var
429 | 	 * @param v1
430 | 	 * @param v2
431 | 	 * @param t1
432 | 	 * @param t2
433 | 	 */
434 | 	private void storeVarForVertices(GRBVar var, int v1, int v2, int t1, int t2){
435 | 		// Add edge var to vertex time map, outgoing first
436 | 		long vtId = getVertexTimeId(v1, t1);
437 | 		Vector<GRBVar> varVec = vertexTimeOutVarVectorMap.get(vtId);
438 | 		if(varVec == null){
439 | 			varVec = new Vector<GRBVar>();
440 | 			vertexTimeOutVarVectorMap.put(vtId, varVec);
441 | 		}
442 | 		varVec.add(var);
443 | 		
444 | 		// Incoming
445 | 		vtId = getVertexTimeId(v2, t2);
446 | 		varVec = vertexTimeInVarVectorMap.get(vtId);
447 | 		if(varVec == null){
448 | 			varVec = new Vector<GRBVar>();
449 | 			vertexTimeInVarVectorMap.put(vtId, varVec);
450 | 		}
451 | 		varVec.add(var);
452 | 	}
453 | 
454 | 	/**
455 | 	 * Store edge var for one edge in the original graph with different time stamps
456 | 	 * @param var
457 | 	 * @param v1
458 | 	 * @param v2
459 | 	 * @param t
460 | 	 */
461 | 	private void storeVarForEdges(GRBVar var, int v1, int v2, int t){
462 | 		// Add edge var to edge time map
463 | 		long etId = getEdgeTimeId(v1, v2, t);
464 | 		Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(etId);
465 | 		if(varVec == null){
466 | 			varVec = new Vector<GRBVar>();
467 | 			edgeTimeVarVectorMap.put(etId, varVec);
468 | 		}
469 | 		varVec.add(var);
470 | 	}
471 | 	
472 | 	@Override
473 | 	protected void callback() {
474 | 		try{
475 | 			if (where == GRB.CB_MIP) {
476 | 				if(expectedBound > getDoubleInfo(GRB.Callback.MIP_OBJBND)){
477 | 					currentModel.terminate();
478 | 				}
479 | 			}
480 | 		} catch (GRBException e) {
481 | 			System.out.println("Error code: " + e.getErrorCode() + ". "
482 | 					+ e.getMessage());
483 | 			e.printStackTrace();
484 | 		}
485 | 
486 | 	}
487 | 
488 | 
489 | }
490 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/MultiagentGraphSolverGurobiTimeR.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.ILP;
  2 | 
  3 | import gurobi.GRB;
  4 | import gurobi.GRBCallback;
  5 | import gurobi.GRBEnv;
  6 | import gurobi.GRBException;
  7 | import gurobi.GRBLinExpr;
  8 | import gurobi.GRBModel;
  9 | import gurobi.GRBVar;
 10 | 
 11 | import java.util.HashSet;
 12 | import java.util.Set;
 13 | import java.util.SortedMap;
 14 | import java.util.TreeMap;
 15 | import java.util.Vector;
 16 | 
 17 | import projects.multipath.advanced.Graph;
 18 | import projects.multipath.advanced.Path;
 19 | 
 20 | /**
 21 |  * This class allows a single move per time step, to simulate the PMG puzzle setup  
 22 |  * @author Jingjin
 23 |  *
 24 |  */
 25 | public class MultiagentGraphSolverGurobiTimeR extends GRBCallback{
 26 | 
 27 | 	public static boolean bPrintPaths = true;
 28 | 	public static boolean bDebugInfo = true;
 29 | 	
 30 | 	private SortedMap<Long, GRBVar> edgeVarMap = new TreeMap<Long, GRBVar>();
 31 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 32 | 	private SortedMap<Long, Vector<GRBVar>> vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 33 | 	private SortedMap<Long, Vector<GRBVar>> avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 34 | 	private SortedMap<Long, Vector<GRBVar>> avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 35 | 	private SortedMap<Long, Vector<GRBVar>> edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
 36 | 	private boolean reachableVertices[][][] = null;
 37 | 	private GRBModel currentModel = null;
 38 | 	private int expectedBound = 0;
 39 | 			
 40 | 	protected int[][] solve(Graph graph, int starts[], int goals[], boolean solveRelaxedFirst, boolean setOptimal, int extraSteps, boolean hasTimeLimit, double timeLimit) throws GRBException{
 41 | 		long startTime = System.currentTimeMillis(); 
 42 | 		
 43 | 		// Check minimal distances necessary for solving the problem
 44 | 		int startTimeSteps = 0;
 45 | 		Path paths[] = PathFinder.findShortestPaths(graph, starts, goals);
 46 | 		for(int i = 0; i < paths.length; i ++){
 47 | 			startTimeSteps += paths[i].vertexList.size() - 1;
 48 | 		}
 49 | 		
 50 | 		// Create LP solver
 51 | 		GRBEnv env = new GRBEnv();
 52 | 		env.set(GRB.IntParam.OutputFlag, 0);
 53 | 		
 54 | 		// Solve the LP relaxation
 55 | 		startTimeSteps += extraSteps;
 56 | 		if(bDebugInfo)System.out.println("\nUsing " + extraSteps + " extra steps.");
 57 | 		updateReachableSets(graph, starts, goals, startTimeSteps);
 58 | 		if(solveRelaxedFirst && extraSteps == 0){
 59 | 			System.out.println("\nSolving relaxed model first. ");
 60 | 			while(true){
 61 | 				timeLimit = timeLimit - (System.currentTimeMillis() - startTime)/1000.0;
 62 | 				System.out.print("Trying T = " + startTimeSteps + (hasTimeLimit?(" with max time limit " + timeLimit):"") + " ... ");
 63 | 				GRBModel model = prepareModel(env, graph, starts, goals, false, false, startTimeSteps, hasTimeLimit ? timeLimit : -1);
 64 | 				model.relax();
 65 | 				model.optimize();
 66 | 				double bestBet = model.get(GRB.DoubleAttr.ObjVal);
 67 | 				model.dispose();
 68 | 				if(bestBet + 0.01 > starts.length){
 69 | 					if(hasTimeLimit && timeLimit - (System.currentTimeMillis() - startTime)/1000.0 < 0){
 70 | 						System.out.println("relaxed model not feasible, time limit reached.");
 71 | 						env.dispose();
 72 | 						return null;
 73 | 					}
 74 | 					System.out.println("relaxed model feasible.");
 75 | 					break;
 76 | 				}
 77 | 				else if(hasTimeLimit && timeLimit - (System.currentTimeMillis() - startTime)/1000.0 < 0){
 78 | 					System.out.println("relaxed model not feasible, time limit reached.");
 79 | 					env.dispose();
 80 | 					return null;
 81 | 				}
 82 | 				System.out.println("relaxed model not feasible, increase T...");
 83 | 				startTimeSteps += 1;
 84 | 				updateReachableSets(graph, starts, goals, startTimeSteps);
 85 | 			}
 86 | 		}
 87 | 		
 88 | 		// Solve the ILP
 89 | 		int val = 0; 
 90 | 		int ps[][] = null;
 91 | 		while(val != starts.length){
 92 | 			timeLimit = timeLimit - (System.currentTimeMillis() - startTime)/1000.0;
 93 | 			if(bDebugInfo){
 94 | 				System.out.print("Trying T = " + startTimeSteps + " for the integer model " + (hasTimeLimit?("with max time limit " + timeLimit):"") + "...");
 95 | 			}
 96 | 			GRBModel model = prepareModel(env, graph, starts, goals, true, setOptimal, startTimeSteps, hasTimeLimit ? timeLimit : -1);
 97 | 			expectedBound = starts.length;
 98 | //			model.setCallback(this);
 99 | 			model.optimize();
100 | //			model.setCallback(null);
101 | 			if(model.get(GRB.IntAttr.SolCount) > 0){
102 | 				val = (int) (model.get(GRB.DoubleAttr.ObjVal));
103 | 			}
104 | 			if(val == starts.length){
105 | 				if(bDebugInfo){
106 | 					System.out.println(" done.\n");
107 | 					System.out.println("Solution with optimal makespan found in " + (System.currentTimeMillis() - startTime)/1000.0 + " seconds.");
108 | 				}
109 | 				ps = retrievePaths(model, graph, starts, startTimeSteps);
110 | 			}
111 | 			else{
112 | 				if(hasTimeLimit && (timeLimit - (System.currentTimeMillis() - startTime)/1000.0) < 0){
113 | 					if(bDebugInfo){
114 | 						System.out.println(" time out.");
115 | 					}
116 | 					model.dispose();
117 | 					env.dispose(); 
118 | 					return null;
119 | 				}
120 | 				else{
121 | 					if(bDebugInfo){
122 | 						System.out.println(" integer model infeasible.");
123 | 					}
124 | 				}
125 | 			}
126 | 			startTimeSteps ++;
127 | 			updateReachableSets(graph, starts, goals, startTimeSteps);
128 | 			model.dispose();
129 | 		}
130 | 		env.dispose();
131 | 		
132 | 		return ps;
133 | 	}
134 | 	
135 | 	private void updateReachableSets(Graph graph, int starts[], int goals[], int startTimeSteps){
136 | 		/**
137 | 		 *  Preparation. First compute whether a vertex is reachable by an agent at a time step.
138 | 		 *  For each agent with start x_i and goal x_g, a vertex v at time t is reachable if  
139 | 		 *  dist(x_i, v) <=  t and dist(v, x_g) <= T - t.   
140 | 		 */
141 | 		reachableVertices = new boolean[starts.length][graph.vertices.length][startTimeSteps + 1];
142 | 		for(int a = 0; a < starts.length; a ++){
143 | 			// Compute the distances from start/goal to all vertices of the graph   
144 | 			int[] distFromStart = PathFinder.getDistancesToAllVertices(graph, starts[a], null); 
145 | 			int[] distFromGoal = PathFinder.getDistancesToAllVertices(graph, goals[a], null);
146 | 			
147 | 			// Assign reachability values
148 | 			for(int t = 0; t <= startTimeSteps; t ++){
149 | 				for(int v = 0; v < graph.vertices.length; v ++){
150 | 					if(distFromStart[v] <= t && distFromGoal[v] <= startTimeSteps - t){
151 | 						reachableVertices[a][v][t] = true;
152 | 					}
153 | 					else{
154 | 						reachableVertices[a][v][t] = false;
155 | 					}
156 | 				}
157 | 			}
158 | 		}
159 | 		
160 | 	}
161 | 	
162 | 	private int getTimeFromEdgeId(long edgeId){
163 | 		return (int)(edgeId%10000);
164 | 	}
165 | 	
166 | 	private boolean isV1V2Same(long edgeId){
167 | 		long v1 = (edgeId%100000000)/10000; 
168 | 		long v2 = (edgeId%1000000000000L)/100000000; 
169 | 		return v1 == v2;
170 | 	}
171 | 	
172 | 	
173 | 	private long getEdgeId(int a, int v1, int v2, int t) {
174 | 		return a * 1000000000000L + v1 * 100000000 + v2 * 10000 + t;
175 | 	}
176 | 
177 | 	private long getVertexTimeId(int v, int t) {
178 | 		return v * 10000 + t;
179 | 	}
180 | 
181 | 	private long getAgentVertexTimeId(int a, int v, int t) {
182 | 		return a * 1000000000000L + v * 10000 + t;
183 | 	}
184 | 
185 | 	private long getEdgeTimeId(int v1, int v2, int t) {
186 | 		if(v1 > v2){
187 | 			int temp = v1; 
188 | 			v1 = v2;
189 | 			v2 = temp;
190 | 		}
191 | 		return v1 * 1000000000 + v2* 10000 + t;
192 | 	}
193 | 
194 | 	private int[][] retrievePaths(GRBModel model, Graph graph, int starts[], int timeSteps) throws GRBException{
195 | 		int paths[][] = new int[starts.length][timeSteps + 1];
196 | 		for(int a = 0; a < starts.length; a ++){
197 | 			if(bPrintPaths){
198 | 				System.out.print("Agent " + a + ": 0:");
199 | 				graph.vertices[starts[a]].printVertex();
200 | 			}
201 | 			paths[a][0] = starts[a];
202 | 			for(int t = 1; t <= timeSteps; t ++){
203 | 				Integer nvs[] = graph.adjacencySetMap.get(paths[a][t-1]).toArray(new Integer[0]);
204 | 				for(int nv = 0; nv < nvs.length; nv ++){
205 | 					if(isEdgeVarSetToTrue(model, a, paths[a][t-1], nvs[nv], t-1)){
206 | 						paths[a][t] = nvs[nv];
207 | 						break;
208 | 					}
209 | 				}
210 | 				if(bPrintPaths){
211 | 					System.out.print(" " + t + ":");
212 | 					graph.vertices[paths[a][t]].printVertex();
213 | 				}
214 | 			}
215 | 			if(bPrintPaths)System.out.println();
216 | 		}
217 | 		return paths;
218 | 	}
219 | 	
220 | 	private boolean isEdgeVarSetToTrue(GRBModel model, int a, int v1, int v2, int t) throws GRBException{
221 | 		long eId = getEdgeId(a, v1, v2, t);
222 | 		GRBVar var = edgeVarMap.get(eId);
223 | 		if(var != null && var.get(GRB.DoubleAttr.X) == 1.0){
224 | 			return true;
225 | 		}
226 | 		return false;
227 | 	}
228 | 	
229 | 	private GRBModel prepareModel(GRBEnv env, Graph graph, int starts[], int goals[], boolean integer, boolean setOptimal, int timeSteps, double timeLimit) throws GRBException{
230 | 		// Reset global storage
231 | 		edgeVarMap = new TreeMap<Long, GRBVar>();
232 | 		vertexTimeInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
233 | 		vertexTimeOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
234 | 		avtInVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
235 | 		avtOutVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
236 | 		edgeTimeVarVectorMap = new TreeMap<Long, Vector<GRBVar>>();
237 | 		
238 | 		// Suppress output if we are solving the relaxation
239 | 		if(bDebugInfo){env.set(GRB.IntParam.OutputFlag, 1);}
240 | 		else{env.set(GRB.IntParam.OutputFlag, 0);}
241 | 
242 | 		// Time limit?
243 | 		if(timeLimit > 0)
244 | 		{ 
245 | 			env.set(GRB.DoubleParam.TimeLimit, timeLimit); 
246 | 		}
247 | 		else{
248 | 			env.set(GRB.DoubleParam.TimeLimit, 10000000000.0);
249 | 		}
250 | 		
251 | 		// Setup model
252 | 		GRBModel model = new GRBModel(env);
253 | 		currentModel = model;
254 | 
255 | 		// Set up variables for all edges, agent by agent
256 | 		for(int a = 0; a < starts.length; a++){
257 | 			Set<Integer> rVs = new HashSet<Integer>();
258 | 			rVs.add(starts[a]);
259 | 			for(int t = 1; t <= timeSteps; t++){
260 | 				Integer[] rvs = rVs.toArray(new Integer[0]);
261 | 				for(int rv = 0; rv < rvs.length; rv ++){
262 | 					int curV = rvs[rv];
263 | 					Integer[] adjVs = graph.adjacencySetMap.get(curV).toArray(new Integer[0]);
264 | 					for(int i = 0; i < adjVs.length; i ++){
265 | 						int nextV = adjVs[i];
266 | 						if(reachableVertices[a][nextV][t]){
267 | 							rVs.add(nextV);
268 | 							GRBVar x = model.addVar(0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
269 | 							long edgeId = getEdgeId(a, curV, nextV, t - 1);
270 | 							edgeVarMap.put(edgeId, x);
271 | 
272 | 							// Store edges indexed by vertices
273 | 							storeVarForVertices(x, curV, nextV, t - 1, t);
274 | 							storeVarForAVT(x, a, curV, nextV, t - 1, t);
275 | 
276 | 							// Store edges indexed by edges, if the start/goal vertice are different
277 | 							if(curV != nextV){
278 | 								storeVarForEdges(x, curV, nextV, t - 1);
279 | 							}
280 | 						}
281 | 					}
282 | 
283 | 					// Remove current vertex if it's no longer reachable
284 | 					if(!reachableVertices[a][curV][t]){
285 | 						rVs.remove(curV);
286 | 					}
287 | 				}
288 | 			}
289 | 		}
290 | 		model.update();
291 | 		
292 | 		// Ensure that at each time step, only one agent can take a step
293 | 		Long[] edgeIds = edgeVarMap.keySet().toArray(new Long[0]);
294 | 		for(int t = 0; t <= timeSteps; t++){
295 | 			GRBLinExpr expr = new GRBLinExpr();
296 | 			for(int e = 0; e < edgeIds.length; e++){
297 | 				if(getTimeFromEdgeId(edgeIds[e]) == t && !isV1V2Same(edgeIds[e])){
298 | 					expr.addTerm(1, edgeVarMap.get(edgeIds[e]));
299 | 				}
300 | 			}
301 | 			model.addConstr(expr, GRB.LESS_EQUAL, 1, null);			
302 | 		}
303 | 		
304 | 		
305 | 		// Special case for last t, only need to setup single edges between respective start/goal
306 | 		GRBLinExpr objExpr = new GRBLinExpr();
307 | 		for(int a = 0; a < starts.length; a ++){
308 | 			long edgeId = getEdgeId(a, goals[a], starts[a], timeSteps);
309 | 			GRBVar x = model.addVar(setOptimal?1.0:0.0, 1.0, 0.0, integer?GRB.BINARY:GRB.CONTINUOUS, null);
310 | 			model.update();
311 | 
312 | 			// Store edges indexed by vertices
313 | 			storeVarForVertices(x, goals[a], starts[a], timeSteps, 0);
314 | 			storeVarForAVT(x, a, goals[a], starts[a], timeSteps, 0);
315 | 			
316 | 			// Setup the objective exprssion 
317 | 			objExpr.addTerm(1, x);
318 | 			edgeVarMap.put(edgeId, x);
319 | 		}
320 | 		model.setObjective(objExpr, GRB.MAXIMIZE);
321 | 		
322 | 
323 | 		// Set up constraints for edges going into a vertex
324 | 		for(int t = 0; t < timeSteps; t++){
325 | 
326 | 			// Set up variables for all edges and expression 
327 | 			for(int v = 0; v < graph.vertices.length; v++){
328 | 				GRBLinExpr expr = null;
329 | 				long vtId = getVertexTimeId(v, t + 1);
330 | 				Vector<GRBVar> inVarVec = vertexTimeInVarVectorMap.get(vtId);
331 | 				if(inVarVec != null){
332 | 					if(expr == null) expr = new GRBLinExpr();
333 | 					for(int i = 0; i < inVarVec.size(); i ++){
334 | 						expr.addTerm(1., inVarVec.get(i));
335 | 					}
336 | 				}
337 | 				if(expr != null){
338 | 					if(expr.size() > 1){
339 | 						model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
340 | 					}
341 | 					expr = null;
342 | 				}
343 | 			}
344 | 		}
345 | 		
346 | 		// Set up constraints for vertices
347 | 		for(int a = 0; a < starts.length; a ++){
348 | 			for(int t = 0; t <= timeSteps; t++){
349 | 				// Set up variables for all edges and expression 
350 | 				for(int v = 0; v < graph.vertices.length; v++){
351 | 					GRBLinExpr expr = null;
352 | 					long vtId = getAgentVertexTimeId(a, v, t);
353 | 					Vector<GRBVar> inVarVec = avtInVarVectorMap.get(vtId);
354 | 					if(inVarVec != null){
355 | 						expr = new GRBLinExpr();
356 | 						for(int i = 0; i < inVarVec.size(); i ++){
357 | 							expr.addTerm(-1., inVarVec.get(i));
358 | 						}
359 | 					}
360 | 					Vector<GRBVar> outVarVec = avtOutVarVectorMap.get(vtId);
361 | 					if(outVarVec != null){
362 | 						if(expr == null) expr = new GRBLinExpr();
363 | 						for(int i = 0; i < outVarVec.size(); i ++){
364 | 							expr.addTerm(1., outVarVec.get(i));
365 | 						}
366 | 					}
367 | 					if(expr != null){
368 | 						model.addConstr(expr, GRB.EQUAL, 0, null);
369 | 						expr = null;
370 | 					}
371 | 				}
372 | 			}
373 | 		}
374 | 		
375 | 		// Setup constraint for single edges
376 | 		Long[] edgeTimeKeys = edgeTimeVarVectorMap.keySet().toArray(new Long[0]);
377 | 		for(int e = 0; e < edgeTimeKeys.length; e ++){
378 | 			GRBLinExpr expr = null;
379 | 			Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(edgeTimeKeys[e]);
380 | 			if(varVec != null){
381 | 				if(expr == null) expr = new GRBLinExpr();
382 | 				for(int i = 0; i < varVec.size(); i ++){
383 | 					expr.addTerm(1., varVec.get(i));
384 | 				}
385 | 			}
386 | 			if(expr != null && expr.size() > 1){
387 | 				model.addConstr(expr, GRB.LESS_EQUAL, 1, null);
388 | 			}
389 | 			expr = null;
390 | 		}
391 | 		model.update();
392 | 		
393 | 		return model;
394 | 	}
395 | 	
396 | 	private void storeVarForAVT(GRBVar var, int a, int v1, int v2, int t1, int t2){
397 | 		// Add edge var to vertex time map, outgoing first
398 | 		long vtId = getAgentVertexTimeId(a, v1, t1);
399 | 		Vector<GRBVar> varVec = avtOutVarVectorMap.get(vtId);
400 | 		if(varVec == null){
401 | 			varVec = new Vector<GRBVar>();
402 | 			avtOutVarVectorMap.put(vtId, varVec);
403 | 		}
404 | 		varVec.add(var);
405 | 		
406 | 		// Incoming
407 | 		vtId = getAgentVertexTimeId(a, v2, t2);
408 | 		varVec = avtInVarVectorMap.get(vtId);
409 | 		if(varVec == null){
410 | 			varVec = new Vector<GRBVar>();
411 | 			avtInVarVectorMap.put(vtId, varVec);
412 | 		}
413 | 		varVec.add(var);
414 | 	}
415 | 
416 | 	/**
417 | 	 * Store edge var for head and tail vertices
418 | 	 * @param var
419 | 	 * @param v1
420 | 	 * @param v2
421 | 	 * @param t1
422 | 	 * @param t2
423 | 	 */
424 | 	private void storeVarForVertices(GRBVar var, int v1, int v2, int t1, int t2){
425 | 		// Add edge var to vertex time map, outgoing first
426 | 		long vtId = getVertexTimeId(v1, t1);
427 | 		Vector<GRBVar> varVec = vertexTimeOutVarVectorMap.get(vtId);
428 | 		if(varVec == null){
429 | 			varVec = new Vector<GRBVar>();
430 | 			vertexTimeOutVarVectorMap.put(vtId, varVec);
431 | 		}
432 | 		varVec.add(var);
433 | 		
434 | 		// Incoming
435 | 		vtId = getVertexTimeId(v2, t2);
436 | 		varVec = vertexTimeInVarVectorMap.get(vtId);
437 | 		if(varVec == null){
438 | 			varVec = new Vector<GRBVar>();
439 | 			vertexTimeInVarVectorMap.put(vtId, varVec);
440 | 		}
441 | 		varVec.add(var);
442 | 	}
443 | 
444 | 	/**
445 | 	 * Store edge var for one edge in the original graph with different time stamps
446 | 	 * @param var
447 | 	 * @param v1
448 | 	 * @param v2
449 | 	 * @param t
450 | 	 */
451 | 	private void storeVarForEdges(GRBVar var, int v1, int v2, int t){
452 | 		// Add edge var to edge time map
453 | 		long etId = getEdgeTimeId(v1, v2, t);
454 | 		Vector<GRBVar> varVec = edgeTimeVarVectorMap.get(etId);
455 | 		if(varVec == null){
456 | 			varVec = new Vector<GRBVar>();
457 | 			edgeTimeVarVectorMap.put(etId, varVec);
458 | 		}
459 | 		varVec.add(var);
460 | 	}
461 | 	
462 | 	@Override
463 | 	protected void callback() {
464 | 		try{
465 | 			if (where == GRB.CB_MIP) {
466 | 				if(expectedBound > getDoubleInfo(GRB.Callback.MIP_OBJBND)){
467 | 					currentModel.terminate();
468 | 				}
469 | 			}
470 | 		} catch (GRBException e) {
471 | 			System.out.println("Error code: " + e.getErrorCode() + ". "
472 | 					+ e.getMessage());
473 | 			e.printStackTrace();
474 | 		}
475 | 
476 | 	}
477 | 
478 | 
479 | }
480 | 


--------------------------------------------------------------------------------
/source/projects/multipath/advanced/Graph.java:
--------------------------------------------------------------------------------
  1 | package projects.multipath.advanced;
  2 | 
  3 | import java.awt.BasicStroke;
  4 | import java.awt.Graphics2D;
  5 | import java.awt.geom.Point2D;
  6 | import java.io.File;
  7 | import java.io.IOException;
  8 | import java.io.ObjectInputStream;
  9 | import java.io.ObjectOutputStream;
 10 | import java.io.PrintWriter;
 11 | import java.io.Serializable;
 12 | import java.util.HashMap;
 13 | import java.util.HashSet;
 14 | import java.util.LinkedList;
 15 | import java.util.List;
 16 | import java.util.Map;
 17 | import java.util.Set;
 18 | import java.util.SortedMap;
 19 | import java.util.TreeMap;
 20 | import java.util.TreeSet;
 21 | 
 22 | public class Graph implements Serializable{
 23 | 	private static final long serialVersionUID = 1L;
 24 | 	
 25 | 	// Vertex set
 26 | 	public Set<Vertex> verticeSet = new HashSet<Vertex>();
 27 | 	public Vertex[] vertices = null;
 28 | 	
 29 | 	// Vertex id set
 30 | 	public SortedMap<Integer, Vertex> idVertexMap = new TreeMap<Integer, Vertex>();
 31 | 	
 32 | 	// Adjacency list (as a set) map
 33 | 	public Map<Integer, Set<Integer>> adjacencySetMap = new HashMap<Integer, Set<Integer>>();
 34 | 
 35 | 	// For rectangle
 36 | 	int rows, columns; 
 37 | 	
 38 | 	/**
 39 | 	 * Create a 2D grid with some rows and columns
 40 | 	 * @param rows
 41 | 	 * @param cols
 42 | 	 * @return
 43 | 	 */
 44 | 	public static Graph create2DGridGraph(int rows, int cols, boolean update){
 45 | 		Graph g = new Graph();
 46 | 		g.rows = rows;
 47 | 		g.columns = cols;
 48 | 		// Create vertices
 49 | 		for(int i = 0; i < rows; i ++){
 50 | 			for(int j = 0; j < cols; j ++){
 51 | 				Vertex v = new Vertex(new Point2D.Double(j, i));
 52 | 				v.id = g.getId(j, i);
 53 | 				g.verticeSet.add(v);
 54 | 				g.idVertexMap.put(v.id, v);
 55 | 			}
 56 | 		}
 57 | 
 58 | 		for(int i = 0; i < rows; i ++){
 59 | 			for(int j = 0; j < cols; j ++){
 60 | 				int id = g.getId(j, i);
 61 | 				Set<Integer> nbrs = new TreeSet<Integer>();
 62 | 				g.adjacencySetMap.put(id, nbrs);
 63 | 				nbrs.add(id);
 64 | 				nbrs.add(id - 1);
 65 | 				nbrs.add(id + 1);
 66 | 				nbrs.add(id - cols);
 67 | 				nbrs.add(id + cols);
 68 | 				
 69 | 				if(i == 0){
 70 | 					nbrs.remove(id - cols);
 71 | 				}
 72 | 				if(j == 0){
 73 | 					nbrs.remove(id - 1);
 74 | 				}
 75 | 				if(i == rows - 1){
 76 | 					nbrs.remove(id + cols);
 77 | 				}
 78 | 				if(j == cols - 1){
 79 | 					nbrs.remove(id + 1);
 80 | 				}
 81 | 			}
 82 | 		}
 83 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
 84 | 		if(!update){
 85 | 			return g;
 86 | 		}
 87 | 		return Graph.convertGraph(g, new int[0], new int[0]);
 88 | 	}
 89 | 
 90 | 	/**
 91 | 	 * Create a 2D grid with some rows and columns
 92 | 	 * @param rows
 93 | 	 * @param cols
 94 | 	 * @return
 95 | 	 */
 96 | 	public static Graph create2DGridGraph8Connected(int rows, int cols, boolean update){
 97 | 		Graph g = new Graph();
 98 | 		g.rows = rows;
 99 | 		g.columns = cols;
100 | 		// Create vertices
101 | 		for(int i = 0; i < rows; i ++){
102 | 			for(int j = 0; j < cols; j ++){
103 | 				Vertex v = new Vertex(new Point2D.Double(j, i));
104 | 				v.id = g.getId(j, i);
105 | 				g.verticeSet.add(v);
106 | 				g.idVertexMap.put(v.id, v);
107 | 			}
108 | 		}
109 | 
110 | 		for(int i = 0; i < rows; i ++){
111 | 			for(int j = 0; j < cols; j ++){
112 | 				int id = g.getId(j, i);
113 | 				Set<Integer> nbrs = new TreeSet<Integer>();
114 | 				g.adjacencySetMap.put(id, nbrs);
115 | 				nbrs.add(id);
116 | 				nbrs.add(id - 1);
117 | 				nbrs.add(id + 1);
118 | 				nbrs.add(id - cols);
119 | 				nbrs.add(id + cols);
120 | 				nbrs.add(id - cols - 1);
121 | 				nbrs.add(id - cols + 1);
122 | 				nbrs.add(id + cols - 1);
123 | 				nbrs.add(id + cols + 1);
124 | 				
125 | 				if(i == 0){
126 | 					nbrs.remove(id - cols);
127 | 					nbrs.remove(id - cols - 1);
128 | 					nbrs.remove(id - cols + 1);
129 | 				}
130 | 				if(j == 0){
131 | 					nbrs.remove(id - 1);
132 | 					nbrs.remove(id - cols - 1);
133 | 					nbrs.remove(id + cols - 1);
134 | 				}
135 | 				if(i == rows - 1){
136 | 					nbrs.remove(id + cols);
137 | 					nbrs.remove(id + cols - 1);
138 | 					nbrs.remove(id + cols + 1);
139 | 				}
140 | 				if(j == cols - 1){
141 | 					nbrs.remove(id + 1);
142 | 					nbrs.remove(id - cols + 1);
143 | 					nbrs.remove(id + cols + 1);
144 | 				}
145 | 			}
146 | 		}
147 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
148 | 		if(!update){
149 | 			return g;
150 | 		}
151 | 		return Graph.convertGraph(g, new int[0], new int[0]);
152 | 	}
153 | 
154 | 	/**
155 | 	 * Create a grid with some vertices removed, keeping the graph connected. 
156 | 	 * @param rows
157 | 	 * @param cols
158 | 	 * @param percentObstacle
159 | 	 * @return
160 | 	 */
161 | 	public static Graph createGeneric2DGridGraphWithHoles8Connected(int rows, int cols, double percentObstacle){
162 | 		Graph g = Graph.create2DGridGraph8Connected(rows, cols, false);
163 | 		int verticesToRemove = (int)(rows*cols*percentObstacle);
164 | 		while(verticesToRemove > 0){
165 | 			int yToRemove =  (int)(Math.random()*rows);
166 | 			int xToRemove =  (int)(Math.random()*cols);
167 | 			int vId = g.getId(xToRemove, yToRemove);
168 | 			if(g.idVertexMap.containsKey(vId)){
169 | 				if(g.isConnectedWithoutVertex(vId)){
170 | 					g.removeVertex(xToRemove, yToRemove);
171 | 					verticesToRemove --;
172 | 				}
173 | 			}
174 | 		}
175 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
176 | 		return g;
177 | 	}
178 | 
179 | 	/**
180 | 	 * Create a grid with some vertices removed, keeping the graph connected. 
181 | 	 * @param rows
182 | 	 * @param cols
183 | 	 * @param percentObstacle
184 | 	 * @return
185 | 	 */
186 | 	public static Graph createGeneric2DGridGraphWithHoles(int rows, int cols, double percentObstacle){
187 | 		Graph g = Graph.create2DGridGraph(rows, cols, false);
188 | 		int verticesToRemove = (int)(rows*cols*percentObstacle);
189 | 		while(verticesToRemove > 0){
190 | 			int yToRemove =  (int)(Math.random()*rows);
191 | 			int xToRemove =  (int)(Math.random()*cols);
192 | 			int vId = g.getId(xToRemove, yToRemove);
193 | 			if(g.idVertexMap.containsKey(vId)){
194 | 				if(g.isConnectedWithoutVertex(vId)){
195 | 					g.removeVertex(xToRemove, yToRemove);
196 | 					verticesToRemove --;
197 | 				}
198 | 			}
199 | 		}
200 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
201 | 		return g;
202 | 	}
203 | 	
204 | 	/**
205 | 	 * Randomly pick some start and goal locations. This function works well only when number is
206 | 	 * relatively small compared to the number of vertices. 
207 | 	 * @param g
208 | 	 * @param number
209 | 	 * @return
210 | 	 */
211 | 	public static int[][] getRandomStartGoal(Graph g, int number){
212 | 		Set<Integer> startSet = new HashSet<Integer>();
213 | 		Set<Integer> goalSet = new HashSet<Integer>();
214 | 		int ret[][] = new int[2][number];
215 | 		for(int i = 0; i < number; i ++){
216 | 			while(true){
217 | 				if(g.columns != 0){
218 | 					// Create start
219 | 					int x = (int)(Math.random()*g.columns);
220 | 					int y = (int)(Math.random()*g.rows);
221 | 					int id = g.getId(x, y);
222 | 					if(startSet.contains(id) || g.idVertexMap.get(id) == null) continue;
223 | 					int x2 = (int)(Math.random()*g.columns);
224 | 					int y2 = (int)(Math.random()*g.rows);
225 | 					int id2 = g.getId(x2, y2);
226 | 					if(goalSet.contains(id2) || g.idVertexMap.get(id2) == null) continue;
227 | 					startSet.add(id);
228 | 					goalSet.add(id2);
229 | 					ret[0][i] = id;
230 | 					ret[1][i] = id2;
231 | 					break;
232 | 				}
233 | 				else {
234 | 					// Create start
235 | 					int id = (int)(Math.random()*g.vertices.length);
236 | 					if(startSet.contains(id) || g.idVertexMap.get(id) == null) continue;
237 | 					int id2 = (int)(Math.random()*g.vertices.length);
238 | 					if(goalSet.contains(id2) || g.idVertexMap.get(id2) == null) continue;
239 | 					startSet.add(id);
240 | 					goalSet.add(id2);
241 | 					ret[0][i] = id;
242 | 					ret[1][i] = id2;
243 | 					break;
244 | 				}
245 | 			}
246 | 		}
247 | 		return ret;
248 | 	}
249 | 	
250 | 	/**
251 | 	 * Randomly pick start and goals for many agents (~ number of vertices)
252 | 	 * @param g
253 | 	 * @param number
254 | 	 * @return
255 | 	 */
256 | 	public static int[][] getRandomStartGoalMany(Graph g, int number){
257 | 		List<Integer> startList = new LinkedList<Integer>();
258 | 		List<Integer> goalList = new LinkedList<Integer>();
259 | 		for(int i = 0; i <g.vertices.length; i++){
260 | 			startList.add(g.vertices[i].id);
261 | 			goalList.add(g.vertices[i].id);
262 | 		}
263 | 		
264 | 		int ret[][] = new int[2][number];
265 | 		for(int i = 0; i < number; i ++){
266 | 			while(true){
267 | 				ret[1][i] = startList.remove((int)(Math.random()*startList.size()));
268 | 				ret[0][i] = goalList.remove(0);
269 | 				break;
270 | 			}
271 | 		}
272 | 		return ret;
273 | 	}
274 | 
275 | 	/**
276 | 	 * Convert the graph so that all vertices are reorderd with consecutive ids that
277 | 	 * starts with 0. 
278 | 	 * @param graph
279 | 	 * @param start
280 | 	 * @param goal
281 | 	 * @return
282 | 	 */
283 | 	protected static Graph convertGraph(Graph graph, int start[], int goal[]){
284 | 		Graph og = new Graph();
285 | 		Map<Integer, Integer> vIdIndexMap = new HashMap<Integer, Integer>();
286 | 		SortedMap<Integer, Vertex> indexVertexMap = new TreeMap<Integer,Vertex>();
287 | 		for(int i = 0; i < graph.vertices.length; i ++){
288 | 			indexVertexMap.put(graph.vertices[i].id, graph.vertices[i]);
289 | 		}
290 | 		Vertex vs[] = indexVertexMap.values().toArray(new Vertex[0]);
291 | 		for(int i = 0; i < graph.vertices.length; i ++){
292 | 			vIdIndexMap.put(vs[i].id, i);
293 | 		}
294 | 		og.vertices = new Vertex[graph.vertices.length];
295 | 		for(int i = 0; i < graph.vertices.length; i ++){
296 | 			og.vertices[i] = new Vertex(vs[i].point);
297 | 			og.vertices[i].id = i;
298 | 			og.idVertexMap.put(i, og.vertices[i]);
299 | 			og.verticeSet.add(og.vertices[i]);
300 | 			Set<Integer> adjSet = new TreeSet<Integer>();
301 | 			og.adjacencySetMap.put(i, adjSet);
302 | 			Integer[] adjs = graph.adjacencySetMap.get(vs[i].id).toArray(new Integer[0]);
303 | 			for(int j = 0; j < adjs.length; j ++){
304 | 				adjSet.add(vIdIndexMap.get(adjs[j]));
305 | 			}
306 | 		}
307 | 		
308 | 		for(int i = 0; i < start.length; i ++){
309 | 			start[i] = vIdIndexMap.get(start[i]);
310 | 			goal[i] = vIdIndexMap.get(goal[i]);
311 | 		}
312 | 		
313 | 		return og;
314 | 	}
315 | 	
316 | 	public static long EDGE_MULTIPLIER = 100000000L;
317 | 	public Set<Long> getEdgeSet(){
318 | 		Set<Long> edgeSet = new HashSet<Long>();
319 | 		for(int i = 0; i < vertices.length; i ++){
320 | 			for(int j: adjacencySetMap.get(i)){
321 | 				if(i == j) continue;
322 | 				edgeSet.add(getEdgeId(i, j));
323 | 			}
324 | 		}
325 | 		return edgeSet;
326 | 	}
327 | 	
328 | 	public static long getEdgeId(int u, int v){
329 | 		return u>v? v*EDGE_MULTIPLIER + u: u*EDGE_MULTIPLIER + v;
330 | 	}
331 | 	
332 | 	public static int getFirstVertexOfEdge(long edge){
333 | 		return (int)(edge/EDGE_MULTIPLIER);
334 | 	}
335 | 	
336 | 	public static int getSecondVertexOfEdge(long edge){
337 | 		return (int)(edge%EDGE_MULTIPLIER);
338 | 	}
339 | 	
340 | 
341 | 	/**
342 | 	 * For saving/retrieving the graph
343 | 	 * @param out
344 | 	 * @throws IOException
345 | 	 */
346 | 	private void writeObject(ObjectOutputStream out) throws IOException{
347 | 		// Gather vertice info and write to object
348 | 		int v[][] = new int[vertices.length][13];
349 | 		for(int i = 0; i <vertices.length; i ++){
350 | 			v[i][0] = vertices[i].id;
351 | 			v[i][1] = vertices[i].getIX();
352 | 			v[i][2] = vertices[i].getIY();
353 | 			
354 | 			// Adj list
355 | 			Integer adj[] = adjacencySetMap.get(vertices[i].id).toArray(new Integer[0]);
356 | 			v[i][3] = adj.length; 
357 | 			for(int j = 0; j < adj.length; j ++){
358 | 				v[i][4 + j] = adj[j];
359 | 			}
360 | 		}
361 | 		out.writeObject(v);
362 | 		out.writeInt(rows);
363 | 		out.writeInt(columns);
364 | 	}
365 | 	
366 | 	/**
367 |  	 * For saving/retrieving the graph
368 | 	 * @param in
369 | 	 * @throws IOException
370 | 	 * @throws ClassNotFoundException
371 | 	 */
372 | 	private void readObject(ObjectInputStream in) throws IOException, ClassNotFoundException{
373 | 		verticeSet = new HashSet<Vertex>();
374 | 		idVertexMap = new TreeMap<Integer, Vertex>();
375 | 		adjacencySetMap = new HashMap<Integer, Set<Integer>>();
376 | 		int v[][] = (int[][])in.readObject();
377 | 		this.vertices = new Vertex[v.length];
378 | 		for(int i = 0; i < v.length; i ++){
379 | 			vertices[i] = new Vertex(new Point2D.Double(v[i][1], v[i][2]));
380 | 			vertices[i].id = v[i][0];
381 | 			verticeSet.add(vertices[i]);
382 | 			idVertexMap.put(vertices[i].id, vertices[i]);
383 | 			Set<Integer> adjSet = new TreeSet<Integer>();
384 | 			adjacencySetMap.put(vertices[i].id, adjSet);
385 | 			for(int j = 0; j <v[i][3]; j ++){
386 | 				adjSet.add(v[i][4 + j]); 
387 | 			}
388 | 		}
389 | 		// vertices = verticeSet.toArray(new Vertex[0]);
390 | 		rows = in.readInt();
391 | 		columns = in.readInt();
392 | 	}
393 | 	
394 | 	/**
395 | 	 * Deleting a vertex from a grid graph, keeping the connectivity
396 | 	 * @param x
397 | 	 * @param y
398 | 	 */
399 | 	private void removeVertex(int x, int y){
400 | 		int id = getId(x, y);
401 | 		Vertex v = idVertexMap.get(id);
402 | 		
403 | 		// Remove the vertex
404 | 		verticeSet.remove(v);
405 | 		
406 | 		// Remove this from idVertexMap 
407 | 		idVertexMap.remove(id);
408 | 		
409 | 		// Remove v from all possible adjacency maps. For now just do brute force
410 | 		adjacencySetMap.remove(id);
411 | 		Integer ids[] = adjacencySetMap.keySet().toArray(new Integer[0]);
412 | 		for(int i = 0; i < ids.length; i ++){
413 | 			Set<Integer> idSet =  adjacencySetMap.get(ids[i]);
414 | 			idSet.remove(id);
415 | 			if(idSet.size() == 0){
416 | 				adjacencySetMap.remove(ids[i]);
417 | 			}
418 | 		}
419 | 	}
420 | 	
421 | 	/**
422 | 	 * Deleting a vertex, keeping the connectivity
423 | 	 * @param v
424 | 	 */
425 | 	public void removeVertex(Vertex v){
426 | 		// Remove the vertex
427 | 		verticeSet.remove(v);
428 | 		
429 | 		// Remove this from idVertexMap 
430 | 		idVertexMap.remove(v.id);
431 | 		
432 | 		// Remove v from all possible adjacency maps. For now just do brute force
433 | 		adjacencySetMap.remove(v.id);
434 | 		Integer ids[] = adjacencySetMap.keySet().toArray(new Integer[0]);
435 | 		for(int i = 0; i < ids.length; i ++){
436 | 			Set<Integer> idSet =  adjacencySetMap.get(ids[i]);
437 | 			idSet.remove(v.id);
438 | 			if(idSet.size() == 0){
439 | 				adjacencySetMap.remove(ids[i]);
440 | 			}
441 | 		}
442 | 	}
443 | 	
444 | 	//////////////////////////////////////////////////
445 | 	// For constructing a new graph 
446 | 	//////////////////////////////////////////////////
447 | 	
448 | 	/**
449 | 	 *  Add a vertex 
450 | 	 * @param v
451 | 	 */
452 | 	public void addVertex(Vertex v){
453 | 		this.verticeSet.add(v);
454 | 		this.idVertexMap.put(v.id, v);
455 | 	}
456 | 	
457 | 	
458 | 	private int getId(int x, int y){
459 | 		return y*columns + x + 1;
460 | 	}
461 | 	
462 | 	public static void updateAdjList(Graph g){
463 | 		Integer[] vIds = g.adjacencySetMap.keySet().toArray(new Integer[0]);
464 | 		for(int i = 0; i < vIds.length; i ++){
465 | 			g.adjacencySetMap.get(vIds[i]).remove(vIds[i]);
466 | 		}
467 | 	}
468 | 	
469 | 	/**
470 | 	 * Is the graph still connected when we remove a vertex?
471 | 	 * @param vId
472 | 	 * @return
473 | 	 */
474 | 	public boolean isConnectedWithoutVertex(int vId){
475 | 		// Get the neighbors
476 | 		Set<Integer> neighborSet = this.adjacencySetMap.get(vId);
477 | 		neighborSet.remove(vId);
478 | 		
479 | 		// If a single neighbor, must be connected
480 | 		if(neighborSet.size() == 1) return true;
481 | 		
482 | 		// For the rest cases, do pairwise test
483 | 		Integer[] vIds = neighborSet.toArray(new Integer[0]);
484 | 		for(int i = 0; i < vIds.length - 1; i ++){
485 | 			if(!areVerticesConnectedWithoutV3(vIds[i], vIds[i + 1], vId)){
486 | 				neighborSet.add(vId);
487 | 				neighborSet.add(vId);
488 | 				return false;
489 | 			}
490 | 		}
491 | 		neighborSet.add(vId);
492 | 		return true;
493 | 	}
494 | 	
495 | 	/**
496 | 	 * Are vertices with vId1 and vId2 connected without going through vId3?
497 | 	 * @param vId1
498 | 	 * @param vId2
499 | 	 * @param vId3
500 | 	 * @return
501 | 	 */
502 | 	private boolean areVerticesConnectedWithoutV3(int vId1, int vId2, int vId3){
503 | 		// House keeping
504 | 		Set<Integer> visitedVertices = new HashSet<Integer>();
505 | 		List<Integer> queue = new LinkedList<Integer>();
506 | 		
507 | 		// Add root
508 | 		queue.add(vId1);
509 | 		
510 | 		// Mark vId3 as visited; this automatically skips vId3 during search
511 | 		visitedVertices.add(vId3);
512 | 		
513 | 		// BFS graph to check connectivity of v1 and v2
514 | 		while(!queue.isEmpty()){
515 | 			int vId = queue.remove(0);
516 | 			Set<Integer> is = adjacencySetMap.get(vId);
517 | 			if(is != null){
518 | 				Integer[] neighborIds = is.toArray(new Integer[0]);
519 | 				for(int i = 0; i <neighborIds.length; i++){
520 | 					if(!visitedVertices.contains(neighborIds[i])){
521 | 						if(neighborIds[i] == vId2){
522 | 							return true;
523 | 						}
524 | 						queue.add(neighborIds[i]);
525 | 						visitedVertices.add(neighborIds[i]);
526 | 					}
527 | 				}
528 | 			}
529 | 		}
530 | 		return false;
531 | 	}
532 | 	
533 | 	protected static int SG[][] = {{0, 3}, {3, 0}};
534 | 	protected static Graph getv4TestGraph(){
535 | 		Graph g = new Graph();
536 | 		Vertex v0 = new Vertex(new Point2D.Double(0, 0));
537 | 		v0.id = 0; 
538 | 		Vertex v1 = new Vertex(new Point2D.Double(0, 1));
539 | 		v1.id = 1; 
540 | 		Vertex v2 = new Vertex(new Point2D.Double(1, 1));
541 | 		v2.id = 2; 
542 | 		Vertex v3 = new Vertex(new Point2D.Double(0, 2));
543 | 		v3.id = 3; 
544 | 		g.addVertex(v0);
545 | 		g.addVertex(v1);
546 | 		g.addVertex(v2);
547 | 		g.addVertex(v3);
548 | 	
549 | 		Set<Integer> nbrs0 = new TreeSet<Integer>();
550 | 		g.adjacencySetMap.put(0, nbrs0);
551 | 		nbrs0.add(0);
552 | 		nbrs0.add(1);
553 | 		Set<Integer> nbrs1 = new TreeSet<Integer>();
554 | 		g.adjacencySetMap.put(1, nbrs1);
555 | 		nbrs1.add(0);
556 | 		nbrs1.add(1);
557 | 		nbrs1.add(2);
558 | 		nbrs1.add(3);
559 | 		Set<Integer> nbrs2 = new TreeSet<Integer>();
560 | 		g.adjacencySetMap.put(2, nbrs2);
561 | 		nbrs2.add(1);
562 | 		nbrs2.add(2);
563 | 		Set<Integer> nbrs3 = new TreeSet<Integer>();
564 | 		g.adjacencySetMap.put(3, nbrs3);
565 | 		nbrs3.add(1);
566 | 		nbrs3.add(3);
567 | 		
568 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
569 | 		
570 | 		return g;
571 | 	}
572 | 
573 | 	static int[][] BRIDGE_TEST_VERTICES= new int[][]{
574 | 		{0, 0},
575 | 		{0, 1},
576 | 		{1, 0},
577 | 		{1, 1},
578 | 		{2, 1},
579 | 		{2, 2},
580 | 		{3, 0},
581 | 		{3, 1},
582 | 		{4, 0},
583 | 		{4, 1},
584 | 		{4, 2},
585 | 		{5, 1},
586 | 		{5, 2},
587 | 		{6, 1}
588 | 	};
589 | 	public static Graph getTestGraphForBridge(){
590 | 		Graph g = new Graph();
591 | 		for(int i = 0; i < BRIDGE_TEST_VERTICES.length; i ++)
592 | 		{
593 | 			g.addVertex(new Vertex(new Point2D.Double(BRIDGE_TEST_VERTICES[i][0], BRIDGE_TEST_VERTICES[i][1]), i));
594 | 		}
595 | 		g.vertices = g.verticeSet.toArray(new Vertex[0]);
596 | 
597 | 		for(Vertex v: g.vertices){
598 | 			Set<Integer> nbrs = new TreeSet<Integer>();
599 | 			g.adjacencySetMap.put(v.id, nbrs);
600 | 			for(Vertex n: g.vertices){
601 | 				if(Math.abs(v.getIX() - n.getIX()) + Math.abs(v.getIY() - n.getIY()) < 2){
602 | 					nbrs.add(n.id);
603 | 				}
604 | 			}
605 | 		}
606 | 		g.finishBuildingGraph();
607 | 		return g;
608 | 	}
609 | 	
610 | 	/**
611 | 	 * Adding a vertex with the set of neighbors
612 | 	 * @param vId
613 | 	 * @param neighbors
614 | 	 */
615 | 	public void addVertex(int vId, int[] neighbors){
616 | 		// Add vertex
617 | 		Vertex v = new Vertex(vId);
618 | 		verticeSet.add(v);
619 | 		idVertexMap.put(v.id, v);
620 | 		
621 | 		// Add neighbors
622 | 		Set<Integer> vSet = new TreeSet<Integer>(); 
623 | 		adjacencySetMap.put(vId, vSet);
624 | 		vSet.add(vId);
625 | 		for(int i = 0; i < neighbors.length; i ++){
626 | 			vSet.add(neighbors[i]);
627 | 		}
628 | 	}
629 | 	
630 | 	/**
631 | 	 * Finish building graphs
632 | 	 */
633 | 	public void finishBuildingGraph(){
634 | 		vertices = idVertexMap.values().toArray(new Vertex[0]); 
635 | 	}
636 | 	
637 | 	/**
638 | 	 * Print graph as adjacency list 
639 | 	 */
640 | 	public void printGraphAsAdjacenyList(){
641 | 		for(int i = 0; i < vertices.length; i ++){
642 | 			System.out.print(i + ": ");
643 | 			Integer[] vIds = adjacencySetMap.get(i).toArray(new Integer[0]);
644 | 			for(int av = 0; av < vIds.length; av++){
645 | 				if(vIds[av] != i){
646 | 					System.out.print(vIds[av] + ", ");
647 | 				}
648 | 			}
649 | 			System.out.println();
650 | 		}
651 | 	}
652 | 	
653 | 	public void paint(Graphics2D g2d, int scale){
654 | 		Vertex root = this.idVertexMap.values().toArray(new Vertex[0])[0];
655 | 		// House keeping stuff
656 | 		Set<Vertex> visitedVertices = new HashSet<Vertex>();
657 | 		List<Vertex> queue = new LinkedList<Vertex>();
658 | 		
659 | 		// Add root
660 | 		queue.add(root);
661 | 		while(!queue.isEmpty()){
662 | 			Vertex v = queue.remove(0);
663 | 			Set<Integer> is = adjacencySetMap.get(v.id);
664 | 			if(is != null){
665 | 				Integer[] neighborIds = is.toArray(new Integer[0]);
666 | 				for(int i = 0; i <neighborIds.length; i++){
667 | 					Vertex n = idVertexMap.get(neighborIds[i]);
668 | 					if(!visitedVertices.contains(n)){
669 | 						queue.add(n);
670 | 						visitedVertices.add(n);
671 | 					}
672 | 					if(v.id < n.id){
673 | 						g2d.setStroke(new BasicStroke(1));
674 | 						g2d.setPaint(new java.awt.Color(0xB0B0D0));
675 | 
676 | 						// Draw edges 
677 | 						g2d.drawLine(scale * v.getIX(), scale* v.getIY(), scale * n.getIX(), scale* n.getIY());
678 | 					}
679 | 				}
680 | 			}
681 | 		}
682 | 	}
683 | 	
684 | }
685 | 


--------------------------------------------------------------------------------
/source/projects/multipath/ILP/Main.java:
--------------------------------------------------------------------------------
   1 | package projects.multipath.ILP;
   2 | 
   3 | import java.io.FileWriter;
   4 | import java.util.SortedSet;
   5 | import java.util.TreeSet;
   6 | 
   7 | import projects.multipath.advanced.Graph;
   8 | import projects.multipath.advanced.Path;
   9 | import projects.multipath.advanced.Problem;
  10 | 
  11 | public class Main {
  12 | 
  13 | 	static String basePath = "D:/temp/data/dmrpp/"; 
  14 | 
  15 | 	public static void main(String argv[]) {
  16 | 		int option = (argv.length == 0)?1:Integer.parseInt(argv[0]);
  17 | 		MultiagentGraphSolverGurobiTime.bDebugInfo = false;
  18 | 		MultiagentGraphSolverGurobiTime.bPrintPaths = false;
  19 | 		Problem p = null;
  20 | 		switch(option){
  21 | 		case 0:
  22 | 			//run32x32PerformanceMetric();
  23 | 			//run32x32PerformanceMetric8ConnectedAll();
  24 | 			//run24x18PerformanceMetric();
  25 | 			//runCrowdedPerformanceMetricCrowded();
  26 | 			//runN2PuzzlePerformanceMetric();
  27 | 			//run24x18PerformanceMetricTotalTime();
  28 | 			//run24x18PerformanceMetricTotalDistanceSplit();
  29 | 			run24x18PerformanceMetricTotalTimeFixedTime();
  30 | 			
  31 | 			//run24x18PerformanceMetricTotalDistanceSplitFixedTime();
  32 | 			break;
  33 | 		case 1:
  34 | 			System.out.println("Solving a randomly generated 16-puzzle, generally solvable in 60 seconds on an intel dual core x64 machine.\n");
  35 | 			p = Problem.createN2Puzzle(4);
  36 | 			PathPlanner.printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
  37 | 			System.out.println("\nSolution path set:\n");
  38 | 			solveProblem(p, true, -1);
  39 | 			break;
  40 | 		case 2:
  41 | 			System.out.println("Solving a randomly generated multi-agent path planning problem on a 15x15 grid with 20% obstacles and 15 agents, with 60 second time limit.\n");
  42 | 			p = Problem.createGridProblem(15, 15, 0.2, 15);
  43 | 			System.out.println("Printing adjacency list of the graph:");
  44 | 			p.graph.printGraphAsAdjacenyList();
  45 | 			System.out.println("\nSolution path set:\n");
  46 | 			solveProblem(p, true, 60);
  47 | 			break;
  48 | 		case 3:
  49 | 			System.out.println("Create a simple graph manually and then solve a problem on this graph.\n");
  50 | 			Graph g = new Graph();
  51 | 			// Note that vertex ids must start from 0 and are consecutive. The resulting
  52 | 			// graph must be connected.
  53 | 			g.addVertex(0, new int[]{1});
  54 | 			g.addVertex(1, new int[]{0, 2, 3});
  55 | 			g.addVertex(2, new int[]{1});
  56 | 			g.addVertex(3, new int[]{1});
  57 | 			g.finishBuildingGraph();
  58 | 			System.out.println("Adjacency list:");
  59 | 			g.printGraphAsAdjacenyList();
  60 | 			System.out.println("\nSolving the problem, solution path set: ");
  61 | 			p = new Problem();
  62 | 			p.graph = g;
  63 | 			p.sg = new int[][]{{0, 3}, {3, 0}};
  64 | 			solveProblem(p, true, -1);
  65 | 			break;
  66 | 		case 4:
  67 | 			System.out.println("Solving problem from file: " + argv[1] + " with 1000 seconds limit.\n");
  68 | 			p = Problem.readFromFile(argv[1]);
  69 | 			PathPlanner.printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
  70 | 			System.out.println("\n");
  71 | 			solveProblem(p, true, 1000);
  72 | 			break;
  73 | 		case 5:
  74 | 			p = Problem.createGridProblem(16, 16, 0, 60);
  75 | 			PathPlanner.printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
  76 | 			solveProblem(p, true, 7200);
  77 | 		case 6:
  78 | 			Graph t = new Graph();
  79 | 			int branches = 4;
  80 | 			int depth = 4;
  81 | 			
  82 | 			// Create root
  83 | 			int[] rootNeighbors = new int[branches];
  84 | 			for(int i = 0; i < branches; i ++){rootNeighbors[i] = i*depth + 1;}
  85 | 			t.addVertex(0, rootNeighbors);
  86 | 			
  87 | 			// Create branches
  88 | 			for(int i = 1; i <= branches; i ++){
  89 | 				for(int j = 0; j < depth; j ++)
  90 | 				{
  91 | 					int offset = (i-1) * depth;
  92 | 					if(j == depth -1){
  93 | 						// Only a single neighbor
  94 | 						t.addVertex(offset + j + 1, new int[]{offset + j});	
  95 | 					}
  96 | 					else{
  97 | 						t.addVertex(offset + j + 1, new int[]{j==0?0:offset + j, offset + j + 2});
  98 | 					}
  99 | 				}
 100 | 			}
 101 | 			t.finishBuildingGraph();
 102 | 			int[][] sg = Graph.getRandomStartGoalMany(t, 8);
 103 | 			
 104 | 			System.out.println("Adjacency list:");
 105 | 			t.printGraphAsAdjacenyList();
 106 | 			
 107 | 			p = new Problem();
 108 | 			p.graph = t;
 109 | 			p.sg = sg;
 110 | 			
 111 | 			//sg[0]= new int[]{1, 2, 3};
 112 | 			//sg[1]= new int[]{7, 8, 9};
 113 | 			//p.sg = sg;
 114 | 
 115 | 			PathPlanner.printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
 116 | 			
 117 | 			System.out.println("\nSolving the problem, solution path set: ");
 118 | 			solveProblem(p, true, -1);
 119 | 			
 120 | 			// solveProblemSuboptimal(p, false, true, 0, 100, 2, false);
 121 | 			break;
 122 | 		}
 123 | 	}
 124 | 	
 125 | 	
 126 | 	/**
 127 | 	 * Solve a problem
 128 | 	 * @param p
 129 | 	 * @param timeLimit
 130 | 	 * @return
 131 | 	 */
 132 | 	private static long[] solveProblemSuboptimal(Problem p, boolean solveLP, boolean setOptimal, int extraSteps,
 133 | 			double timeLimit, int splitLevel, boolean noCycle){
 134 | 		// Solve the problem
 135 | 		PathPlanner ms = new PathPlanner();
 136 | 		int makespanLb = PathFinder.getMakespanLowerBound(p.graph, p.sg[0], p.sg[1]);
 137 | 		long time = System.currentTimeMillis();
 138 | 		int[][] paths = null;
 139 | 		// int[][] paths = ms.planPathsAdvancedSplitBySize(p.graph, p.sg[0], p.sg[1], setOptimal, timeLimit, 3);
 140 | 		// int[][] paths = ms.planPathsAdvancedSplitBySizeNoCycle(p.graph, p.sg[0], p.sg[1], setOptimal, timeLimit, 4, true);
 141 | 		// int[][] paths = ms.planPathsAdvancedSplit(p.graph, p.sg[0], p.sg[1], setOptimal, timeLimit, 2);
 142 | 		if(noCycle){
 143 | 			paths = ms.planPathsAdvancedSplitNoCycle(p.graph, p.sg[0], p.sg[1], setOptimal, timeLimit, splitLevel, true);
 144 | 		}
 145 | 		else{
 146 | 			paths = ms.multiThreadedSplitPlanning(p.graph, p.sg[0], p.sg[1], timeLimit, splitLevel);
 147 | 		}
 148 | 		PathPlanner.printPaths(p.graph, paths);
 149 | 
 150 | 		time = System.currentTimeMillis() - time;
 151 | 		if(paths != null && PathPlanner.isPathSetValid(paths, p.graph, p.sg[0], p.sg[1])){
 152 | 			int cycles = PathPlanner.hasCycles(paths);
 153 | 			if(cycles > 0){
 154 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 155 | 			}
 156 | 			return new long[]{paths[0].length - 1, time, 0, makespanLb - 1};
 157 | 		}
 158 | 		else{
 159 | 			return null;
 160 | 		}
 161 | 	}
 162 | 	
 163 | 	/**
 164 | 	 * Solve a problem
 165 | 	 * @param p
 166 | 	 * @param timeLimit
 167 | 	 * @return
 168 | 	 */
 169 | 	private static long[] solveProblem(Problem p, boolean solveLP, double timeLimit){
 170 | 		// Solve the problem
 171 | 		PathPlanner ms = new PathPlanner();
 172 | 		long time = System.currentTimeMillis();
 173 | 		int[][] paths = ms.planPathsAdvanced(p.graph, p.sg[0], p.sg[1], solveLP, 0, true, timeLimit);
 174 | 		time = System.currentTimeMillis() - time;
 175 | 		int makespanLb = PathFinder.getMakespanLowerBound(p.graph, p.sg[0], p.sg[1]);
 176 | 		if(paths != null){
 177 | 			int cycles = PathPlanner.hasCycles(paths);
 178 | 			if(cycles > 0){
 179 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 180 | 			}
 181 | 			PathPlanner.printPaths(p.graph, paths);
 182 | 			
 183 | 			return new long[]{paths[0].length - 1, time, PathFinder.getTotalDistance(paths), makespanLb - 1};
 184 | 		}
 185 | 		else{
 186 | 			return null;
 187 | 		}
 188 | 	}
 189 | 	
 190 | 	/**
 191 | 	 * Solve a problem
 192 | 	 * @param p
 193 | 	 * @param timeLimit
 194 | 	 * @return
 195 | 	 */
 196 | 	private static long[] solveProblemPMG(Problem p, boolean solveLP, double timeLimit){
 197 | 		// Solve the problem
 198 | 		PathPlanner ms = new PathPlanner();
 199 | 		long time = System.currentTimeMillis();
 200 | 		int[][] paths = ms.planPathsAdvancedPMG(p.graph, p.sg[0], p.sg[1], solveLP, 0, true, timeLimit);
 201 | 		time = System.currentTimeMillis() - time;
 202 | 		int makespanLb = PathFinder.getMakespanLowerBound(p.graph, p.sg[0], p.sg[1]);
 203 | 		if(paths != null){
 204 | 			int cycles = PathPlanner.hasCycles(paths);
 205 | 			if(cycles > 0){
 206 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 207 | 			}
 208 | 			return new long[]{paths[0].length - 1, time, PathFinder.getTotalDistance(paths), makespanLb - 1};
 209 | 		}
 210 | 		else{
 211 | 			return null;
 212 | 		}
 213 | 	}
 214 | 
 215 | 	/**
 216 | 	 * Optimize for total time. 
 217 | 	 * @param p
 218 | 	 * @param timeLimit
 219 | 	 * @return
 220 | 	 */
 221 | 	private static long[] solveProblemTotalTime(Problem p, double gap, double timeLimit){
 222 | 		PathPlanner ms = new PathPlanner();
 223 | 		long time = System.currentTimeMillis();
 224 | 		int opt[] = new int[1];
 225 | 		// Solve the makespan problem using a 4 split
 226 | 		int[][] paths = ms.planPathsAdvancedSplit(p.graph, p.sg[0], p.sg[1], false, -1, 2);
 227 | 		timeLimit -= (System.currentTimeMillis() - time)/1000.;
 228 | 		if(timeLimit <= 0) return null;
 229 | 		int timeSteps = paths[0].length - 1;
 230 | 		paths = ms.planPathsAdvancedTT(p.graph, p.sg[0], p.sg[1], opt, gap, timeSteps, timeLimit);
 231 | 		time = System.currentTimeMillis() - time;
 232 | 		int ttLB = PathFinder.getTotalTimeLowerBound(p.graph, p.sg[0], p.sg[1]);
 233 | 		
 234 | 		if(paths != null){
 235 | 			int cycles = PathPlanner.hasCycles(paths);
 236 | 			if(cycles > 0){
 237 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 238 | 			}
 239 | 			return new long[]{opt[0], time, PathFinder.getTotalDistance(paths), ttLB};
 240 | 		}
 241 | 		else{
 242 | 			return null;
 243 | 		}
 244 | 	}
 245 | 	
 246 | 	/**
 247 | 	 * Optimize for total time. 
 248 | 	 * @param p
 249 | 	 * @param timeLimit
 250 | 	 * @return
 251 | 	 */
 252 | 	private static long[] solveProblemDistance(Problem p, double gap, double timeLimit){
 253 | 		PathPlanner ms = new PathPlanner();
 254 | 		long time = System.currentTimeMillis();
 255 | 		// Solve the makespan problem using a 4 split
 256 | 		int[][] paths = ms.planPathsAdvancedSplit(p.graph, p.sg[0], p.sg[1], false, -1, 2);
 257 | 		timeLimit -= (System.currentTimeMillis() - time)/1000.;
 258 | 		if(timeLimit <= 0) return null;
 259 | 		int timeSteps = paths[0].length - 1;
 260 | 		paths = ms.planPathsAdvancedTD(p.graph, p.sg[0], p.sg[1], gap, timeSteps, timeLimit);
 261 | 		time = System.currentTimeMillis() - time;
 262 | 		int ttLB = PathFinder.getTotalTimeLowerBound(p.graph, p.sg[0], p.sg[1]);
 263 | 		
 264 | 		if(paths != null){
 265 | 			int cycles = PathPlanner.hasCycles(paths);
 266 | 			if(cycles > 0){
 267 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 268 | 			}
 269 | 			return new long[]{PathFinder.getTotalDistance(paths), time, PathFinder.getTotalDistance(paths), ttLB};
 270 | 		}
 271 | 		else{
 272 | 			return null;
 273 | 		}
 274 | 	}
 275 | 	
 276 | 	private static long[] solveProblemDistanceSplit(Problem p, double gap, double timeLimit, int splits){
 277 | 		PathPlanner ms = new PathPlanner();
 278 | 		long time = System.currentTimeMillis();
 279 | 		// Solve the makespan problem using a 4 split
 280 | 		int[][] paths = ms.planPathsAdvancedSplitDist(p.graph, p.sg[0], p.sg[1], gap, timeLimit, splits);
 281 | 		time = System.currentTimeMillis() - time;
 282 | 		int ttLB = PathFinder.getTotalTimeLowerBound(p.graph, p.sg[0], p.sg[1]);
 283 | 		
 284 | 		if(paths != null){
 285 | 			int cycles = PathPlanner.hasCycles(paths);
 286 | 			if(cycles > 0){
 287 | 				System.out.println("\nThere are " + cycles + " cycles in the result paths");
 288 | 			}
 289 | 			PathPlanner.printPaths(p.graph, paths);
 290 | 			return new long[]{PathFinder.getTotalDistance(paths), time, PathFinder.getTotalDistance(paths), ttLB};
 291 | 		}
 292 | 		else{
 293 | 			return null;
 294 | 		}
 295 | 	}
 296 | 	
 297 | 	/**
 298 | 	 * Solve a n^2 puzzle
 299 | 	 * @param p
 300 | 	 * @param timeLimit
 301 | 	 * @return
 302 | 	 */
 303 | 	private static long[] solvePuzzleProblem(int instance, Problem p, double timeLimit){
 304 | 		// Solve the problem
 305 | 		long time = System.currentTimeMillis();
 306 | 		int paths[][] = null;
 307 | 		paths = PuzzleSolver.solve(p, timeLimit);
 308 | 		time = System.currentTimeMillis() - time;
 309 | 		int makespanLb = PathFinder.getMakespanLowerBound(p.graph, p.sg[0], p.sg[1]);
 310 | 		if(paths != null){
 311 | 			writePuzzleResult(instance, time, p.graph, paths);
 312 | 			PathPlanner.printPaths(p.graph, paths);
 313 | 			return new long[]{paths[0].length - 1, time, PathFinder.getTotalDistance(paths), makespanLb - 1};
 314 | 		}
 315 | 		return null;
 316 | 	}
 317 | 	
 318 | 	/**
 319 | 	 * Solve a problem without optimality guarantee, but fast
 320 | 	 * @param p
 321 | 	 * @return
 322 | 	 */
 323 | 	private static long[] solveProblemWithHeuristic(Problem p){
 324 | 		PathFinder.bHeuristic = true;
 325 | 		// Solve the problem
 326 | 		PathPlanner ms = new PathPlanner();
 327 | 		long time = System.currentTimeMillis();
 328 | 		PathPlanner.printStartAndGoals(p.graph, p.sg[0], p.sg[1]);
 329 | 		int[][] paths = ms.planHeuristicPaths(p.graph, p.sg[0], p.sg[1]);
 330 | 		time = System.currentTimeMillis() - time;
 331 | 		if(paths != null){
 332 | 			int goalsReached = PathPlanner.numberOfGoalsReached(paths, p.sg[1]);
 333 | 			// PathPlanner.printPaths(p.graph, paths);
 334 | 			if(!PathPlanner.isPathSetValid(paths, p.graph, p.sg[0], p.sg[1])){
 335 | 				System.out.println("Path set is INVALID.");
 336 | 			}
 337 | 			
 338 | 			// Find out path lengths
 339 | 			long length = 0, leastPossibleLength = 0;
 340 | 			PathFinder.bHeuristic = false;
 341 | 			Path[] hp = PathFinder.findShortestPaths(p.graph, p.sg[0], p.sg[1]);
 342 | 			PathFinder.bHeuristic = true;
 343 | 			for(int a = 0; a < p.sg[0].length; a++){
 344 | 				leastPossibleLength += (hp[a].vertexList.size() - 1);
 345 | 				for(int i = 0; i < paths[a].length - 1; i ++){
 346 | 					if(paths[a][i] != paths[a][i+1]){
 347 | 						length++;
 348 | 					}
 349 | 				}
 350 | 			}
 351 | 			
 352 | 			return new long[]{paths[0].length, time, goalsReached, 
 353 | 					(goalsReached==p.sg[0].length)?length-1:0, (goalsReached==p.sg[0].length)?leastPossibleLength-1:0};
 354 | 		}
 355 | 		else{
 356 | 			return null;
 357 | 		}
 358 | 	}
 359 | 	
 360 | 	static String currentTest = null;
 361 | 	static double subTotal = 0;
 362 | 	static int pathLengths = 0;
 363 | 	static int solvedInstances = 0;
 364 | 	static double totalTime = 0;
 365 | 	static int successCount = 0;
 366 | 	static long goalsReached = 0;
 367 | 	static long pathLength = 0;
 368 | 	static long expPathLength = 0;
 369 | 	static SortedSet<Double> finishingTimeSet = new TreeSet<Double>();
 370 | 	static int makeSpanTotal = 0;
 371 | 	
 372 | 	private static void resetStats(String test){
 373 | 		currentTest = test;
 374 | 		subTotal = 0;
 375 | 		pathLengths = 0;
 376 | 		solvedInstances = 0;
 377 | 		successCount = 0;
 378 | 		makeSpanTotal = 0;
 379 | 		finishingTimeSet = new TreeSet<Double>(); 
 380 | 	}
 381 | 	
 382 | 	private static void resetHeuristicStats(String test){
 383 | 		currentTest = test;
 384 | 		goalsReached = 0;
 385 | 		subTotal = 0;
 386 | 		pathLengths = 0;
 387 | 		solvedInstances = 0;
 388 | 		successCount = 0;
 389 | 		pathLength = 0;
 390 | 		expPathLength = 0;
 391 | 		finishingTimeSet = new TreeSet<Double>(); 
 392 | 	}
 393 | 	
 394 | 	private static void processStats(long[] stat){
 395 | 		if(stat != null){
 396 | 			int len = (int)stat[0];
 397 | 			long time = stat[1];
 398 | 			successCount += 1;
 399 | 			pathLengths += len;
 400 | 			finishingTimeSet.add(time/1000.0);
 401 | 			System.out.println("Time spent = " + time/1000.0 + " seconds.");
 402 | 			subTotal += time/1000.0;
 403 | 			totalTime += time/1000.0;
 404 | 			makeSpanTotal += stat[3];
 405 | 			System.out.println("Total time spent: " + totalTime + " seconds.\n");
 406 | 		}
 407 | 	}
 408 | 	
 409 | 	private static void processHeuristicStats(long[] stat, int n){
 410 | 		if(stat != null){
 411 | 			int len = (int)stat[0];
 412 | 			long time = stat[1];
 413 | 			pathLengths += len;
 414 | 			finishingTimeSet.add(time/1000.0);
 415 | 			goalsReached += stat[2];
 416 | 			System.out.println("Goals reached = " + goalsReached);
 417 | 			if(stat[2] == n){successCount += 1;}
 418 | 			System.out.println("Time spent = " + time/1000.0 + " seconds.");
 419 | 			subTotal += time/1000.0;
 420 | 			totalTime += time/1000.0;
 421 | 			System.out.println("Total time spent: " + totalTime + " seconds.\n");
 422 | 			pathLength += stat[3];
 423 | 			expPathLength += stat[4];
 424 | 		}
 425 | 	}
 426 | 	
 427 | 	private static void writeStats(){
 428 | 		try {
 429 | 			Double[] ft = finishingTimeSet.toArray(new Double[0]);
 430 | 			String outString = currentTest + "Total success: " + successCount + ", ave time: " + subTotal/successCount + ", median:" + ft[ft.length/2] + ", min: " + ft[0] + ", max: " + ft[ft.length - 1] + ", ave solution: " + (pathLengths*1./successCount) + ", min possible: " + (makeSpanTotal*1./successCount) +"\n";
 431 | 			writeString(outString);
 432 | //			FileWriter fw = new FileWriter(basePath + "\\n2puzzle-result.txt", true);
 433 | //			fw.append(outString);
 434 | //			fw.close();
 435 | 		} catch (Exception e) {
 436 | 			e.printStackTrace();
 437 | 		}
 438 | 	}
 439 | 	
 440 | 	private static void writeHeuristicStats(){
 441 | 		try {
 442 | 			Double[] ft = finishingTimeSet.toArray(new Double[0]);
 443 | 			String outString = currentTest + "Total success: " + successCount + ", goals reached: " + goalsReached + ", ave time: " + subTotal/successCount + ", median:" + ft[ft.length/2] + ", min: " + ft[0] + ", max: " + ft[ft.length - 1] + ", steps: " + pathLength + ", exp: " + expPathLength + ", ratio: " + pathLength*1./expPathLength + "\n";
 444 | 			FileWriter fw = new FileWriter(basePath + "\\n2puzzle-result.txt", true);
 445 | 			fw.append(outString);
 446 | 			fw.close();
 447 | 		} catch (Exception e) {
 448 | 			e.printStackTrace();
 449 | 		}
 450 | 	}
 451 | 	
 452 | 	private static void writePuzzleStats(int instance, double time){
 453 | 		try {
 454 | 			String outString = "Instance: " + instance + ", time: " + time + "\n";
 455 | 			FileWriter fw = new FileWriter(basePath + "\\n2puzzle-result.txt", true);
 456 | 			fw.append(outString);
 457 | 			fw.close();
 458 | 		} catch (Exception e) {
 459 | 			e.printStackTrace();
 460 | 		}
 461 | 	}
 462 | 	
 463 | 	private static void writeString(String str){
 464 | 		try {
 465 | 			FileWriter fw = new FileWriter(basePath + "\\n2puzzle-result.txt", true);
 466 | 			fw.append(str);
 467 | 			fw.close();
 468 | 		} catch (Exception e) {
 469 | 			e.printStackTrace();
 470 | 		}
 471 | 	}
 472 | 	
 473 | 	private static void writePuzzleResult(int instance, double time, Graph g, int[][] paths){
 474 | 		try {
 475 | 			String outString = "Instance: " + (instance  + 1) + ", time: " + time/1000 + "\n";
 476 | 			FileWriter fw = new FileWriter(basePath + "\\25puzzle.txt", true);
 477 | 			fw.append(outString);
 478 | 			fw.append(PathPlanner.printPathToString(g, paths));
 479 | 			fw.append("\n");
 480 | 			fw.close();
 481 | 		} catch (Exception e) {
 482 | 			e.printStackTrace();
 483 | 		}
 484 | 	}
 485 | 	
 486 | 	protected static void runN2PuzzlePerformanceMetric(){
 487 | 		try{
 488 | 			totalTime = 0;
 489 | 			for(int n = 3; n <= 5; n ++){
 490 | 				resetStats("" + n*n + "-puzzle, 100 runs. ");
 491 | 				for(int i = 0; i < 100 ; i ++){
 492 | 					Problem p = Problem.readFromFile(basePath + "\\n2puzzle\\" + (n*n) + "-puzzle-" + (1001 + i) + ".dat");
 493 | 					System.out.println("Working on " +  + (n*n) + "-puzzle-" + (1001 + i) + ".dat");
 494 | 					long stat[] = solvePuzzleProblem(i, p, -1);
 495 | 					if(stat != null){
 496 | 						processStats(stat);
 497 | 						writePuzzleStats(i + 1, (stat[1]/1000.));
 498 | 					}
 499 | 				}
 500 | 				writeStats();
 501 | 			}
 502 | 		}
 503 | 		catch(Exception e){
 504 | 			e.printStackTrace();
 505 | 		}
 506 | 	}
 507 | 	
 508 | 	protected static void runCrowdedPerformanceMetric8x8(int from, int to, double timeLimit, int splitLevels, boolean noCycle){
 509 | 		totalTime = 0;
 510 | 		for(int a = from; a <= to; a = a + 10){
 511 | 			boolean success = true;
 512 | 			// if(a == 60){a = 55;}
 513 | 			resetStats("8x8, " + a + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 514 | 			for(int i = 0; i < 10; i ++){
 515 | 				Problem p = Problem.readFromFile(basePath + "\\8x8-grid\\" + a + "-agts-" + (1001 + i) + ".dat");
 516 | 				System.out.println("Working on " + "8x8 grid performance testing," + (noCycle?" no cycles, ":"") + " problem: " + a + "-agts-" + (1001 + i) + ".dat");
 517 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, splitLevels, noCycle);
 518 | 				processStats(stat);
 519 | 				if(stat == null){success = false; break;}
 520 | 			}
 521 | 			if(success==false)break;
 522 | 			writeStats();
 523 | 		}
 524 | 		writeString("\n");
 525 | 	}
 526 | 
 527 | 	protected static void runCrowdedPerformanceMetric16x16(int from, int to, double timeLimit, int splitLevels, boolean noCycle){
 528 | 		totalTime = 0;
 529 | 		for(int a = from; a <= to; a = a + 10){
 530 | 			boolean success = true;
 531 | 			resetStats("16x16, " + a + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 532 | 			for(int i = 0; i < 10; i ++){
 533 | 				Problem p = Problem.readFromFile(basePath + "\\16x16-grid\\" + a + "-agts-" + (1001 + i) + ".dat");
 534 | 				System.out.println("Working on " + "16x16 grid performance testing, " + (noCycle?" no cycles, ":"") + "problem: " + a + "-agts-" + (1001 + i) + ".dat");
 535 | 				// long stat[] = solveProblem(p, false, 7200);
 536 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, splitLevels, noCycle);
 537 | 				processStats(stat);
 538 | 				if(stat == null){success = false; break;}
 539 | 			}
 540 | 			if(success==false)break;
 541 | 			writeStats();
 542 | 		}
 543 | 		writeString("\n");
 544 | 	}
 545 | 	
 546 | 	protected static void run24x18PerformanceMetric(){
 547 | 		writeString("24 x 18 grid, exact\n");
 548 | 		totalTime = 0;
 549 | 		for(int obs = 0; obs <= 30; obs=obs+5){
 550 | 			boolean stopCurrentRun = false;
 551 | 			for(int n = 10; n <= 120; n=n+10){
 552 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 553 | 				for(int i = 0; i < 10; i ++){
 554 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 555 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 556 | 					long stat[] = solveProblemSuboptimal(p, false, false, 0, 600, 0, false);
 557 | 					processStats(stat);
 558 | 					if(stat == null){
 559 | 						stopCurrentRun  = true;
 560 | 						break;
 561 | 					}
 562 | 				}
 563 | 				writeStats();
 564 | 				if(stopCurrentRun == true){
 565 | 					break;
 566 | 				}
 567 | 			}
 568 | 			writeString("\n");
 569 | 		}
 570 | 
 571 | 		for(int k = 0; k <=3; k ++){
 572 | 			writeString("24 x 18 grid, "+ (1 << (k+1)) + "-way split\n");
 573 | 			totalTime = 0;
 574 | 			for(int obs = 0; obs <= 30; obs=obs+5){
 575 | 				boolean stopCurrentRun = false;
 576 | 				for(int n = 20; n <= 300; n=n+20){
 577 | 					resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 578 | 					for(int i = 0; i < 10; i ++){
 579 | 						System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 580 | 						Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 581 | 						long stat[] = solveProblemSuboptimal(p, false, false, 0, 600, k+1, false);
 582 | 						processStats(stat);
 583 | 						if(stat == null){
 584 | 							stopCurrentRun  = true;
 585 | 							break;
 586 | 						}
 587 | 					}
 588 | 					writeStats();
 589 | 					if(stopCurrentRun == true){
 590 | 						break;
 591 | 					}
 592 | 				}
 593 | 			}
 594 | 		}
 595 | 	}
 596 | 	
 597 | 	protected static void run32x32PerformanceMetric(){
 598 | 		writeString("32 x 32 grid, 20% obstacles, exact\n");
 599 | 		totalTime = 0;
 600 | 		for(int a = 10; a <= 90; a = a + 10){
 601 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 602 | 			for(int i = 0; i < 10; i ++){
 603 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 604 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 605 | 				long stat[] = solveProblem(p, false, 1800);
 606 | 				processStats(stat);
 607 | 			}
 608 | 			writeStats();
 609 | 		}
 610 | 
 611 | 		double timeLimit = 1800; boolean noCycle = true;
 612 | 		
 613 | 		writeString("32 x 32 grid, 20% obstacles, 2-way split\n");
 614 | 		totalTime = 0;
 615 | 		for(int n = 20; n <= 120; n = n + 20){
 616 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 617 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 618 | 			for(int i = 0; i < 10; i ++){
 619 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 620 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 621 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, 1, noCycle);
 622 | 				processStats(stat);
 623 | 			}
 624 | 			writeStats();
 625 | 		}
 626 | 
 627 | 		writeString("32 x 32 grid, 20% obstacles, 4-way split\n");
 628 | 		totalTime = 0;
 629 | 		for(int n = 20; n <= 160; n = n + 20){
 630 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 631 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 632 | 			for(int i = 0; i < 10; i ++){
 633 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 634 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 635 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, 2, noCycle);
 636 | 				processStats(stat);
 637 | 			}
 638 | 			writeStats();
 639 | 		}
 640 | 		
 641 | 		writeString("32 x 32 grid, 20% obstacles, 8-way split\n");
 642 | 		totalTime = 0;
 643 | 		for(int n = 20; n <= 200; n = n + 20){
 644 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 645 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 646 | 			for(int i = 0; i < 10; i ++){
 647 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 648 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 649 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, 3, noCycle);
 650 | 				processStats(stat);
 651 | 			}
 652 | 			writeStats();
 653 | 		}
 654 | 
 655 | 		writeString("32 x 32 grid, 20% obstacles, 16-way split\n");
 656 | 		totalTime = 0;
 657 | 		for(int n = 20; n <= 200; n = n + 20){
 658 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 659 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 660 | 			for(int i = 0; i < 10; i ++){
 661 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 662 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 663 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, 4, noCycle);
 664 | 				processStats(stat);
 665 | 			}
 666 | 			writeStats();
 667 | 		}
 668 | 	}
 669 | 	
 670 | 	protected static void run32x32PerformanceMetric8ConnectedAll(){
 671 | 		writeString("32 x 32 grid, 8 connected, 20% obstacles, exact\n");
 672 | 		totalTime = 0;
 673 | 		for(int a = 20; a <= 100; a = a + 20){
 674 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 675 | 			for(int i = 0; i < 10; i ++){
 676 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 677 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-" + (1001 + i) + ".dat");
 678 | 				// long stat[] = solveProblem(p, false, 3600);
 679 | 				long stat[] = solveProblemSuboptimal(p, false, false, 0, 1800, 0, false);
 680 | 				processStats(stat);
 681 | 			}
 682 | 			writeStats();
 683 | 		}
 684 | 
 685 | 		writeString("32 x 32 grid, 8 connected, 20% obstacles, 2-way split\n");
 686 | 		totalTime = 0;
 687 | 		for(int a = 20; a <= 200; a = a + 20){
 688 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 689 | 			for(int i = 0; i < 10; i ++){
 690 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 691 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-" + (1001 + i) + ".dat");
 692 | 				// long stat[] = solveProblem(p, false, 3600);
 693 | 				long stat[] = solveProblemSuboptimal(p, false, false, 0, 1800, 1, false);
 694 | 				processStats(stat);
 695 | 			}
 696 | 			writeStats();
 697 | 		}
 698 | 		
 699 | 		writeString("32 x 32 grid, 8 connected, 20% obstacles, 4-way split\n");
 700 | 		totalTime = 0;
 701 | 		for(int a = 20; a <= 300; a = a + 20){
 702 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 703 | 			for(int i = 0; i < 10; i ++){
 704 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 705 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-" + (1001 + i) + ".dat");
 706 | 				// long stat[] = solveProblem(p, false, 3600);
 707 | 				long stat[] = solveProblemSuboptimal(p, false, false, 0, 1800, 2, false);
 708 | 				processStats(stat);
 709 | 			}
 710 | 			writeStats();
 711 | 		}
 712 | 		
 713 | 		writeString("32 x 32 grid, 8 connected, 20% obstacles, 8-way split\n");
 714 | 		totalTime = 0;
 715 | 		for(int a = 20; a <= 400; a = a + 20){
 716 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 717 | 			for(int i = 0; i < 10; i ++){
 718 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 719 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-" + (1001 + i) + ".dat");
 720 | 				// long stat[] = solveProblem(p, false, 3600);
 721 | 				long stat[] = solveProblemSuboptimal(p, false, false, 0, 1800, 3, false);
 722 | 				processStats(stat);
 723 | 			}
 724 | 			writeStats();
 725 | 		}
 726 | 		
 727 | 	}
 728 | 
 729 | 	protected static void runCrowdedPerformanceMetricCrowded(){
 730 | 		writeString("8 x 8 grid, exact\n");
 731 | 		runCrowdedPerformanceMetric8x8(10, 50, 1800, 0, false);
 732 | 		for(int k = 1; k <= 3; k ++){
 733 | 			writeString("8 x 8 grid, " + (1 << k) + "-way split\n");
 734 | 			runCrowdedPerformanceMetric8x8(10, 60, 1800, k, false);
 735 | 		}
 736 | 
 737 | 		for(int k = 1; k <= 3; k ++){
 738 | 			writeString("8 x 8 grid, " + (1 << k) + "-way split, no cycle\n");
 739 | 			runCrowdedPerformanceMetric8x8(10, 50, 1800, k, true);
 740 | 		}
 741 | 
 742 | 		writeString("16 x 16 grid, exact\n");
 743 | 		runCrowdedPerformanceMetric16x16(10, 100, 1800, 0, false);
 744 | 		for(int k = 1; k <= 3; k ++){
 745 | 			writeString("16 x 16 grid, " + (1 << k) + "-way split\n");
 746 | 			runCrowdedPerformanceMetric16x16(10, 250, 1800, k, false);
 747 | 		}
 748 | 
 749 | 		for(int k = 1; k <= 3; k ++){
 750 | 			writeString("16 x 16 grid, " + (1 << k) + "-way split, no cycle\n");
 751 | 			runCrowdedPerformanceMetric16x16(10, 250, 1800, k, true);
 752 | 		}
 753 | 	}
 754 | 	
 755 | 
 756 | 	/**
 757 | 	 * Total time algorithm evaluation
 758 | 	 */
 759 | 	protected static void run24x18PerformanceMetricTotalTime(){
 760 | 		writeString("24 x 18 grid, total time solution\n");
 761 | 		totalTime = 0;
 762 | 		for(int obs = 0; obs <= 30; obs=obs+5){
 763 | 			boolean stopCurrentRun = false;
 764 | 			for(int n = 10; n <= 160; n=n+10){
 765 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 766 | 				for(int i = 0; i < 10; i ++){
 767 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 768 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 769 | 					long stat[] = solveProblemTotalTime(p, n*0.002 + obs*0.002 + (n > 50 ? n*0.001 : 0) + (n > 100 ? n*0.0005 : 0), 600);
 770 | 					processStats(stat);
 771 | 					if(stat == null){
 772 | 						stopCurrentRun  = true;
 773 | 						break;
 774 | 					}
 775 | 				}
 776 | 				writeStats();
 777 | 				if(stopCurrentRun == true){
 778 | 					break;
 779 | 				}
 780 | 			}
 781 | 			writeString("\n");
 782 | 		}
 783 | 	}
 784 | 	
 785 | 	/**
 786 | 	 * Total distance algorithm evaluation
 787 | 	 */
 788 | 	protected static void run24x18PerformanceMetricTotalDistance(){
 789 | 		writeString("24 x 18 grid, total distance solution\n");
 790 | 		totalTime = 0;
 791 | 		for(int obs = 0; obs <= 30; obs=obs+5){
 792 | 			boolean stopCurrentRun = false;
 793 | 			for(int n = 10; n <= 160; n=n+10){
 794 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 795 | 				for(int i = 0; i < 10; i ++){
 796 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 797 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 798 | 					long stat[] = solveProblemDistance(p, n*0.002 + obs*0.002, 600);
 799 | 					processStats(stat);
 800 | 					if(stat == null){
 801 | 						stopCurrentRun  = true;
 802 | 						break;
 803 | 					}
 804 | 				}
 805 | 				if(stopCurrentRun == true){
 806 | 					break;
 807 | 				}
 808 | 				writeStats();
 809 | 			}
 810 | 			writeString("\n");
 811 | 		}
 812 | 	}
 813 | 	
 814 | 	/**
 815 | 	 * Total distance algorithm evaluation
 816 | 	 */
 817 | 	protected static void run24x18PerformanceMetricTotalDistanceSplit(){
 818 | 		writeString("24 x 18 grid, total distance solution, split computation\n");
 819 | 		totalTime = 0;
 820 | 		int split = 2;
 821 | 		for(int obs = 0; obs <= 30; obs=obs+5){
 822 | 			if(obs > 20) split = 3;
 823 | 			boolean stopCurrentRun = false;
 824 | 			for(int n = 10; n <= 160; n=n+10){
 825 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 826 | 				for(int i = 0; i < 10; i ++){
 827 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 828 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 829 | 					long stat[] = solveProblemDistanceSplit(p, n*0.001 + obs*0.0025 + (n > 70 ? n*0.001 : 0), 600, split);
 830 | 					processStats(stat);
 831 | 					if(stat == null){
 832 | 						stopCurrentRun  = true;
 833 | 						break;
 834 | 					}
 835 | 				}
 836 | 				if(stopCurrentRun == true){
 837 | 					break;
 838 | 				}
 839 | 				writeStats();
 840 | 			}
 841 | 			writeString("\n");
 842 | 		}
 843 | 	}
 844 | 	
 845 | 	/**
 846 | 	 * Fixing time and evaluate solution optimality, total time 
 847 | 	 */
 848 | 	protected static void run24x18PerformanceMetricTotalTimeFixedTime(){
 849 | 		MultiagentGraphSolverGurobiTotalTime.bReturnAnySolution = true;
 850 | 		writeString("24 x 18 grid, total time solution, fixed time (100s)\n");
 851 | 		totalTime = 0;
 852 | 		for(int obs = 0; obs <= 20; obs=obs+5){
 853 | 			boolean stopCurrentRun = false;
 854 | 			for(int n = 70; n <= 100; n=n+10){
 855 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 856 | 				for(int i = 0; i < 10; i ++){
 857 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 858 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 859 | 					long stat[] = solveProblemTotalTime(p, 0.001, 100);
 860 | 					processStats(stat);
 861 | 					if(stat == null){
 862 | 						stopCurrentRun  = true;
 863 | 						break;
 864 | 					}
 865 | 				}
 866 | 				writeStats();
 867 | 				if(stopCurrentRun == true){
 868 | 					break;
 869 | 				}
 870 | 			}
 871 | 			writeString("\n");
 872 | 		}
 873 | 		MultiagentGraphSolverGurobiTotalTime.bReturnAnySolution = false;
 874 | 	}
 875 | 	
 876 | 	/**
 877 | 	 * Fixing time and evaluate solution optimality, total distance 
 878 | 	 */
 879 | 	protected static void run24x18PerformanceMetricTotalDistanceSplitFixedTime(){
 880 | 		MultiagentGraphSolverGurobiDistance.bReturnAnySolution = true;
 881 | 		writeString("24 x 18 grid, total distance solution, split computation, fixed time (100s)\n");
 882 | 		totalTime = 0;
 883 | 		for(int obs = 0; obs <= 20; obs=obs+5){
 884 | 			boolean stopCurrentRun = false;
 885 | 			for(int n = 10; n <= 150; n=n+10){
 886 | 				resetStats("24x18, " + obs + "% obstacles, " + n + " agents, 10 runs. ");
 887 | 				for(int i = 0; i < 10; i ++){
 888 | 					System.out.println("Working on " + "24x18 grid performance testing, problem: " + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 889 | 					Problem p = Problem.readFromFile(basePath + "\\24x18-grid\\" + n + "-agt-"+ (obs) + "-pct-obs-" + (1001 + i) + ".dat");
 890 | 					long stat[] = solveProblemDistanceSplit(p, 0.001, 100, 2);
 891 | 					processStats(stat);
 892 | 					if(stat == null){
 893 | 						stopCurrentRun  = true;
 894 | 						break;
 895 | 					}
 896 | 				}
 897 | 				if(stopCurrentRun == true){
 898 | 					break;
 899 | 				}
 900 | 				writeStats();
 901 | 			}
 902 | 			writeString("\n");
 903 | 		}
 904 | 		MultiagentGraphSolverGurobiDistance.bReturnAnySolution = false;
 905 | 	}
 906 | 	
 907 | 	
 908 | 
 909 | 	
 910 | 	
 911 | 	
 912 | 	
 913 | 	
 914 | 	
 915 | 	
 916 | 	
 917 | 	
 918 | 	
 919 | 	
 920 | 	
 921 | 	
 922 | 	
 923 | 	
 924 | 	
 925 | 	// Previous stuff - not very relevant now...
 926 | 	
 927 | 
 928 | 	protected static void run32x32PerformanceMetricSplitting(int from, int to, int spacing, double timeLimit, int splitLevels, boolean noCycle){
 929 | 		totalTime = 0;
 930 | 		for(int n = from; n <= to; n = n + spacing){
 931 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 932 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 933 | 			for(int i = 0; i < 10; i ++){
 934 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 935 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".dat");
 936 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, splitLevels, noCycle);
 937 | 				processStats(stat);
 938 | 			}
 939 | 			writeStats();
 940 | 		}
 941 | 	}
 942 | 	
 943 | 	protected static void run32x32PerformanceMetric8Connected(){
 944 | 		totalTime = 0;
 945 | 		for(int a = 10; a <= 60; a = a + 10){
 946 | 			resetStats("32x32, 20% obstacles, " + a + " agents, 10 runs. ");
 947 | 			for(int i = 0; i < 10; i ++){
 948 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + a + "-agts-" + (1001 + i) + ".dat");
 949 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + a + "-" + (1001 + i) + ".dat");
 950 | 				// long stat[] = solveProblem(p, false, 3600);
 951 | 				long stat[] = solveProblemSuboptimal(p, false, false, 0, 3600, 1, false);
 952 | 				processStats(stat);
 953 | 			}
 954 | 			writeStats();
 955 | 		}
 956 | 	}
 957 | 
 958 | 	/**
 959 | 	 * This one does not do exact computation - do not use!
 960 | 	 */
 961 | 	protected static void run32x32PerformanceMetricHeuristic(){
 962 | 		totalTime = 0;
 963 | 		for(int n = 20; n <= 200; n = n + 20){
 964 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents, 100 runs. ");
 965 | 			for(int i = 0; i < 10; i ++){
 966 | 				System.out.println("Working on " + "32x32 grid performance testing, problem: 20-pct-obs-" + n + "-" + (1001 + i) + ".dat");
 967 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid\\20-pct-obs-" + n + "-" + (1001 + i) + ".dat");
 968 | 				long stat[] = solveProblemWithHeuristic(p);
 969 | 				processHeuristicStats(stat, n);
 970 | 			}
 971 | 			writeHeuristicStats();
 972 | 		}
 973 | 	}
 974 | 	
 975 | 	protected static void run32x32PerformanceMetricSplitting8Connected(int from, int to, double timeLimit, int splitLevels, boolean noCycle){
 976 | 		totalTime = 0;
 977 | 		for(int n = from; n <= to; n = n + 25){
 978 | 			resetStats("32x32, 20% obstacles, "+ n + " agents, 10 runs. ");
 979 | 			resetHeuristicStats("32x32, 20% obstacles, "+ n + " agents," + (noCycle?" no cycles, ":"") + " 10 runs. ");
 980 | 			for(int i = 0; i < 10; i ++){
 981 | 				System.out.println("Working on " + "32x32 grid performance testing," + (noCycle?" no cycles, ":"") + " problem: 20-pct-obs-" + n + "-agts-" + (1001 + i) + ".txt");
 982 | 				Problem p = Problem.readFromFile(basePath + "\\32x32-grid-8c\\20-pct-obs-" + n + "-agts-" + (1001 + i) + ".txt");
 983 | 				long stat[] = solveProblemSuboptimal(p, false, true, 0, timeLimit, splitLevels, noCycle);
 984 | 				processStats(stat);
 985 | 			}
 986 | 			writeStats();
 987 | 		}
 988 | 	}
 989 | 
 990 | 	protected static void runFullPerformanceMetric(){
 991 | 		// No splitting, cycle
 992 | 		writeString("No splitting\n");
 993 | 		runCrowdedPerformanceMetric8x8(10, 50, 3600, 0, false);
 994 | 		runCrowdedPerformanceMetric16x16(10, 70, 3600, 0, false);
 995 | 		run32x32PerformanceMetric();
 996 | 		
 997 | 		// 2 - split, cycle
 998 | 		writeString("2 - splitting\n");
 999 | 		runCrowdedPerformanceMetric8x8(10, 60, 3600, 1, false);
1000 | 		runCrowdedPerformanceMetric16x16(10, 120, 3600, 1, false);
1001 | 		run32x32PerformanceMetricSplitting(25, 125, 25, 3600, 1, false);
1002 | 		
1003 | 		// 2 - split, no cycle
1004 | 		writeString("2 - splitting, no cycle\n");
1005 | 		runCrowdedPerformanceMetric8x8(10, 40, 3600, 1, true);
1006 | 		runCrowdedPerformanceMetric16x16(10, 80, 3600, 1, true);
1007 | 		
1008 | 		// 4 - split, cycle
1009 | 		writeString("4 - splitting\n");
1010 | 		runCrowdedPerformanceMetric8x8(10, 60, 3600, 2, false);
1011 | 		runCrowdedPerformanceMetric16x16(10, 160, 3600, 2, false);
1012 | 		run32x32PerformanceMetricSplitting(25, 100, 25, 3600, 2, false);
1013 | 		
1014 | 		// 4 - split, no cycle
1015 | 		writeString("4 - splitting, no cycle\n");
1016 | 		runCrowdedPerformanceMetric8x8(10, 40, 3600, 2, true);
1017 | 		runCrowdedPerformanceMetric16x16(10, 100, 3600, 2, true);
1018 | 		
1019 | 		// 8 - split, cycle
1020 | 		writeString("8 - splitting\n");
1021 | 		runCrowdedPerformanceMetric8x8(10, 60, 3600, 3, false);
1022 | 		runCrowdedPerformanceMetric16x16(10, 210, 3600, 3, false);
1023 | 		run32x32PerformanceMetricSplitting(25, 150, 25, 3600, 3, false);
1024 | 		
1025 | 		// 8 - split, no cycle
1026 | 		writeString("8 - splitting, no cycle\n");
1027 | 		runCrowdedPerformanceMetric8x8(10, 40, 3600, 3, true);
1028 | 		runCrowdedPerformanceMetric16x16(10, 120, 3600, 3, true);
1029 | 		
1030 | 		// 16 - split, cycle
1031 | 		writeString("16 - splitting\n");
1032 | 		runCrowdedPerformanceMetric8x8(20, 60, 3600, 4, false);
1033 | 		runCrowdedPerformanceMetric16x16(20, 220, 3600, 4, false);
1034 | 		run32x32PerformanceMetricSplitting(25, 250, 25, 3600, 4, false);
1035 | 		
1036 | 		// 16 - split, no cycle
1037 | 		writeString("16 - splitting, no cycle\n");
1038 | 		runCrowdedPerformanceMetric8x8(20, 40, 3600, 4, true);
1039 | 		runCrowdedPerformanceMetric16x16(20, 130, 3600, 4, true);
1040 | 	}
1041 | 
1042 | 	protected static void runFullPerformanceMetricExtras(){
1043 | 		// No splitting, cycle
1044 | 		writeString("No splitting\n");
1045 | 		runCrowdedPerformanceMetric16x16(90, 90, 3600, 0, false);
1046 | 	}
1047 | }
1048 | 


--------------------------------------------------------------------------------