Revision 137:498c57de5543

View differences:

src/main/java/tt/jointeuclid2ni/sipp/SippUtils.java
50 50
        return new Interval(startTime, startTime + duration);
51 51
    }
52 52

  
53
    public static <E extends Line> SafeIntervalList safeIntervalsForSingleEdge(E edge, DynamicEnvironment environment, int step) {
53
    public static <E extends Line> SafeIntervalList safeIntervalsForSingleEdge(E edge, DynamicEnvironment environment, int bodyRadius, int step) {
54 54
        //could be implemented using variant for multiple points, but than there would be unnecessary overhead and vice versa
55 55
        SafeIntervalList intervalCollection = new SafeIntervalList();
56 56

  
57 57
        Trajectory[] obstacles = environment.getObstacles();
58
        int[] separations = environment.getSeparations();
58
        int[] radiuses = environment.getObstacleRadiuses();
59 59

  
60 60
        for (int i = 0; ; i++) {
61 61
            int time = i * step;
......
66 66
                Point obstacle = trajectory.get(time);
67 67

  
68 68
                double minDist = Geometry2i.distance(edge, obstacle);
69
                if (minDist < separations[j])
69
                if (minDist < radiuses[j] + bodyRadius)
70 70
                    intervalCollection.markCollisionInTime(i, time);
71 71
            }
72 72

  
......
76 76
        return intervalCollection;
77 77
    }
78 78

  
79
    public static <V extends Point> SafeIntervalList safeIntervalsForSinglePoint(V point, DynamicEnvironment environment, int step) {
79
    public static <V extends Point> SafeIntervalList safeIntervalsForSinglePoint(V point, DynamicEnvironment environment, int bodyRadius, int step) {
80 80
        //could be implemented using variant for multiple points, but than there would be unnecessary overhead and vice versa
81 81
        SafeIntervalList intervalCollection = new SafeIntervalList();
82 82

  
83 83
        Trajectory[] obstacles = environment.getObstacles();
84
        int[] separations = environment.getSeparations();
84
        int[] radiuses = environment.getObstacleRadiuses();
85 85

  
86 86
        for (int i = 0; ; i++) {
87 87
            int time = i * step;
......
91 91
                    continue;
92 92

  
93 93
                Point obstaclePosition = obstacles[a].get(time);
94
                if (obstaclePosition.distance(point) < separations[a])
94
                if (obstaclePosition.distance(point) < radiuses[a] + bodyRadius)
95 95
                    intervalCollection.markCollisionInTime(i, time);
96 96
            }
97 97

  
......
102 102
        return intervalCollection;
103 103
    }
104 104

  
105
    public static <E extends Line> SafeIntervalBuilder<E> safeIntervalsForEdges(Set<E> edges, DynamicEnvironment environment, int step) {
105
    public static <E extends Line> SafeIntervalBuilder<E> safeIntervalsForEdges(Set<E> edges, DynamicEnvironment environment, int bodyRadius, int step) {
106 106
        SafeIntervalBuilder<E> edgeSIBuilder = new SafeIntervalBuilder<E>();
107 107

  
108 108
        Trajectory[] obstacles = environment.getObstacles();
109
        int[] separations = environment.getSeparations();
109
        int[] radiuses = environment.getObstacleRadiuses();
110 110

  
111 111
        for (int i = 0; ; i++) {
112 112
            int time = i * step;
......
118 118

  
119 119
                for (E edge : edges) {
120 120
                    double minDist = Geometry2i.distance(edge, obstacle);
121
                    if (minDist < separations[j])
121
                    if (minDist < radiuses[j] + bodyRadius)
122 122
                        edgeSIBuilder.markCollision(i, time, edge);
123 123
                }
124 124
            }
......
129 129
        return edgeSIBuilder;
130 130
    }
131 131

  
132
    public static <V extends Point> SafeIntervalBuilder<V> safeIntervalsForPoints(Set<V> vertices, DynamicEnvironment environment, int step) {
132
    public static <V extends Point> SafeIntervalBuilder<V> safeIntervalsForPoints(Set<V> vertices, DynamicEnvironment environment, int bodyRadius, int step) {
133 133
        SafeIntervalBuilder<V> pointSIBuilder = new SafeIntervalBuilder<V>();
134 134
        KdTree<V> kdTree = fillKDTree(vertices);
135 135
        int N = kdTree.size();
136 136

  
137 137
        Trajectory[] obstacles = environment.getObstacles();
138
        int[] separations = environment.getSeparations();
138
        int[] radiuses = environment.getObstacleRadiuses();
139 139

  
140 140
        for (int i = 0; ; i++) {
141 141
            int time = i * step;
......
154 154

  
155 155
                while (iterator.hasNext()) {
156 156
                    V v = iterator.next();
157
                    if (point.distance(v) < separations[a]) {
157
                    if (point.distance(v) < radiuses[a] + bodyRadius) {
158 158
                        nodes.add(v);
159 159
                    } else {
160 160
                        break;
src/main/java/tt/jointeuclid2ni/sipp/SippWrapper.java
21 21
    private SafeIntervalBuilder<E> edgeSIBuilder;
22 22
    private SafeIntervalBuilder<V> pointSIBuilder;
23 23

  
24
    private int bodyRadius;
24 25
    private int speed;
25 26
    private int step;
26 27

  
27 28

  
28
    public SippWrapper(DirectedGraph<V, E> graph, DynamicEnvironment environment, int speed, int samplingStep) {
29
    public SippWrapper(DirectedGraph<V, E> graph, DynamicEnvironment environment, int bodyRadius, int speed, int samplingStep) {
29 30
        this.graph = graph;
30 31
        this.env = environment;
32
        this.bodyRadius = bodyRadius;
31 33
        this.speed = speed;
32 34
        this.step = samplingStep;
33 35

  
......
35 37
    }
36 38

  
37 39
    protected void initIntervals() {
38
        pointSIBuilder = SippUtils.safeIntervalsForPoints(graph.vertexSet(), env, step);
39
        edgeSIBuilder = SippUtils.safeIntervalsForEdges(graph.edgeSet(), env, step);
40
        pointSIBuilder = SippUtils.safeIntervalsForPoints(graph.vertexSet(), env, bodyRadius, step);
41
        edgeSIBuilder = SippUtils.safeIntervalsForEdges(graph.edgeSet(), env, bodyRadius, step);
40 42
    }
41 43

  
42 44
    @Override
......
87 89
                    SippNode<V> end = new SippNode<V>(successor, childSI, interval.end);
88 90
                    SippEdge<V> sippEdge = new SippEdge<V>(node, end, straights);
89 91

  
90
                    if (!SeparationDetector.hasAnyPairwiseConflict(sippEdge.trajectory(), env.getObstacles(), env.getSeparations(), step)) {
92
                    int[] separations = SippUtils.radiusesToSeparations(bodyRadius, env.getObstacleRadiuses());
93

  
94
                    if (!SeparationDetector.hasAnyPairwiseConflict(sippEdge.trajectory(), env.getObstacles(), separations, step)) {
91 95
                        //System.out.printf("      SUCCESSFULLY ADDED %n");
92 96
                        outgoingEdges.add(sippEdge);
93 97
                    } else {
src/main/java/tt/jointeuclid2ni/sipprrts/DynamicEnvironment.java
9 9

  
10 10
    public Trajectory[] getObstacles();
11 11

  
12
    public int[] getSeparations();
12
    public int[] getObstacleRadiuses();
13 13

  
14 14
    public int getMaxTime();
15 15
}
src/main/java/tt/jointeuclid2ni/sipprrts/DynamicEnvironmentImpl.java
8 8
public class DynamicEnvironmentImpl implements DynamicEnvironment {
9 9

  
10 10
    private Trajectory[] obstacles;
11
    private int[] separations;
11
    private int[] radiuses;
12 12
    private int maxTime;
13 13

  
14
    public DynamicEnvironmentImpl(Trajectory[] obstacles, int[] separations, int maxTime) {
14
    public DynamicEnvironmentImpl(Trajectory[] obstacles, int[] radiuses, int maxTime) {
15 15
        this.obstacles = obstacles;
16
        this.separations = separations;
16
        this.radiuses = radiuses;
17 17
        this.maxTime = maxTime;
18 18
    }
19 19

  
......
23 23
    }
24 24

  
25 25
    @Override
26
    public int[] getSeparations() {
27
        return separations;
26
    public int[] getObstacleRadiuses() {
27
        return radiuses;
28 28
    }
29 29

  
30 30
    @Override
src/main/java/tt/jointeuclid2ni/sipprrts/SippRRTDemo.java
320 320
        GraphPath<Point, Line> path = AStarShortestPathSimple.findPathBetween(grid, new ZeroHeuristic<Point>(), problem.getStart(0), problem.getTarget(0));
321 321
        BasicSegmentedTrajectory trajectory = SegmentedTrajectoryFactory.createConstantSpeedTrajectory(path, 0, SPEED, MAX_TIME, path.getWeight());
322 322

  
323
        return new DynamicEnvironmentImpl(new Trajectory[]{trajectory}, new int[]{2 * BODY_RADIUS}, MAX_TIME);
323
        return new DynamicEnvironmentImpl(new Trajectory[]{trajectory}, new int[]{BODY_RADIUS}, MAX_TIME);
324 324
    }
325 325

  
326 326
    private static SippRRTMission secondAgentAsMissionToSolve(EarliestArrivalProblem problem) {
327
        return new SippRRTMissionImpl(problem.getStart(1), problem.getTarget(1), BODY_RADIUS, SPEED);
327
        return new SippRRTMissionImpl(problem.getStart(1), problem.getTarget(1), BODY_RADIUS, BODY_RADIUS, SPEED);
328 328
    }
329 329
}
src/main/java/tt/jointeuclid2ni/sipprrts/SippRRTDomain.java
2 2

  
3 3
import tt.euclid2i.Line;
4 4
import tt.euclid2i.Point;
5
import tt.euclid2i.Region;
5 6
import tt.euclid2i.probleminstance.Environment;
6 7
import tt.euclid2i.util.SeparationDetector;
7 8
import tt.euclid2i.util.Util;
......
13 14
import tt.planner.rrtstar.util.Extension;
14 15
import tt.planner.rrtstar.util.ExtensionEstimate;
15 16

  
17
import java.util.Collection;
16 18
import java.util.List;
17 19
import java.util.Random;
18 20

  
......
21 23
 */
22 24
public class SippRRTDomain implements Domain<SippRRTNode, SippRRTEdge> {
23 25

  
24
    private final Environment statEnv;
26
    private final Region boundary;
27
    private final Collection<Region> inflatedObstacles;
25 28
    private final DynamicEnvironment dynEnv;
29

  
26 30
    private final SippRRTMission mission;
27 31

  
28 32
    private Random random;
29 33
    private final int step;
30 34

  
31 35
    public SippRRTDomain(Environment statEnv, DynamicEnvironment dynEnv, SippRRTMission mission, int samplingStep, int seed) {
32
        this.statEnv = statEnv;
36
        this.inflatedObstacles = Util.inflateRegions(statEnv.getObstacles(), mission.getBodyRadius());
37
        this.boundary = statEnv.getBoundary();
33 38
        this.dynEnv = dynEnv;
34 39
        this.mission = mission;
35 40
        this.step = samplingStep;
......
39 44
    @Override
40 45
    public SippRRTNode sampleState() {
41 46
        Point point = sampleSpace();
42
        SafeIntervalList intervals = SippUtils.safeIntervalsForSinglePoint(point, dynEnv, step);
47
        SafeIntervalList intervals = SippUtils.safeIntervalsForSinglePoint(point, dynEnv, 0, step);
43 48
        int sampledInterval = random.nextInt(intervals.size());
44 49

  
45 50
        return new SippRRTNode(point, intervals, sampledInterval);
......
48 53
    public Point sampleSpace() {
49 54
        if (random.nextDouble() > 0.02)
50 55
            //TODO remove hack .getBoundingBox...
51
            return Util.sampleFreeSpace(statEnv.getBoundary().getBoundingBox(), statEnv.getObstacles(), random);
56
            return Util.sampleFreeSpace(boundary.getBoundingBox(), inflatedObstacles, random);
52 57
        else
53 58
            return mission.getTarget();
54 59
    }
55 60

  
56 61
    public SippRRTNode createRoot(Point point) {
57
        SafeIntervalList intervals = SippUtils.safeIntervalsForSinglePoint(point, dynEnv, step);
62
        SafeIntervalList intervals = SippUtils.safeIntervalsForSinglePoint(point, dynEnv, mission.getBodyRadius(), step);
58 63

  
59 64
        if (intervals.isEmpty())
60 65
            throw new IllegalArgumentException("No safe interval in the root point");
......
64 69

  
65 70
    @Override
66 71
    public Extension<SippRRTNode, SippRRTEdge> extendTo(SippRRTNode from, SippRRTNode to) {
67
        SippRRTEdge edge = getSIPPEdge(from, to, true);
72
        SippRRTEdge edge = createExtension(from, to, true);
68 73

  
69 74
        if (edge == null)
70 75
            return null;
......
74 79

  
75 80
    @Override
76 81
    public ExtensionEstimate estimateExtension(SippRRTNode from, SippRRTNode to) {
77
        SippRRTEdge edge = getSIPPEdge(from, to, false);
82
        SippRRTEdge edge = createExtension(from, to, false);
78 83

  
79 84
        if (edge == null)
80 85
            return null;
......
82 87
        return new ExtensionEstimate(edge.getDuration(), true);
83 88
    }
84 89

  
85
    public SippRRTEdge getSIPPEdge(SippRRTNode from, SippRRTNode to, boolean updatetime) {
90
    public SippRRTEdge createExtension(SippRRTNode from, SippRRTNode to, boolean updatetime) {
86 91
        Point point1 = from.getPoint();
87 92
        Point point2 = to.getPoint();
88 93
        Line line = new Line(point1, point2);
89 94

  
90
        SafeIntervalList edgeIntervals = SippUtils.safeIntervalsForSingleEdge(line, dynEnv, step);
95
        SafeIntervalList edgeIntervals = SippUtils.safeIntervalsForSingleEdge(line, dynEnv, mission.getBodyRadius(), step);
91 96
        for (Interval edgeInterval : edgeIntervals) {
92 97
            Interval interval = SippUtils.safeTransitionInterval(from.getSafeInterval(), edgeInterval,
93 98
                    to.getSafeInterval(), from.getTime(), duration(from, to));
......
113 118

  
114 119
        SippRRTEdge edge = new SippRRTEdge(from, to, straights, duration);
115 120

  
116
        if (!SeparationDetector.hasAnyPairwiseConflict(edge.trajectory(), dynEnv.getObstacles(), dynEnv.getSeparations(), step)) {
121
        int[] separations = SippUtils.radiusesToSeparations(mission.getBodyRadius(), dynEnv.getObstacleRadiuses());
122

  
123
        if (!SeparationDetector.hasAnyPairwiseConflict(edge.trajectory(), dynEnv.getObstacles(), separations, step)
124
                && Util.isVisible(source, target, inflatedObstacles)) {
117 125
            if (updatetime)
118 126
                to.setTime(endTime);
119 127
            return edge;
src/main/java/tt/jointeuclid2ni/sipprrts/SippRRTMission.java
11 11

  
12 12
    Point getTarget();
13 13

  
14
    double getTargetRadius();
14
    int getBodyRadius();
15 15

  
16
    double getSpeed();
16
    int getTargetRadius();
17

  
18
    int getSpeed();
17 19
}
src/main/java/tt/jointeuclid2ni/sipprrts/SippRRTMissionImpl.java
9 9

  
10 10
    private Point start;
11 11
    private Point target;
12
    private double radius;
13
    private double speed;
12
    private int bodyRadius;
13
    private int targetRadius;
14
    private int speed;
14 15

  
15
    public SippRRTMissionImpl(Point start, Point target, double radius, double speed) {
16
    public SippRRTMissionImpl(Point start, Point target, int bodyRadius, int targetRadius, int speed) {
16 17
        this.start = start;
17 18
        this.target = target;
18
        this.radius = radius;
19
        this.bodyRadius = bodyRadius;
20
        this.targetRadius = targetRadius;
19 21
        this.speed = speed;
20 22
    }
21 23

  
......
30 32
    }
31 33

  
32 34
    @Override
33
    public double getTargetRadius() {
34
        return radius;
35
    public int getBodyRadius() {
36
        return bodyRadius;
35 37
    }
36 38

  
37 39
    @Override
38
    public double getSpeed() {
40
    public int getTargetRadius() {
41
        return targetRadius;
42
    }
43

  
44
    @Override
45
    public int getSpeed() {
39 46
        return speed;
40 47
    }
41 48
}
src/main/java/tt/jointeuclid2ni/solver/impl/AlgorithmPPSIPP.java
41 41
            Trajectory[] trajArr = trajectories.toArray(new Trajectory[trajectories.size()]);
42 42
            int[] radArr = new int[bodyRadiuses.size()];
43 43
            for (int j = 0; j < radArr.length; j++) {
44
                radArr[j] = bodyRadiuses.get(j) + problem.getBodyRadius(i);
44
                radArr[j] = bodyRadiuses.get(j);
45 45
            }
46 46

  
47 47
            DynamicEnvironmentImpl environment = new DynamicEnvironmentImpl(trajArr, radArr, params.maxTime);
48 48

  
49
            SippWrapper<Point, Line> wrapper = new SippWrapper<Point, Line>(graph[i], environment, problem.getMaxSpeed(i), 2);
49
            SippWrapper<Point, Line> wrapper = new SippWrapper<Point, Line>(graph[i], environment, problem.getBodyRadius(i), problem.getMaxSpeed(i), 2);
50 50
            SippNode<Point> start = new SippNode<Point>(problem.getStart(i), Interval.toInfinity(0), 0);
51 51
            SippHeuristic<Point> heuristic = new SippHeuristic<Point>(getHeuristic(graph[i], problem.getTarget(i)));
52 52
            SippGoal<Point> goal = new SippGoal<Point>(problem.getTarget(i));
src/test/java/tt/jointeuclid2ni/sipp/SippWrapperTest.java
155 155
    }
156 156

  
157 157
    private EvaluatedTrajectory solveBySIPP(EarliestArrivalProblem problem, Trajectory firstTrajectory, DirectedGraph<Point, Line> graph) {
158
        DynamicEnvironment environment = new DynamicEnvironmentImpl(new Trajectory[]{firstTrajectory}, new int[]{2 * BODY_RADIUS}, MAX_TIME);
158
        DynamicEnvironment environment = new DynamicEnvironmentImpl(new Trajectory[]{firstTrajectory}, new int[]{BODY_RADIUS}, MAX_TIME);
159 159

  
160
        SippWrapper<Point, Line> wrapper = new SippWrapper<Point, Line>(graph, environment, problem.getMaxSpeed(1), 2);
160
        SippWrapper<Point, Line> wrapper = new SippWrapper<Point, Line>(graph, environment, problem.getBodyRadius(1), problem.getMaxSpeed(1), 2);
161 161
        SippGoal<Point> goal = new SippGoal<Point>(problem.getTarget(1));
162 162
        SippNode<Point> start = new SippNode<Point>(problem.getStart(1), Interval.toInfinity(0), 0);
163 163
        HeuristicToGoal<SippNode<Point>> heuristic = new SippHeuristic<Point>(new tt.euclid2i.discretization.L1Heuristic(problem.getTarget(1)));

Also available in: Unified diff