Revision 68:eaa8e29beac0

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/environments/empty-unbounded.xml -nagents  13 -radius 98 -gridpattern 4  -gridstep 200 -seed 1000"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-env src/main/resources/environments/empty-unbounded.xml -nagents  60 -radius 48 -gridpattern 4  -gridstep 50 -seed 1000"/>
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>
misc/eclipse/Solver (test).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.Solver"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KDPMD -k 2  -problemfile src/main/resources/problems/superconflict.xml -maxtime 2500 -gridstep 50 -grid 8 -summary -showvis"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KDPMD -k 1 -problemfile src/main/resources/problems/superconflict4.xml -maxtime 2500 -gridstep 25 -grid 8 -summary -showvis"/>
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/solver/impl/AbstractAlgorithm.java
37 37
import tt.vis.GraphLayer;
38 38
import tt.vis.LabeledCircleLayer;
39 39
import tt.vis.ParameterControlLayer;
40
import tt.vis.TimeParameterHolder;
40 41
import cz.agents.alite.vis.VisManager;
41 42
import cz.agents.alite.vis.layer.common.ColorLayer;
42 43
import cz.agents.alite.vis.layer.common.VisInfoLayer;
......
47 48

  
48 49
    protected Parameters params;
49 50
    protected EarliestArrivalProblem problem;
50
    protected TimeParameter time;
51 51

  
52 52
    protected abstract SearchResult solveProblem();
53 53

  
......
90 90
                params.gridPattern,
91 91
                params.gridStep).generateFullGraph();
92 92

  
93

  
94
        System.out.println("Agent i, " + grid.vertexSet().size() + " vertices " + grid.edgeSet().size() + " edges");
95

  
96

  
93 97
        // create discretization
94 98
        final DirectedGraph<tt.euclid2i.Point, Line> spatialGraph
95 99
                = new AdditionalPointsExtension(grid, Collections.singleton(problem.getTarget(i)), params.gridStep, true);
96 100

  
101

  
102
        if (i==2) {
97 103
        // graph
98 104
        KeyToggleLayer toggleLayer = KeyToggleLayer.create("g");
99 105
        toggleLayer.addSubLayer(
......
107 113
        );
108 114
        VisManager.registerLayer(toggleLayer);
109 115
        // create spatio-temporal graph
116
        }
110 117
        return spatialGraph;
111 118
    }
112 119

  
......
189 196
        // Overlay
190 197
        VisManager.registerLayer(VisInfoLayer.create());
191 198

  
192
        time = new TimeParameter((int) Math.round(((double)params.gridStep / (double)params.maxSpeed) / (double) 5));
199
        if (TimeParameterHolder.time == null) {
200
            TimeParameterHolder.time = new TimeParameter((int) Math.round(((double)params.gridStep / (double)params.maxSpeed) / (double) 5));
201
            VisManager.registerLayer(ParameterControlLayer.create(TimeParameterHolder.time));
202
        }
193 203

  
194
        VisManager.registerLayer(ParameterControlLayer.create(time));
204

  
195 205
    }
196 206

  
197 207
    protected void visualizeSolution(final SearchResult result) {
......
232 242
                                                                public Color getColor(int i) {
233 243
                                                                    return AgentColors.getColorForAgent(i);
234 244
                                                                }
235
                                                            }, time
245
                                                            },TimeParameterHolder.time
236 246
            ));
237 247

  
238 248
        }
src/main/java/tt/jointeuclid2ni/solver/impl/AbstractDPMBasedAlgorithm.java
10 10
import tt.euclidtime3i.L1Heuristic;
11 11
import tt.euclidtime3i.L2Heuristic;
12 12
import tt.euclidtime3i.PerfectBasedHeuristic;
13
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
14 13
import tt.euclidtime3i.discretization.softconstraints.ConstantSeparationPenaltyFunction;
15
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
16
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
17
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
18 14
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
19 15
import tt.jointtraj.separableflow.AStarTrajectoryOptimizer;
20 16
import tt.jointtraj.separableflow.TrajectoryOptimizer;
......
51 47
        HeuristicToGoal<tt.euclidtime3i.Point>[] heuristics = new HeuristicToGoal[problem.nAgents()];
52 48

  
53 49
        for (int i = 0; i < problem.nAgents(); i++) {
54
        	switch (params.heuristic) {
55
			case PERFECT:
56
				heuristics[i] = new PerfectBasedHeuristic(graph[i], problem.getTarget(i));
57
				break;
50
            switch (params.heuristic) {
51
            case PERFECT:
52
                heuristics[i] = new PerfectBasedHeuristic(graph[i], problem.getTarget(i));
53
                break;
58 54

  
59
			case L1:
60
				heuristics[i] = new L1Heuristic(problem.getTarget(i));
61
				break;
55
            case L1:
56
                heuristics[i] = new L1Heuristic(problem.getTarget(i));
57
                break;
62 58

  
63
			case L2:
64
				heuristics[i] = new L2Heuristic(problem.getTarget(i));
65
				break;
59
            case L2:
60
                heuristics[i] = new L2Heuristic(problem.getTarget(i));
61
                break;
66 62

  
67
			default:
68
				throw new NotImplementedException();
69
			}
63
            default:
64
                throw new NotImplementedException();
65
            }
70 66
        }
71 67

  
72 68
        // Create trajectory optimizers
src/main/java/tt/jointtraj/separableflow/AStarTrajectoryOptimizer.java
30 30
    final int vmax;
31 31
    final private HeuristicToGoal<tt.euclidtime3i.Point> heuristic;
32 32
    final DirectedGraph<tt.euclidtime3i.Point, Straight> agentMotions;
33
	private int constraintSamplingInterval;
33
    private int constraintSamplingInterval;
34 34

  
35 35

  
36 36
    public AStarTrajectoryOptimizer(
......
49 49
        this.vmax = speed;
50 50
        this.heuristic = heuristic;
51 51
        this.agentMotions = new FreeOnTargetWaitExtension(
52
        		new ConstantSpeedTimeExtension(spatialGraph, target.getTime(), new int[]{vmax}, new LinkedList<Region>(), waitMoveDuration),
53
        			target.getPosition());
52
                new ConstantSpeedTimeExtension(spatialGraph, target.getTime(), new int[]{vmax}, new LinkedList<Region>(), waitMoveDuration),
53
                    target.getPosition());
54 54
        this.constraintSamplingInterval = constraintSamplingInterval;
55 55
    }
56 56

  
57 57
    @Override
58
	public EvaluatedTrajectory getOptimalTrajectoryUnconstrained (double maxAllowedCost, long runtimeDeadlineMs) {
59
    	return getOptimalTrajectoryConstrained(new PenaltyFunction[0], new Trajectory[0],  null, maxAllowedCost, runtimeDeadlineMs);
58
    public EvaluatedTrajectory getOptimalTrajectoryUnconstrained (double maxAllowedCost, long runtimeDeadlineMs) {
59
        return getOptimalTrajectoryConstrained(new PenaltyFunction[0], new Trajectory[0],  null, maxAllowedCost, runtimeDeadlineMs);
60 60
    }
61 61

  
62 62
    @Override
63
	public EvaluatedTrajectory getOptimalTrajectoryConstrained(
64
			PenaltyFunction[] penaltyFunctions,
65
			Trajectory[] otherTrajectories,
66
			Trajectory initialTraj,
67
			double maxAllowedCost, long runtimeDeadlineMs) {
63
    public EvaluatedTrajectory getOptimalTrajectoryConstrained(
64
            PenaltyFunction[] penaltyFunctions,
65
            Trajectory[] otherTrajectories,
66
            Trajectory initialTraj,
67
            double maxAllowedCost, long runtimeDeadlineMs) {
68 68

  
69 69
        PairwiseConstraint[] constraints = new PairwiseConstraint[otherTrajectories.length];
70 70
        for (int i = 0; i < constraints.length; i++) {
71
			constraints[i] = new SeparationConstraint(penaltyFunctions[i], constraintSamplingInterval);
72
		}
71
            constraints[i] = new SeparationConstraint(penaltyFunctions[i], constraintSamplingInterval);
72
        }
73 73

  
74 74
        StraightSegmentPenaltyFunction penalty = new PairwiseConstraintStraightSegmentPenalty(constraints, otherTrajectories);
75 75

  
76
		Graph<tt.euclidtime3i.Point, Straight> graphWithPenalty
77
        	= new PenaltyFunctionWrapper(agentMotions, penalty);
76
        Graph<tt.euclidtime3i.Point, Straight> graphWithPenalty
77
            = new PenaltyFunctionWrapper(agentMotions, penalty);
78 78

  
79
		AStarShortestPathSimple<Point, Straight> alg
80
			= new AStarShortestPathSimple<Point, Straight>(graphWithPenalty, heuristic, start, new Goal<Point>() {
81
					@Override
82
					public boolean isGoal(Point current) {
83
						if (current.getPosition().equals(target.getPosition())) {
84
							if (current.getTime() == target.getTime()) {
85
								return true;
86
							}
87
						}
88
						return false;
79
        AStarShortestPathSimple<Point, Straight> alg
80
            = new AStarShortestPathSimple<Point, Straight>(graphWithPenalty, heuristic, start, new Goal<Point>() {
81
                    @Override
82
                    public boolean isGoal(Point current) {
83
                        if (current.getPosition().equals(target.getPosition())) {
84
                            if (current.getTime() == target.getTime()) {
85
                                return true;
86
                            }
87
                        }
88
                        return false;
89 89

  
90
					}
91
				});
90
                    }
91
                });
92 92

  
93 93
        GraphPath<Point, Straight> path = alg.findPathCostAndDeadlineLimit(maxAllowedCost, runtimeDeadlineMs);
94 94

  
......
96 96
            return null;
97 97

  
98 98
        EvaluatedTrajectory traj = new BasicSegmentedTrajectory(path.getEdgeList(), target.getTime(), path.getWeight());;
99
		return traj;
100
	}
99
        return traj;
100
    }
101 101

  
102 102
}
src/main/java/tt/jointtraj/separableflow/SeparableFlowOptimizer.java
1 1
package tt.jointtraj.separableflow;
2 2

  
3 3
import java.awt.Color;
4
import java.awt.image.SampleModel;
5 4
import java.util.Arrays;
6
import java.util.Iterator;
7

  
8
import org.jgrapht.Graph;
9

  
10
import com.vividsolutions.jts.index.bintree.Key;
11 5

  
12 6
import tt.euclid2i.EvaluatedTrajectory;
13
import tt.euclid2i.Line;
14 7
import tt.euclid2i.Trajectory;
15 8
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
16 9
import tt.euclidtime3i.discretization.softconstraints.ConstantSeparationPenaltyFunction;
......
23 16
import tt.util.Verbose;
24 17
import tt.vis.FastAgentsLayer;
25 18
import tt.vis.FastTrajectoriesLayer;
26
import tt.vis.GraphLayer;
27 19
import tt.vis.FastTrajectoriesLayer.ColorProvider;
28 20
import tt.vis.FastTrajectoriesLayer.TrajectoriesProvider;
29 21
import tt.vis.ParameterControlLayer;
22
import tt.vis.TimeParameterHolder;
30 23
import cz.agents.alite.vis.VisManager;
31 24
import cz.agents.alite.vis.layer.toggle.KeyToggleLayer;
32 25

  
33 26
public class SeparableFlowOptimizer {
34 27

  
35
	private static final int TAN_COEF = 5;
36
	private static boolean showProgress;
37
	protected static EvaluatedTrajectory[] currentTrajectories;
38
	protected static int[] bodyRadiuses;
28
    private static final int TAN_COEF = 5;
29
    private static boolean showProgress;
30
    protected static EvaluatedTrajectory[] currentTrajectories;
31
    protected static int[] bodyRadiuses;
39 32

  
40
	public static EvaluatedTrajectory[] solve(
41
			TrajectoryOptimizer[] trajectoryOptimizers,
42
			final PenaltyFunction[][] penaltyFunctions, int k, double maxCost,
43
			long runtimeDeadlineMs, boolean showProgressArg) {
33
    public static EvaluatedTrajectory[] solve(
34
            TrajectoryOptimizer[] trajectoryOptimizers,
35
            final PenaltyFunction[][] penaltyFunctions, int k, double maxCost,
36
            long runtimeDeadlineMs, boolean showProgressArg) {
44 37

  
45
		int nAgents = trajectoryOptimizers.length;
46
		showProgress = showProgressArg;
47
		Verbose.setVerbose(showProgress);
38
        int nAgents = trajectoryOptimizers.length;
39
        showProgress = showProgressArg;
40
        Verbose.setVerbose(showProgress);
48 41

  
49
		// Debug visualisation
50
		if (showProgress) {
51
			bodyRadiuses = new int[penaltyFunctions.length];
52
			// HACK: Assuming that everyone has the same radius and that we have at least two agents
53
			int radius = 1;
54
			if (penaltyFunctions[0][1] instanceof BumpSeparationPenaltyFunction) {
55
				 radius = (int) (((BumpSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
56
			} else if (penaltyFunctions[0][1] instanceof LinearSeparationPenaltyFunction) {
57
				radius = (int) (((LinearSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
58
			} else if (penaltyFunctions[0][1] instanceof ConstantSeparationPenaltyFunction) {
59
				radius = (int) (((ConstantSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
60
			}
42
        // Debug visualisation
43
        if (showProgress) {
44
            bodyRadiuses = new int[penaltyFunctions.length];
45
            // HACK: Assuming that everyone has the same radius and that we have at least two agents
46
            int radius = 1;
47
            if (penaltyFunctions[0][1] instanceof BumpSeparationPenaltyFunction) {
48
                 radius = (int) (((BumpSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
49
            } else if (penaltyFunctions[0][1] instanceof LinearSeparationPenaltyFunction) {
50
                radius = (int) (((LinearSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
51
            } else if (penaltyFunctions[0][1] instanceof ConstantSeparationPenaltyFunction) {
52
                radius = (int) (((ConstantSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
53
            }
61 54

  
62
			for (int i=0; i<trajectoryOptimizers.length; i++) {
63
				bodyRadiuses[i] = radius;
64
			}
65
			initProgressVisio();
66
		}
55
            for (int i=0; i<trajectoryOptimizers.length; i++) {
56
                bodyRadiuses[i] = radius;
57
            }
58
            initProgressVisio();
59
        }
67 60

  
68
		EvaluatedTrajectory[] trajectories = new EvaluatedTrajectory[nAgents];
69
		if (k == 1) {
70
			trajectories = prioritizedPlanning(trajectories,
71
					trajectoryOptimizers, penaltyFunctions, maxCost,
72
					runtimeDeadlineMs);
73
		}
74
		if (k >= 2) {
75
			// Find initial unconstrained trajectories
76
			trajectories = initUnconstrainedTrajectories(trajectoryOptimizers,
77
					maxCost, runtimeDeadlineMs);
61
        EvaluatedTrajectory[] trajectories = new EvaluatedTrajectory[nAgents];
62
        if (k == 1) {
63
            trajectories = prioritizedPlanning(trajectories,
64
                    trajectoryOptimizers, penaltyFunctions, maxCost,
65
                    runtimeDeadlineMs);
66
        }
78 67

  
79
			if (trajectories == null)
80
				return null;
68
        if (k >= 2) {
69
            // Find initial unconstrained trajectories
70
            trajectories = initUnconstrainedTrajectories(trajectoryOptimizers,
71
                    maxCost, runtimeDeadlineMs);
81 72

  
82
			// Iteratively replan
83
			int iterations = nAgents * (k - 2);
84
			trajectories = iterativelyReplan(trajectories,
85
					trajectoryOptimizers, penaltyFunctions, iterations,
86
					maxCost, runtimeDeadlineMs);
73
            if (trajectories == null)
74
                return null;
87 75

  
88
			if (trajectories == null)
89
				return null;
76
            if (k >= 3) {
77
                // Iteratively replan
78
                int iterations = nAgents * (k - 2);
79
                trajectories = iterativelyReplan(trajectories,
80
                        trajectoryOptimizers, penaltyFunctions, iterations,
81
                        maxCost, runtimeDeadlineMs);
90 82

  
91
			// Replan with hard-constraints
92
			trajectories = replanWithInfiniteWeight(trajectories,
93
			trajectoryOptimizers, penaltyFunctions, maxCost,
94
			runtimeDeadlineMs);
83
                if (trajectories == null)
84
                    return null;
95 85

  
96
			// !!! TODO check if the trajectories are conflict-free !!!
97
		}
86
            }
98 87

  
99
		return trajectories;
100
	}
88
            // Replan with hard-constraints
89
            trajectories = replanWithInfiniteWeight(trajectories,
90
            trajectoryOptimizers, penaltyFunctions, maxCost,
91
            runtimeDeadlineMs);
92
        }
101 93

  
102
	private static void initProgressVisio() {
94
        return trajectories;
95
    }
103 96

  
104
		TimeParameter time = new TimeParameter(5);
105
		VisManager.registerLayer(ParameterControlLayer.create(time));
97
    private static void initProgressVisio() {
98

  
99
        if (TimeParameterHolder.time == null) {
100
            TimeParameterHolder.time = new TimeParameter(10);
101
            VisManager.registerLayer(ParameterControlLayer.create(TimeParameterHolder.time));
102
        }
106 103

  
107 104
        KeyToggleLayer toggleLayer = KeyToggleLayer.create("p");
108 105

  
109 106
        toggleLayer.addSubLayer(FastTrajectoriesLayer.create(
110
        				new TrajectoriesProvider() {
107
                        new TrajectoriesProvider() {
111 108

  
112
        					@Override
113
        					public Trajectory[] getTrajectories() {
114
        						return currentTrajectories;
115
        					}
116
        				}, new ColorProvider() {
109
                            @Override
110
                            public Trajectory[] getTrajectories() {
111
                                return currentTrajectories;
112
                            }
113
                        }, new ColorProvider() {
117 114

  
118
        					@Override
119
        					public Color getColor(int i) {
120
        						return AgentColors.getColorForAgent(i);
121
        					}
115
                            @Override
116
                            public Color getColor(int i) {
117
                                return AgentColors.getColorForAgent(i);
118
                            }
122 119

  
123
        				}, 2, 5));
120
                        }, 2, 5));
124 121

  
125 122

  
126 123

  
127 124
        toggleLayer.addSubLayer(FastAgentsLayer.create(new FastAgentsLayer.TrajectoriesProvider() {
128 125

  
129
			@Override
130
			public Trajectory[] getTrajectories() {
131
				return currentTrajectories;
132
			}
126
            @Override
127
            public Trajectory[] getTrajectories() {
128
                return currentTrajectories;
129
            }
133 130

  
134
			@Override
135
			public int[] getBodyRadiuses() {
136
				return bodyRadiuses;
137
			}
138
		}, new FastAgentsLayer.ColorProvider() {
139
			@Override
140
			public Color getColor(int i) {
141
				return AgentColors.getColorForAgent(i);
142
			}
143
		}, time));
131
            @Override
132
            public int[] getBodyRadiuses() {
133
                return bodyRadiuses;
134
            }
135
        }, new FastAgentsLayer.ColorProvider() {
136
            @Override
137
            public Color getColor(int i) {
138
                return AgentColors.getColorForAgent(i);
139
            }
140
        }, TimeParameterHolder.time));
144 141

  
145 142
        VisManager.registerLayer(toggleLayer);
146
	}
143
    }
147 144

  
148
	private static EvaluatedTrajectory[] prioritizedPlanning(
149
			EvaluatedTrajectory[] trajectories,
150
			TrajectoryOptimizer[] trajectoryOptimizers,
151
			final PenaltyFunction[][] penaltyFunctions, double maxCost,
152
			long runtimeDeadlineMs) {
153
		int nAgents = trajectoryOptimizers.length;
145
    private static EvaluatedTrajectory[] prioritizedPlanning(
146
            EvaluatedTrajectory[] trajectories,
147
            TrajectoryOptimizer[] trajectoryOptimizers,
148
            final PenaltyFunction[][] penaltyFunctions, double maxCost,
149
            long runtimeDeadlineMs) {
150
        int nAgents = trajectoryOptimizers.length;
154 151

  
155
		for (int i = 0; i < nAgents; i++) {
152
        for (int i = 0; i < nAgents; i++) {
156 153

  
157
			Verbose.printf("Prioritized planning for agent %d. \n", i);
154
            Verbose.printf("Prioritized planning for agent %d. \n", i);
158 155

  
159
			Trajectory[] otherTrajectories = higherPriorityTrajectories(
160
					trajectories, i);
161
			PenaltyFunction[] penaltyFunctionsToOtherTrajectories = constraintsWithHigherPriorityAgents(
162
					penaltyFunctions, i);
163
			penaltyFunctionsToOtherTrajectories = applyWeight(
164
					penaltyFunctionsToOtherTrajectories,
165
					Double.POSITIVE_INFINITY);
156
            Trajectory[] otherTrajectories = higherPriorityTrajectories(
157
                    trajectories, i);
158
            PenaltyFunction[] penaltyFunctionsToOtherTrajectories = constraintsWithHigherPriorityAgents(
159
                    penaltyFunctions, i);
160
            penaltyFunctionsToOtherTrajectories = applyWeight(
161
                    penaltyFunctionsToOtherTrajectories,
162
                    Double.POSITIVE_INFINITY);
166 163

  
167
			EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[i]
168
					.getOptimalTrajectoryConstrained(
169
							penaltyFunctionsToOtherTrajectories,
170
							otherTrajectories, null, maxCost, runtimeDeadlineMs);
164
            EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[i]
165
                    .getOptimalTrajectoryConstrained(
166
                            penaltyFunctionsToOtherTrajectories,
167
                            otherTrajectories, null, maxCost, runtimeDeadlineMs);
171 168

  
172
			if (bestResponseTraj != null) {
173
				trajectories[i] = bestResponseTraj;
174
				currentTrajectories = trajectories;
175
			} else {
176
				return null;
177
			}
178
		}
169
            if (bestResponseTraj != null) {
170
                trajectories[i] = bestResponseTraj;
171
                currentTrajectories = trajectories;
172
            } else {
173
                return null;
174
            }
175
        }
179 176

  
180
		return trajectories;
181
	}
177
        return trajectories;
178
    }
182 179

  
183
	private static EvaluatedTrajectory[] iterativelyReplan(
184
			EvaluatedTrajectory[] trajectories,
185
			TrajectoryOptimizer[] trajectoryOptimizers,
186
			final PenaltyFunction[][] penaltyFunctions, int nIterations,
187
			double maxCost, long runtimeDeadlineMs) {
188
		int nAgents = trajectoryOptimizers.length;
180
    private static EvaluatedTrajectory[] iterativelyReplan(
181
            EvaluatedTrajectory[] trajectories,
182
            TrajectoryOptimizer[] trajectoryOptimizers,
183
            final PenaltyFunction[][] penaltyFunctions, int nIterations,
184
            double maxCost, long runtimeDeadlineMs) {
185
        int nAgents = trajectoryOptimizers.length;
189 186

  
190
		double gcost = computeGlobalCost(trajectories);
191
		double gpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
187
        double gcost = computeGlobalCost(trajectories);
188
        double gpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
192 189

  
193
		for (int i = 0; i < nIterations; i++) {
194
			int r = i % nAgents;
190
        for (int i = 0; i < nIterations; i++) {
191
            int r = i % nAgents;
195 192

  
196 193

  
197 194

  
198 195

  
199
			double w = calculateSoftConstraintsWeight(i, nIterations);
200
			Verbose.printf("Agent: %d; k:%d/%d  w: %.4f. \n", r, i, nIterations, w);
196
            double w = calculateSoftConstraintsWeight(i, nIterations);
197
            Verbose.printf("Agent: %d; k:%d/%d  w: %.4f. \n", r, i, nIterations, w);
201 198

  
202 199

  
203
			Trajectory[] otherTrajectories = otherTrajectories(trajectories, r);
204
			PenaltyFunction[] penaltyFunctionsOfOtherTrajectories = constraintsOf(
205
					penaltyFunctions, r);
200
            Trajectory[] otherTrajectories = otherTrajectories(trajectories, r);
201
            PenaltyFunction[] penaltyFunctionsOfOtherTrajectories = constraintsOf(
202
                    penaltyFunctions, r);
206 203

  
207
			PenaltyFunction[] penaltyFunctionsToOtherTrajectoriesWeighted = applyWeight(
208
					penaltyFunctionsOfOtherTrajectories, w);
204
            PenaltyFunction[] penaltyFunctionsToOtherTrajectoriesWeighted = applyWeight(
205
                    penaltyFunctionsOfOtherTrajectories, w);
209 206

  
210
			printPenaltyBreakout(trajectories[r],
211
					penaltyFunctionsOfOtherTrajectories, otherTrajectories, w);
207
            printPenaltyBreakout(trajectories[r],
208
                    penaltyFunctionsOfOtherTrajectories, otherTrajectories, w);
212 209

  
213
			EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[r]
214
					.getOptimalTrajectoryConstrained(
215
							penaltyFunctionsToOtherTrajectoriesWeighted,
216
							otherTrajectories, trajectories[r], maxCost,
217
							runtimeDeadlineMs);
210
            EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[r]
211
                    .getOptimalTrajectoryConstrained(
212
                            penaltyFunctionsToOtherTrajectoriesWeighted,
213
                            otherTrajectories, trajectories[r], maxCost,
214
                            runtimeDeadlineMs);
218 215

  
219
			if (bestResponseTraj != null) {
220
				trajectories[r] = bestResponseTraj;
221
				currentTrajectories = trajectories;
222
			} else {
223
				return null;
224
			}
216
            if (bestResponseTraj != null) {
217
                trajectories[r] = bestResponseTraj;
218
                currentTrajectories = trajectories;
219
            } else {
220
                return null;
221
            }
225 222

  
226
			double newgcost = computeGlobalCost(trajectories);
227
			double newgpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
223
            double newgcost = computeGlobalCost(trajectories);
224
            double newgpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
228 225

  
229
			Verbose.printf("Global cost: %.4f (Δ: %.4f) Global Penalty: %.4f (Δ: %.4f)\n\n", newgcost, (newgcost-gcost), newgpenalty, (newgpenalty-gpenalty));
226
            Verbose.printf("Global cost: %.4f (Δ: %.4f) Global Penalty: %.4f (Δ: %.4f)\n\n", newgcost, (newgcost-gcost), newgpenalty, (newgpenalty-gpenalty));
230 227

  
231
			gcost = newgcost;
232
			gpenalty = newgpenalty;
233
		}
228
            gcost = newgcost;
229
            gpenalty = newgpenalty;
230
        }
234 231

  
235
		return trajectories;
236
	}
232
        return trajectories;
233
    }
237 234

  
238
	private static double computeGlobalPenalty(
239
			PenaltyFunction[][] penaltyFunctions,
240
			EvaluatedTrajectory[] trajectories) {
241
		double penalty = 0;
242
		for (int i = 0; i < penaltyFunctions.length; i++) {
243
			for (int j = 0; j < i; j++) {
244
				penalty += PenaltyIntegrator.integratePenalty(trajectories[i], penaltyFunctions[i][j], trajectories[j], 10);
245
			}
246
		}
247
		return penalty;
235
    private static double computeGlobalPenalty(
236
            PenaltyFunction[][] penaltyFunctions,
237
            EvaluatedTrajectory[] trajectories) {
238
        double penalty = 0;
239
        for (int i = 0; i < penaltyFunctions.length; i++) {
240
            for (int j = 0; j < i; j++) {
241
                penalty += PenaltyIntegrator.integratePenalty(trajectories[i], penaltyFunctions[i][j], trajectories[j], 10);
242
            }
243
        }
244
        return penalty;
248 245

  
249
	}
246
    }
250 247

  
251
	private static double computeGlobalCost(EvaluatedTrajectory[] trajectories) {
252
		double cost = 0;
253
		for (int i = 0; i < trajectories.length; i++) {
254
			cost += trajectories[i].getCost();
255
		}
256
		return cost;
257
	}
248
    private static double computeGlobalCost(EvaluatedTrajectory[] trajectories) {
249
        double cost = 0;
250
        for (int i = 0; i < trajectories.length; i++) {
251
            cost += trajectories[i].getCost();
252
        }
253
        return cost;
254
    }
258 255

  
259
	private static void printPenaltyBreakout(EvaluatedTrajectory thisTraj,
260
			PenaltyFunction[] penaltyFunctionsOfOtherTrajectories,
261
			Trajectory[] otherTrajectories, double w) {
256
    private static void printPenaltyBreakout(EvaluatedTrajectory thisTraj,
257
            PenaltyFunction[] penaltyFunctionsOfOtherTrajectories,
258
            Trajectory[] otherTrajectories, double w) {
262 259

  
263
		Verbose.printf("cost: %.4f\t", thisTraj.getCost());
260
        Verbose.printf("cost: %.4f\t", thisTraj.getCost());
264 261

  
265
		for (int i = 0; i < otherTrajectories.length; i++) {
266
			double penalty = PenaltyIntegrator.integratePenalty(thisTraj,
267
					penaltyFunctionsOfOtherTrajectories[i],
268
					otherTrajectories[i], 10);
262
        for (int i = 0; i < otherTrajectories.length; i++) {
263
            double penalty = PenaltyIntegrator.integratePenalty(thisTraj,
264
                    penaltyFunctionsOfOtherTrajectories[i],
265
                    otherTrajectories[i], 10);
269 266

  
270
			Verbose.printf("%.4f * %.4f = %.4f\t", penalty, w, penalty*w);
271
		}
267
            Verbose.printf("%.4f * %.4f = %.4f\t", penalty, w, penalty*w);
268
        }
272 269

  
273
		Verbose.print("\n");
274
	}
270
        Verbose.print("\n");
271
    }
275 272

  
276
	private static PenaltyFunction[] applyWeight(
277
			final PenaltyFunction[] penaltyFunctions, final double w) {
273
    private static PenaltyFunction[] applyWeight(
274
            final PenaltyFunction[] penaltyFunctions, final double w) {
278 275

  
279
		PenaltyFunction[] weighted = new PenaltyFunction[penaltyFunctions.length];
276
        PenaltyFunction[] weighted = new PenaltyFunction[penaltyFunctions.length];
280 277

  
281
		for (int i = 0; i < weighted.length; i++) {
282
			final int iFinal = i;
283
			weighted[i] = new PenaltyFunction() {
278
        for (int i = 0; i < weighted.length; i++) {
279
            final int iFinal = i;
280
            weighted[i] = new PenaltyFunction() {
284 281

  
285
				@Override
286
				public double getPenalty(double dist, double t) {
287
					double penalty = penaltyFunctions[iFinal].getPenalty(dist,
288
							t);
289
					if (penalty == 0) {
290
						return 0;
291
					} else {
292
						return w * penaltyFunctions[iFinal].getPenalty(dist, t);
293
					}
294
				}
295
			};
296
		}
282
                @Override
283
                public double getPenalty(double dist, double t) {
284
                    double penalty = penaltyFunctions[iFinal].getPenalty(dist,
285
                            t);
286
                    if (penalty == 0) {
287
                        return 0;
288
                    } else {
289
                        return w * penaltyFunctions[iFinal].getPenalty(dist, t);
290
                    }
291
                }
292
            };
293
        }
297 294

  
298
		return weighted;
299
	}
295
        return weighted;
296
    }
300 297

  
301
	private static EvaluatedTrajectory[] replanWithInfiniteWeight(
302
			EvaluatedTrajectory[] trajectories,
303
			TrajectoryOptimizer[] trajectoryOptimizers,
304
			final PenaltyFunction[][] penaltyFunctions, double maxCost,
305
			long runtimeDeadlineMs) {
306
		int nAgents = trajectoryOptimizers.length;
298
    private static EvaluatedTrajectory[] replanWithInfiniteWeight(
299
            EvaluatedTrajectory[] trajectories,
300
            TrajectoryOptimizer[] trajectoryOptimizers,
301
            final PenaltyFunction[][] penaltyFunctions, double maxCost,
302
            long runtimeDeadlineMs) {
303
        int nAgents = trajectoryOptimizers.length;
307 304

  
308
		for (int i = 0; i < nAgents; i++) {
305
        for (int i = 0; i < nAgents; i++) {
309 306

  
310
			Verbose.printf("Hard constraints planning for agent %d. \n", i);
307
            Verbose.printf("Hard constraints planning for agent %d. \n", i);
311 308

  
312
			Trajectory[] otherTrajectories = otherTrajectories(trajectories, i);
313
			PenaltyFunction[] penaltyFunctionsOfOtherTrajectories = constraintsOf(
314
					penaltyFunctions, i);
315
			penaltyFunctionsOfOtherTrajectories = applyWeight(
316
					penaltyFunctionsOfOtherTrajectories,
317
					Double.POSITIVE_INFINITY);
309
            Trajectory[] otherTrajectories = otherTrajectories(trajectories, i);
310
            PenaltyFunction[] penaltyFunctionsOfOtherTrajectories = constraintsOf(
311
                    penaltyFunctions, i);
312
            penaltyFunctionsOfOtherTrajectories = applyWeight(
313
                    penaltyFunctionsOfOtherTrajectories,
314
                    Double.POSITIVE_INFINITY);
318 315

  
319
			EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[i]
320
					.getOptimalTrajectoryConstrained(
321
							penaltyFunctionsOfOtherTrajectories,
322
							otherTrajectories, trajectories[i], maxCost,
323
							runtimeDeadlineMs);
316
            EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[i]
317
                    .getOptimalTrajectoryConstrained(
318
                            penaltyFunctionsOfOtherTrajectories,
319
                            otherTrajectories, trajectories[i], maxCost,
320
                            runtimeDeadlineMs);
324 321

  
325
			if (bestResponseTraj != null) {
326
				trajectories[i] = bestResponseTraj;
327
				currentTrajectories = trajectories;
328
			} else {
329
				return null;
330
			}
331
		}
322
            if (bestResponseTraj != null) {
323
                trajectories[i] = bestResponseTraj;
324
                currentTrajectories = trajectories;
325
            } else {
326
                return null;
327
            }
328
        }
332 329

  
333
		return trajectories;
334
	}
330
        return trajectories;
331
    }
335 332

  
336
	private static EvaluatedTrajectory[] initUnconstrainedTrajectories(
337
			TrajectoryOptimizer[] trajectoryOptimizers, double maxCost,
338
			long runtimeDeadlineMs) {
333
    private static EvaluatedTrajectory[] initUnconstrainedTrajectories(
334
            TrajectoryOptimizer[] trajectoryOptimizers, double maxCost,
335
            long runtimeDeadlineMs) {
339 336

  
340
		EvaluatedTrajectory[] trajectories = new EvaluatedTrajectory[trajectoryOptimizers.length];
341
		currentTrajectories = trajectories;
337
        EvaluatedTrajectory[] trajectories = new EvaluatedTrajectory[trajectoryOptimizers.length];
338
        currentTrajectories = trajectories;
342 339

  
343
		for (int i = 0; i < trajectoryOptimizers.length; i++) {
344
			Verbose.printf("Unconstrained planning for agent %d. \n", i);
345
			trajectories[i] = trajectoryOptimizers[i]
346
					.getOptimalTrajectoryUnconstrained(maxCost,
347
							runtimeDeadlineMs);
348
		}
340
        for (int i = 0; i < trajectoryOptimizers.length; i++) {
341
            Verbose.printf("Unconstrained planning for agent %d. \n", i);
342
            trajectories[i] = trajectoryOptimizers[i]
343
                    .getOptimalTrajectoryUnconstrained(maxCost,
344
                            runtimeDeadlineMs);
349 345

  
350
		return trajectories;
351
	}
346
            if (trajectories[i] == null) {
347
                return null;
348
            }
349
        }
352 350

  
353
	private static void assertSymmetric(PairwiseConstraint[][] constraints) {
354
		for (int i = 0; i < constraints.length; i++) {
355
			for (int j = 0; j < constraints[i].length; j++) {
356
				if (i != j) {
357
					assert constraints[i][j] == constraints[j][i];
358
				}
359
			}
360
		}
361
	}
351
        return trajectories;
352
    }
362 353

  
363
	private static double calculateSoftConstraintsWeight(int i,
364
			int maxIterations) {
365
		return TAN_COEF * Math.tan(Math.PI / 2 * i / (maxIterations + 1));
366
	}
354
    private static void assertSymmetric(PairwiseConstraint[][] constraints) {
355
        for (int i = 0; i < constraints.length; i++) {
356
            for (int j = 0; j < constraints[i].length; j++) {
357
                if (i != j) {
358
                    assert constraints[i][j] == constraints[j][i];
359
                }
360
            }
361
        }
362
    }
367 363

  
368
	private static Trajectory[] higherPriorityTrajectories(Trajectory[] trajs,
369
			int id) {
370
		return Arrays.copyOfRange(trajs, 0, id);
371
	}
364
    private static double calculateSoftConstraintsWeight(int i,
365
            int maxIterations) {
366
        return TAN_COEF * Math.tan(Math.PI / 2 * i / (maxIterations + 1));
367
    }
372 368

  
373
	private static PenaltyFunction[] constraintsWithHigherPriorityAgents(
374
			PenaltyFunction[][] penaltyFunctions, int id) {
375
		return Arrays.copyOfRange(penaltyFunctions[id], 0, id);
376
	}
369
    private static Trajectory[] higherPriorityTrajectories(Trajectory[] trajs,
370
            int id) {
371
        return Arrays.copyOfRange(trajs, 0, id);
372
    }
377 373

  
378
	private static Trajectory[] otherTrajectories(Trajectory[] trajs,
379
			int excludeId) {
380
		Trajectory[] otherTrajs = new Trajectory[trajs.length - 1];
381
		int j = 0;
382
		for (int i = 0; i < trajs.length; i++) {
383
			if (i != excludeId)
384
				otherTrajs[j++] = trajs[i];
385
		}
386
		return otherTrajs;
387
	}
374
    private static PenaltyFunction[] constraintsWithHigherPriorityAgents(
375
            PenaltyFunction[][] penaltyFunctions, int id) {
376
        return Arrays.copyOfRange(penaltyFunctions[id], 0, id);
377
    }
388 378

  
389
	private static PenaltyFunction[] constraintsOf(
390
			PenaltyFunction[][] penaltyFunctions, int excludeId) {
391
		PenaltyFunction[] otherConstraints = new PenaltyFunction[penaltyFunctions[excludeId].length - 1];
392
		int j = 0;
393
		for (int i = 0; i < penaltyFunctions[excludeId].length; i++) {
394
			if (i != excludeId)
395
				otherConstraints[j++] = penaltyFunctions[excludeId][i];
396
		}
397
		return otherConstraints;
398
	}
379
    private static Trajectory[] otherTrajectories(Trajectory[] trajs,
380
            int excludeId) {
381
        Trajectory[] otherTrajs = new Trajectory[trajs.length - 1];
382
        int j = 0;
383
        for (int i = 0; i < trajs.length; i++) {
384
            if (i != excludeId)
385
                otherTrajs[j++] = trajs[i];
386
        }
387
        return otherTrajs;
388
    }
389

  
390
    private static PenaltyFunction[] constraintsOf(
391
            PenaltyFunction[][] penaltyFunctions, int excludeId) {
392
        PenaltyFunction[] otherConstraints = new PenaltyFunction[penaltyFunctions[excludeId].length - 1];
393
        int j = 0;
394
        for (int i = 0; i < penaltyFunctions[excludeId].length; i++) {
395
            if (i != excludeId)
396
                otherConstraints[j++] = penaltyFunctions[excludeId][i];
397
        }
398
        return otherConstraints;
399
    }
399 400

  
400 401

  
401 402

  
src/main/java/tt/vis/TimeParameterHolder.java
1
package tt.vis;
2

  
3
import tt.euclidtime3i.vis.TimeParameter;
4

  
5
public class TimeParameterHolder {
6
    static public TimeParameter time;
7
}
src/main/resources/problems/superconflict.xml
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<!-- X: 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 782.8427 900.0000 -->
3
<!-- Y: 500.0000 782.8427 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 -->
4
<multiagentproblem>
5
<environment>
6
<obstacles/>
7
<bounds>
8
<point>
9
<x>-1000</x>
10
<y>-1000</y>
11
</point>
12
<point>
13
<x>2000</x>
14
<y>2000</y>
15
</point>
16
</bounds>
17
</environment>
18
<agents>
19

  
20
<agent>
21
<start>
22
<point>
23
<x>900</x>
24
<y>500</y>
25
</point>
26
</start>
27
<target>
28
<point>
29
<x>100</x>
30
<y>500</y>
31
</point>
32
</target>
33
<radius>70</radius>
34
<maxspeed>1</maxspeed>
35
</agent>
36

  
37
<agent>
38
<start>
39
<point>
40
<x>782</x>
41
<y>782</y>
42
</point>
43
</start>
44
<target>
45
<point>
46
<x>217</x>
47
<y>217</y>
48
</point>
49
</target>
50
<radius>70</radius>
51
<maxspeed>1</maxspeed>
52
</agent>
53

  
54
<agent>
55
<start>
56
<point>
57
<x>500</x>
58
<y>900</y>
59
</point>
60
</start>
61
<target>
62
<point>
63
<x>500</x>
64
<y>100</y>
65
</point>
66
</target>
67
<radius>70</radius>
68
<maxspeed>1</maxspeed>
69
</agent>
70

  
71
<agent>
72
<start>
73
<point>
74
<x>217</x>
75
<y>782</y>
76
</point>
77
</start>
78
<target>
79
<point>
80
<x>782</x>
81
<y>217</y>
82
</point>
83
</target>
84
<radius>70</radius>
85
<maxspeed>1</maxspeed>
86
</agent>
87

  
88
<agent>
89
<start>
90
<point>
91
<x>100</x>
92
<y>500</y>
93
</point>
94
</start>
95
<target>
96
<point>
97
<x>900</x>
98
<y>500</y>
99
</point>
100
</target>
101
<radius>70</radius>
102
<maxspeed>1</maxspeed>
103
</agent>
104

  
105
<agent>
106
<start>
107
<point>
108
<x>217</x>
109
<y>217</y>
110
</point>
111
</start>
112
<target>
113
<point>
114
<x>782</x>
115
<y>782</y>
116
</point>
117
</target>
118
<radius>70</radius>
119
<maxspeed>1</maxspeed>
120
</agent>
121

  
122
<agent>
123
<start>
124
<point>
125
<x>500</x>
126
<y>100</y>
127
</point>
128
</start>
129
<target>
130
<point>
131
<x>500</x>
132
<y>900</y>
133
</point>
134
</target>
135
<radius>70</radius>
136
<maxspeed>1</maxspeed>
137
</agent>
138

  
139
<agent>
140
<start>
141
<point>
142
<x>782</x>
143
<y>217</y>
144
</point>
145
</start>
146
<target>
147
<point>
148
<x>217</x>
149
<y>782</y>
150
</point>
151
</target>
152
<radius>70</radius>
153
<maxspeed>1</maxspeed>
154
</agent>
155

  
156
</agents>
157
</multiagentproblem>
src/main/resources/problems/superconflict4.xml
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<!-- X: 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 782.8427 900.0000 -->
3
<!-- Y: 500.0000 782.8427 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 -->
4
<multiagentproblem>
5
<environment>
6
<obstacles/>
7
<bounds>
8
<point>
9
<x>-1000</x>
10
<y>-1000</y>
11
</point>
12
<point>
13
<x>2000</x>
14
<y>2000</y>
15
</point>
16
</bounds>
17
</environment>
18
<agents>
19

  
20
<agent>
21
<start>
22
<point>
23
<x>900</x>
24
<y>500</y>
25
</point>
26
</start>
27
<target>
28
<point>
29
<x>100</x>
30
<y>500</y>
31
</point>
32
</target>
33
<radius>70</radius>
34
<maxspeed>1</maxspeed>
35
</agent>
36

  
37

  
38
<agent>
39
<start>
40
<point>
41
<x>500</x>
42
<y>900</y>
43
</point>
44
</start>
45
<target>
46
<point>
47
<x>500</x>
48
<y>100</y>
49
</point>
50
</target>
51
<radius>70</radius>
52
<maxspeed>1</maxspeed>
53
</agent>
54

  
55

  
56
<agent>
57
<start>
58
<point>
59
<x>100</x>
60
<y>500</y>
61
</point>
62
</start>
63
<target>
64
<point>
65
<x>900</x>
66
<y>500</y>
67
</point>
68
</target>
69
<radius>70</radius>
70
<maxspeed>1</maxspeed>
71
</agent>
72

  
73
<agent>
74
<start>
75
<point>
76
<x>500</x>
77
<y>100</y>
78
</point>
79
</start>
80
<target>
81
<point>
82
<x>500</x>
83
<y>900</y>
84
</point>
85
</target>
86
<radius>70</radius>
87
<maxspeed>1</maxspeed>
88
</agent>
89

  
90

  
91
</agents>
92
</multiagentproblem>
src/main/resources/problems/superconflict8.xml
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<!-- X: 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 782.8427 900.0000 -->
3
<!-- Y: 500.0000 782.8427 900.0000 782.8427 500.0000 217.1573 100.0000 217.1573 500.0000 -->
4
<multiagentproblem>
5
<environment>
6
<obstacles/>
7
<bounds>
8
<point>
9
<x>-1000</x>
10
<y>-1000</y>
11
</point>
12
<point>
13
<x>2000</x>
14
<y>2000</y>
15
</point>
16
</bounds>
17
</environment>
18
<agents>
19

  
20
<agent>
21
<start>
22
<point>
23
<x>900</x>
24
<y>500</y>
25
</point>
26
</start>
27
<target>
28
<point>
29
<x>100</x>
30
<y>500</y>
31
</point>
32
</target>
33
<radius>70</radius>
34
<maxspeed>1</maxspeed>
35
</agent>
36

  
37
<agent>
38
<start>
39
<point>
40
<x>782</x>
41
<y>782</y>
42
</point>
43
</start>
44
<target>
45
<point>
46
<x>217</x>
47
<y>217</y>
48
</point>
49
</target>
50
<radius>70</radius>
51
<maxspeed>1</maxspeed>
52
</agent>
53

  
54
<agent>
55
<start>
56
<point>
57
<x>500</x>
58
<y>900</y>
59
</point>
60
</start>
61
<target>
62
<point>
63
<x>500</x>
64
<y>100</y>
65
</point>
66
</target>
67
<radius>70</radius>
68
<maxspeed>1</maxspeed>
69
</agent>
70

  
71
<agent>
72
<start>
73
<point>
74
<x>217</x>
75
<y>782</y>
76
</point>
77
</start>
78
<target>
79
<point>
80
<x>782</x>
81
<y>217</y>
82
</point>
83
</target>
84
<radius>70</radius>
85
<maxspeed>1</maxspeed>
86
</agent>
87

  
88
<agent>
89
<start>
90
<point>
91
<x>100</x>
92
<y>500</y>
93
</point>
94
</start>
95
<target>
96
<point>
97
<x>900</x>
98
<y>500</y>
99
</point>
100
</target>
101
<radius>70</radius>
102
<maxspeed>1</maxspeed>
103
</agent>
104

  
105
<agent>
106
<start>
107
<point>
108
<x>217</x>
109
<y>217</y>
110
</point>
111
</start>
112
<target>
113
<point>
114
<x>782</x>
115
<y>782</y>
116
</point>
117
</target>
118
<radius>70</radius>
119
<maxspeed>1</maxspeed>
120
</agent>
121

  
122
<agent>
123
<start>
124
<point>
125
<x>500</x>
126
<y>100</y>
127
</point>
128
</start>
129
<target>
130
<point>
131
<x>500</x>
132
<y>900</y>
133
</point>
134
</target>
135
<radius>70</radius>
136
<maxspeed>1</maxspeed>
137
</agent>
138

  
139
<agent>
140
<start>
141
<point>
142
<x>782</x>
143
<y>217</y>
144
</point>
145
</start>
146
<target>
147
<point>
148
<x>217</x>
149
<y>782</y>
150
</point>
151
</target>
152
<radius>70</radius>
153
<maxspeed>1</maxspeed>
154
</agent>
155

  
156
</agents>
157
</multiagentproblem>

Also available in: Unified diff