Revision 200:f64571c86ac1

View differences:

misc/eclipse/GenerateInstance.launch
8 8
</listAttribute>
9 9
<stringAttribute key="org.eclipse.jdt.launching.CLASSPATH_PROVIDER" value="org.eclipse.m2e.launchconfig.classpathProvider"/>
10 10
<stringAttribute key="org.eclipse.jdt.launching.MAIN_TYPE" value="tt.jointeuclid2ni.probleminstance.generator.GenerateInstance"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-env src/main/resources/d-environments/101.xml  -nagents 15 -radius 10 -seed 1005 -outfile src/main/resources/problems/101.xml -sgavoiding -showvis -verbose"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-env src/main/resources/d-environments/101.xml  -nagents 35 -radius 10 -seed 1005 -outfile src/main/resources/problems/101.xml -sgavoiding -showvis -verbose"/>
12 12
<stringAttribute key="org.eclipse.jdt.launching.PROJECT_ATTR" value="deconflictiontools"/>
13 13
<stringAttribute key="org.eclipse.jdt.launching.SOURCE_PATH_PROVIDER" value="org.eclipse.m2e.launchconfig.sourcepathProvider"/>
14 14
</launchConfiguration>
src/main/java/tt/jointeuclid2ni/probleminstance/generator/ConflictGenerator.java
2 2

  
3 3
import java.util.ArrayList;
4 4
import java.util.Collection;
5
import java.util.HashSet;
5 6
import java.util.LinkedList;
6 7
import java.util.List;
8
import java.util.Queue;
7 9
import java.util.Random;
8 10
import java.util.Set;
9 11

  
......
11 13

  
12 14
import org.jgrapht.DirectedGraph;
13 15
import org.jgrapht.GraphPath;
16
import org.jgrapht.Graphs;
14 17
import org.jgrapht.alg.AStarShortestPathSimple;
18
import org.jgrapht.alg.ConnectivityInspector;
15 19

  
16 20
import tt.euclid2i.HeuristicToPoint;
17 21
import tt.euclid2i.Line;
......
44 48
    private static final int MAX_ATTEMPTS_MISSION = 150;
45 49
    private static final int MAX_ATTEMPTS_START = 5000;
46 50
    private static final int MAX_ATTEMPTS_TARGET = 5000;
47

  
48
//    @SuppressWarnings("unchecked")
49
//    public ConflictGenerator(Environment environment, DirectedGraph<Point, Line> planningGraph, int[] bodyRadiuses, int[][] gridPatter, int step, Rectangle missionRegion) {
50
//        this.environment = environment;
51
//        this.planningGraph = planningGraph;
52
//        this.bodyRadiuses = bodyRadiuses;
53
//        this.nAgents = bodyRadiuses.length;
54
//        this.listeners = new ArrayList<ProblemCreatedListener>();
55
//
56
//
57
//        if (planningGraph == null) {
58
//            this.graphs = createGridGraphs(environment, bodyRadiuses, gridPatter, step);
59
//        } else {
60
//            this.graphs = createGraphs(planningGraph);
61
//        }
62
//        this.listsOfVertices = fillVertexSets(missionRegion);
63
//        this.maxTime = calculateMaxTime(environment);
64
//    }
51
    private static final int RADIUS_BUFFER = 1; 
65 52

  
66 53
    private static DirectedGraph<Point, Line>[] toGraphArray(DirectedGraph<Point, Line> planningGraph, int nAgents) {
67 54

  
......
121 108
        return (int) (3 * bounds.getCorner1().distance(bounds.getCorner2()) / MAX_SPEED);
122 109
    }
123 110
    
124
    
125

  
126 111
    public static EarliestArrivalProblem generateInstance(Environment environment,
127 112
		    int[] bodyRadiuses,
128 113
		    int[][] gridPattern, 
......
205 190
        }
206 191
    }
207 192

  
208
//    static private  void fillProblemWithAgentsStartGoalAvoiding(ExtensibleProblem problem, List<Point>[] listsOfVertices) throws MissionNotAddedException {
209
//    	int[] bodyRadiuses = problem.getBodyRadiuses();
210
//    	int nAgents = problem.getBodyRadiuses().length;
211
//    	
212
//    	int attempts = 0;
213
//    	boolean success = false;
214
//    	AgentMission[] missions = null;
215
//    	do {
216
//
217
//    		attempts++;
218
//    		Verbose.printf(" -- -- -- -- attempt %d to find start-goal avoiding missions... ", attempts);
219
//	    	// Generate mutually disjoint starts and goals
220
//
221
//    		missions = new AgentMission[nAgents];
222
//    		Collection<Region> startGoalRegions = new LinkedList<Region>();
223
//    		for (int agent = 0; agent < nAgents; agent++) {
224
//	        	List<Point> list = filterVertices(listsOfVertices[agent], startGoalRegions, bodyRadiuses[agent]);
225
//	        	Point start = randomVertex(list);
226
//	        	startGoalRegions.add(new Circle(start, bodyRadiuses[agent]));
227
//
228
//	        	list = filterVertices(listsOfVertices[agent], startGoalRegions, bodyRadiuses[agent]); // we need to filter the list again to make sure that start and goal won't overlap
229
//	        	Point target = randomVertex(list);
230
//	        	startGoalRegions.add(new Circle(target, bodyRadiuses[agent]));
231
//	        	missions[agent] = new AgentMissionImpl(start, target, bodyRadiuses[agent], 1);
232
//	        }
233
//
234
//	        try {
235
//	        	Trajectory[] trajectories = new Trajectory[nAgents];
236
//
237
//		        // Try to find start/goal avoiding trajectories
238
//		        for (int agent = 0; agent < nAgents; agent++) {
239
//		        	 DirectedGraph<Point, Line> graph = removeStartsTargetsOfOtherAgents(planningGraphs[agent], missions, agent);
240
//		        	 trajectories[agent] = solveMission(graph, missions[agent]);
241
//		        }
242
//		        success = true;
243
//
244
//	        }
245
//	        catch (PathNotFoundException e) {
246
//	        	Verbose.print(" ...failed\n");
247
//	        	success = false;
248
//	        }
249
//    	} while (!(success || attempts > MAX_ATTEMPTS_MISSION));
250
//
251
//    	if (success) {
252
//    		for (AgentMission mission : missions) {
253
//    			problem.addAgent(mission);
254
//    		}
255
//    	} else {
256
//    		throw new MissionNotAddedException();
257
//    	}
258
//
259
//    }
260

  
261 193
    static private List<Point> filterVertices(List<Point> list, Collection<Region> startGoalRegions, int agentBodyRadius) {
262 194

  
263 195
    	Collection<Region> inflatedRegions = Util.inflateRegions(startGoalRegions, agentBodyRadius);
......
280 212
        int attempts = 0;
281 213
        do {
282 214
            try {
283
                AgentMission mission = generateMission(problem, bodyRadius, possibleEndpoints, false, problem.getAgentMissions(), otherTrajectoriesArray, rnd);
284

  
215
                AgentMission mission = generateMission(problem, bodyRadius, possibleEndpoints, false, requireStartGoalAvoiding, planningGraph, problem.getAgentMissions(), rnd);
285 216
               	Trajectory trajectory = solveMission(planningGraph, mission, maxTime, problem.getAgentMissions(), requireStartGoalAvoiding);
286 217

  
287 218
               	if (otherTrajectories.isEmpty() || (requireConflict ? SeparationDetector.hasAnyPairwiseConflict(trajectory, otherTrajectoriesArray, separations, 5) : true)) {
288
               		
289 219
               		problem.addAgent(mission);
290 220
                    otherTrajectories.add(trajectory);
291 221
                    return;
......
308 238

  
309 239
	static private AgentMission generateMission(ExtensibleProblem problem,
310 240
			int bodyRadius, Collection<Point> possibleEndpoints,
311
			boolean startAndGoalsCanOverlap, AgentMission[] otherMissions, Trajectory[] otherTrajectories,
241
			boolean startAndGoalsCanOverlap, boolean sgAvoiding, 
242
			DirectedGraph<Point, Line> planningGraph, AgentMission[] otherMissions, 
312 243
			Random rnd) throws MissionNotAddedException {
313 244

  
314 245
        Point start = null;
......
320 251
                throw new MissionNotAddedException();
321 252
            }
322 253
        } while (collidesWithOtherStartPoint(start, bodyRadius, problem) ||
323
        		!startAndGoalsCanOverlap ? startsGoalsOverlap(start, bodyRadius, problem): false || 
324
        		 collidesWithOtherTrajectory(start, bodyRadius, otherMissions, otherTrajectories));
254
        		(!startAndGoalsCanOverlap ? startsGoalsOverlap(start, bodyRadius, problem) : false) || 
255
        		(sgAvoiding ? !maintainsConnectivityIfRemoved(start, bodyRadius, planningGraph, otherMissions) : false));
325 256

  
326 257
        Point target = null;
327 258
        int targetPointAttempts = 0;
......
332 263
                throw new MissionNotAddedException();
333 264
            }
334 265
        } while (collidesWithOtherTargetPoint(target, bodyRadius, problem) || start == target ||
335
        		!startAndGoalsCanOverlap ? startsGoalsOverlap(target, bodyRadius, problem): false ||
336
        		collidesWithOtherTrajectory(target, bodyRadius, otherMissions, otherTrajectories));
266
        		(!startAndGoalsCanOverlap ? startsGoalsOverlap(target, bodyRadius, problem): false) ||
267
        		(sgAvoiding ? !maintainsConnectivityIfRemoved(target, bodyRadius, planningGraph, otherMissions) : false));
337 268

  
338 269
        Verbose.printf(" -- -- -- -- new random mission S:%s G:%s%n", start, target);
339 270

  
340 271
        return new AgentMissionImpl(start, target, bodyRadius, MAX_SPEED);
341 272
    }
342

  
343

  
344
	private static boolean collidesWithOtherTrajectory(Point point, int bodyRadius,
345
			AgentMission[] otherMissions, Trajectory[] otherTrajectories) {
273
	
274
	private static boolean maintainsConnectivityIfRemoved(Point point,
275
			int bodyRadius, DirectedGraph<Point, Line> planningGraph, 
276
			AgentMission[] otherMissions) {
346 277
		
347
		for (int i = 0; i < otherTrajectories.length; i++) {
348
			MovingCircle mc = new MovingCircle(otherTrajectories[i], otherMissions[i].getBodyRadius() + bodyRadius);
349
			if (mc.intersectsPoint(point)) {
350
				return true;
278
		for (int i=0; i<otherMissions.length; i++) {
279
			// check connectivity for agent i
280
			Point otherAgentStart = otherMissions[i].getStart();
281
			Point otherAgentTarget = otherMissions[i].getTarget(); 
282
			int otherAgentBodyRadius = otherMissions[i].getBodyRadius();
283
			AgentMission[] otherAgentOtherMissions = new AgentMission[otherMissions.length];
284
			
285
			int j=0;
286
			for (AgentMission mission : otherMissions) {
287
				if (!mission.getStart().equals(otherAgentStart)) {
288
					otherAgentOtherMissions[j] = mission; 
289
					j++;
290
				}
291
			}
292
			
293
			// Hack! We will assume put start = target = point to be able to reuse removeStartsTargetsOfOtherAgents
294
			otherAgentOtherMissions[j] = new AgentMissionImpl(point, point, bodyRadius, 1); 
295
			
296
			DirectedGraph<Point, Line> augmentedGraph = removeStartsTargetsOfOtherAgents(planningGraph, otherAgentOtherMissions, otherAgentBodyRadius);
297
			
298
			if (!isReachable(otherAgentStart, otherAgentTarget, augmentedGraph)) {
299
				return false;
351 300
			}
352 301
		}
353 302
		
354
		return false;
303
		return true;
355 304
	}
356 305

  
357 306
	private static Trajectory solveMission(DirectedGraph<Point, Line> graph, AgentMission mission, int maxTime, AgentMission[] otherMissions, boolean requireStartGoalAvoiding) throws PathNotFoundException {
358 307
    	
359
    	final int RADIUS_BUFFER = 1;
360
    	
361 308
        Point target = mission.getTarget();
362 309
        Point start = mission.getStart();
363 310
        
364 311
        if (requireStartGoalAvoiding) {
365
        	graph = removeStartsTargetsOfOtherAgents(graph, otherMissions, mission.getBodyRadius() + RADIUS_BUFFER);
312
        	graph = removeStartsTargetsOfOtherAgents(graph, otherMissions, mission.getBodyRadius());
366 313
        }
367 314

  
368 315
        GraphPath<Point, Line> path = AStarShortestPathSimple.findPathBetween(graph, new HeuristicToPoint(target), start, target);
......
380 327
    	Collection<Region> startTargetRegions = new LinkedList<Region>();
381 328

  
382 329
    	for (int i=0; i<missions.length; i++) {
383
    		startTargetRegions.add(new Circle(missions[i].getStart(), missions[i].getBodyRadius() + bodyRadius));
384
    		startTargetRegions.add(new Circle(missions[i].getTarget(), missions[i].getBodyRadius()  + bodyRadius));
330
    		startTargetRegions.add(new Circle(missions[i].getStart(), missions[i].getBodyRadius() + bodyRadius + RADIUS_BUFFER));
331
    		startTargetRegions.add(new Circle(missions[i].getTarget(), missions[i].getBodyRadius()  + bodyRadius + RADIUS_BUFFER));
385 332
    	}
386 333

  
387 334
    	return new ObstacleWrapper<Point, Line>(directedGraph, startTargetRegions);
......
428 375
        }
429 376
        return false;
430 377
    }
378
	
379
	static private boolean isReachable(Point from, Point to, DirectedGraph<Point, Line> graph) {
380
		Queue<Point> open = new LinkedList<Point>();
381
		Set<Point> closed = new HashSet<Point>();
382
		
383
		open.add(from);
384
		
385
		while(!open.isEmpty()) {
386
			Point current = open.poll();
387
			
388
			if (current.equals(to)) {
389
				return true;
390
			}
391
			
392
			for (Line edge : graph.outgoingEdgesOf(current)) {
393
				Point neighbor = Graphs.getOppositeVertex(graph, edge, current);
394
				if (!closed.contains(neighbor)) {
395
					open.add(neighbor);
396
					closed.add(neighbor);
397
				}
398
			}
399
		}
400
		
401
		return false;
402
	}
431 403
    
432 404
    private static Point randomVertex(Collection<Point> list, Random rnd) {
433 405
        int size = list.size();

Also available in: Unified diff