Revision 74:bd3e25d12ca5

View differences:

src/main/java/tt/jointeuclid2ni/sipp/Interval.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
public class Interval {
4

  
5
    public static Interval zeroToInfinity() {
6
        return fromValueToInfinity(0);
7
    }
8

  
9
    public static Interval fromValueToInfinity(int val) {
10
        return new Interval(val, Integer.MAX_VALUE);
11
    }
12

  
13
    public int start;
14
    public int end;
15

  
16
    public Interval(int start, int end) {
17
        this.start = start;
18
        this.end = end;
19
    }
20

  
21
    @Override
22
    public String toString() {
23
        return String.format("Interval{%d,%d}", start, end);
24
    }
25

  
26
    @Override
27
    public boolean equals(Object o) {
28
        if (this == o) return true;
29
        if (o == null || getClass() != o.getClass()) return false;
30

  
31
        Interval interval = (Interval) o;
32
        return end == interval.end
33
                && start == interval.start;
34
    }
35

  
36
    @Override
37
    public int hashCode() {
38
        return 31 * start + end;
39
    }
40
}
41

  
src/main/java/tt/jointeuclid2ni/sipp/SIPPEdge.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
import tt.euclid2i.Point;
4
import tt.euclid2i.Trajectory;
5
import tt.euclid2i.trajectory.BasicSegmentedTrajectory;
6
import tt.euclidtime3i.discretization.Straight;
7

  
8
import java.util.List;
9

  
10
public class SIPPEdge<V extends Point> {
11

  
12
    private SIPPNode<V> start, end;
13
    private List<Straight> lines;
14

  
15
    public SIPPEdge(SIPPNode<V> start, SIPPNode<V> end, List<Straight> lines) {
16
        this.start = start;
17
        this.end = end;
18
        this.lines = lines;
19
    }
20

  
21
    public SIPPNode getStart() {
22
        return start;
23
    }
24

  
25
    public SIPPNode getEnd() {
26
        return end;
27
    }
28

  
29
    public List<Straight> getLines() {
30
        return lines;
31
    }
32

  
33
    public int weight() {
34
        int weight = 0;
35
        for (Straight line : lines) {
36
            weight += line.duration();
37
        }
38
        return weight;
39
    }
40

  
41
    @Override
42
    public boolean equals(Object o) {
43
        if (this == o) return true;
44
        if (o == null || getClass() != o.getClass()) return false;
45

  
46
        SIPPEdge sippEdge = (SIPPEdge) o;
47

  
48
        if (end != null ? !end.equals(sippEdge.end) : sippEdge.end != null) return false;
49
        if (lines != null ? !lines.equals(sippEdge.lines) : sippEdge.lines != null) return false;
50
        if (start != null ? !start.equals(sippEdge.start) : sippEdge.start != null) return false;
51

  
52
        return true;
53
    }
54

  
55
    @Override
56
    public int hashCode() {
57
        int result = start != null ? start.hashCode() : 0;
58
        result = 31 * result + (end != null ? end.hashCode() : 0);
59
        result = 31 * result + (lines != null ? lines.hashCode() : 0);
60
        return result;
61
    }
62

  
63
    public Trajectory trajectory() {
64
        int duration = weight();
65
        return new BasicSegmentedTrajectory(lines, duration, duration);
66
    }
67
}
src/main/java/tt/jointeuclid2ni/sipp/SIPPNode.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
import tt.euclid2i.Point;
4

  
5
public class SIPPNode<V extends Point> {
6

  
7
    private V point;
8
    private Interval safeInterval;
9
    private int time;
10

  
11
    public SIPPNode(V point, Interval safeInterval, int time) {
12
        this.point = point;
13
        this.safeInterval = safeInterval;
14
        this.time = time;
15
    }
16

  
17
    public V getPoint() {
18
        return point;
19
    }
20

  
21
    public int getTime() {
22
        return time;
23
    }
24

  
25
    public Interval getSafeInterval() {
26
        return safeInterval;
27
    }
28

  
29
    @Override
30
    public boolean equals(Object o) {
31
        if (this == o) return true;
32
        if (o == null || getClass() != o.getClass()) return false;
33

  
34
        SIPPNode sippNode = (SIPPNode) o;
35

  
36
        if (time != sippNode.time) return false;
37
        if (!point.equals(sippNode.point)) return false;
38
        if (!safeInterval.equals(sippNode.safeInterval)) return false;
39

  
40
        return true;
41
    }
42

  
43
    @Override
44
    public int hashCode() {
45
        int result = point.hashCode();
46
        result = 31 * result + safeInterval.hashCode();
47
        result = 31 * result + time;
48
        return result;
49
    }
50
}
src/main/java/tt/jointeuclid2ni/sipp/SIPPWrapper.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
import ags.utils.dataStructures.KdTree;
4
import ags.utils.dataStructures.NearestNeighborIterator;
5
import ags.utils.dataStructures.SquareEuclideanDistanceFunction;
6
import org.jgrapht.Graph;
7
import org.jgrapht.alg.specifics.Specifics;
8
import org.jgrapht.alg.specifics.SpecificsFactory;
9
import org.jgrapht.graph.AbstractDirectedGraphWrapper;
10
import tt.euclid2i.Point;
11
import tt.euclid2i.Trajectory;
12
import tt.euclid2i.util.SeparationDetector;
13
import tt.euclidtime3i.discretization.Straight;
14

  
15
import java.util.*;
16

  
17
public class SIPPWrapper<V extends Point, E> extends AbstractDirectedGraphWrapper<SIPPNode<V>, SIPPEdge<V>> {
18

  
19
    private Map<V, SafeIntervals> safeIntervalMap;
20
    private Trajectory[] trajectories;
21
    private int[] separations;
22
    private Graph<V, E> graph;
23
    private int speed;
24
    private Specifics<V, E> specifics;
25
    private int radius;
26
    private int step;
27

  
28
    public SIPPWrapper(Graph<V, E> graph, Trajectory[] trajectories, int[] radiuses, int speed, int radius, int step) {
29
        this.trajectories = trajectories;
30
        this.separations = radiusesToSeparations(radiuses, radius);
31
        this.graph = graph;
32
        this.speed = speed;
33
        this.specifics = SpecificsFactory.create(graph);
34
        this.radius = radius;
35
        this.step = step;
36
        this.safeIntervalMap = initIntervals();
37
    }
38

  
39
    @Override
40
    public double getEdgeWeight(SIPPEdge<V> edge) {
41
        return edge.weight();
42
    }
43

  
44
    @Override
45
    public Set<SIPPEdge<V>> outgoingEdgesOf(SIPPNode<V> node) {
46
        Set<SIPPEdge<V>> outgoingEdges = new HashSet<SIPPEdge<V>>();
47
        V point = node.getPoint();
48
        int time = node.getTime();
49

  
50
        Interval interval = node.getSafeInterval();
51

  
52
        Set<V> vertexSet = specifics.succesorVertexSet(point);
53
        for (V successor : vertexSet) {
54

  
55
            int duration = (int) (successor.distance(point) / speed);
56
            int start_t = time + duration;
57
            int end_t = time + duration;
58

  
59
            SafeIntervals succIntervals = safeIntervalMap.get(successor);
60

  
61
            for (Interval i : succIntervals) {
62
                if (i.start > end_t || i.end < start_t)
63
                    continue;
64

  
65
                int endTime;
66
                List<Straight> straights = new ArrayList<Straight>();
67
                if (i.start <= start_t) {
68
                    straights.add(new Straight(point, time, successor, start_t));
69
                    endTime = start_t;
70
                } else {
71
                    straights.add(new Straight(point, time, point, i.start - duration));
72
                    straights.add(new Straight(point, i.start - duration, successor, i.start));
73
                    endTime = i.start;
74
                }
75

  
76
                SIPPNode<V> end = new SIPPNode<V>(successor, i, endTime);
77
                SIPPEdge<V> edge = new SIPPEdge<V>(node, end, straights);
78

  
79
                if (!SeparationDetector.hasAnyPairwiseConflict(edge.trajectory(), trajectories, separations, step))
80
                    outgoingEdges.add(edge);
81
            }
82

  
83
        }
84

  
85
        return outgoingEdges;
86
    }
87

  
88
    private Map<V, SafeIntervals> initIntervals() {
89
        KdTree kdTree = fillKDTree();
90
        return findSafeIntervals(kdTree);
91
    }
92

  
93
    private KdTree<V> fillKDTree() {
94
        KdTree<V> kdTree = new KdTree<V>(2);
95

  
96
        Set<V> vertices = graph.vertexSet();
97
        for (V v : vertices) {
98
            kdTree.addPoint(v.toDoubleArray(), v);
99
        }
100

  
101
        return kdTree;
102
    }
103

  
104
    private Map<V, SafeIntervals> findSafeIntervals(KdTree<V> kdTree) {
105
        int N = kdTree.size();
106
        int maxTime = trajectoryMaxTime();
107

  
108

  
109
        Map<V, SafeIntervals> safeIntervalsMap = new HashMap<V, SafeIntervals>();
110
        for (int i = 0; ; i++) {
111
            int t = i * step;
112

  
113
            Set<V> nodes = new HashSet<V>();
114

  
115
            for (int a = 0; a < trajectories.length; a++) {
116
                if (trajectories[a].getMaxTime() < t)
117
                    continue;
118

  
119
                Point point = trajectories[a].get(t);
120
                double[] search = point.toDoubleArray();
121

  
122
                NearestNeighborIterator<V> iterator
123
                        = kdTree.getNearestNeighborIterator(search, N, new SquareEuclideanDistanceFunction());
124

  
125
                addNodesCloserThanRadius(point, iterator, nodes, separations[a]);
126
            }
127

  
128
            for (V node : nodes) {
129
                SafeIntervals safeIntervals = safeIntervalsMap.get(node);
130
                safeIntervals.removeSafePoint(i, t);
131
            }
132

  
133
            if (t > maxTime)
134
                break;
135
        }
136

  
137
        return safeIntervalsMap;
138
    }
139

  
140
    private int trajectoryMaxTime() {
141
        int maxTime = Integer.MAX_VALUE;
142
        for (Trajectory trajectory : trajectories) {
143
            int trajMaxTime = trajectory.getMaxTime();
144
            maxTime = maxTime < trajMaxTime ? trajMaxTime : maxTime;
145
        }
146
        return maxTime;
147
    }
148

  
149
    private void addNodesCloserThanRadius(Point point, Iterator<V> iterator, Collection<V> collection, int distance) {
150
        while (iterator.hasNext()) {
151
            V v = iterator.next();
152
            if (point.distance(v) <= distance)
153
                collection.add(v);
154
        }
155
    }
156

  
157
    private int[] radiusesToSeparations(int[] radiuses, int radius) {
158
        int[] separations = new int[radiuses.length];
159
        for (int i = 0; i < separations.length; i++) {
160
            separations[i] = radiuses[i] + radius;
161
        }
162
        return separations;
163
    }
164
}
src/main/java/tt/jointeuclid2ni/sipp/SafeIntervals.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
import java.util.Iterator;
4
import java.util.LinkedList;
5

  
6
public class SafeIntervals implements Iterable<Interval> {
7

  
8
    private int lastID;
9
    private LinkedList<Interval> safeIntervals;
10

  
11
    public SafeIntervals() {
12
        this.lastID = -1;
13
        this.safeIntervals = new LinkedList<Interval>();
14
        this.safeIntervals.add(Interval.zeroToInfinity());
15
    }
16

  
17
    public Interval safeInterval(int k) {
18
        return safeIntervals.get(k);
19
    }
20

  
21
    public int safeIntervals() {
22
        return safeIntervals.size();
23
    }
24

  
25
    public void removeSafePoint(int id, double time) {
26
        if (id == lastID + 1) {
27
            shortenLastSafeInterval(time);
28
        } else {
29
            appendNewSafeInterval(time);
30
        }
31

  
32
        lastID = id;
33
    }
34

  
35
    private void appendNewSafeInterval(double time) {
36
        Interval last = safeIntervals.getLast();
37
        last.end = time;
38

  
39
        // Marginal condition - in the beginning it might happen
40
        // that start == end == 0
41

  
42
        if (last.start == last.end)
43
            safeIntervals.pop();
44

  
45
        safeIntervals.add(Interval.fromValueToInfinity(time));
46
    }
47

  
48
    private void shortenLastSafeInterval(double time) {
49
        Interval last = safeIntervals.getLast();
50
        last.start = time;
51
    }
52

  
53
    @Override
54
    public Iterator<Interval> iterator() {
55
        return safeIntervals.iterator();
56
    }
57
}
src/test/java/tt/jointeuclid2ni/sipp/SafeIntervalsTest.java
1
package tt.jointeuclid2ni.sipp;
2

  
3
import org.junit.Test;
4

  
5
import java.util.Iterator;
6
import java.util.LinkedList;
7
import java.util.List;
8

  
9
import static org.junit.Assert.assertTrue;
10

  
11
public class SafeIntervalsTest {
12

  
13
    @Test
14
    public void testWithZero() throws Exception {
15
        SafeIntervals intervals = new SafeIntervals();
16
        List<Interval> expected = new LinkedList<Interval>();
17

  
18
        intervals.removeSafePoint(0, 0);
19
        intervals.removeSafePoint(1, 1);
20
        intervals.removeSafePoint(2, 2);
21
        intervals.removeSafePoint(5, 5);
22
        intervals.removeSafePoint(6, 6);
23
        intervals.removeSafePoint(9, 9);
24

  
25
        expected.add(new Interval(2, 5));
26
        expected.add(new Interval(6, 9));
27
        expected.add(new Interval(9, Integer.MAX_VALUE));
28

  
29
        checkEquals(intervals.iterator(), expected.iterator());
30
    }
31

  
32
    public void testWithoutZero() throws Exception {
33
        SafeIntervals intervals = new SafeIntervals();
34
        List<Interval> expected = new LinkedList<Interval>();
35

  
36
        intervals.removeSafePoint(1, 1);
37
        intervals.removeSafePoint(2, 2);
38
        intervals.removeSafePoint(5, 5);
39
        intervals.removeSafePoint(6, 6);
40
        intervals.removeSafePoint(9, 9);
41

  
42
        expected.add(new Interval(0, 1));
43
        expected.add(new Interval(2, 5));
44
        expected.add(new Interval(6, 9));
45
        expected.add(new Interval(9, Integer.MAX_VALUE));
46

  
47
        checkEquals(intervals.iterator(), expected.iterator());
48
    }
49

  
50
    private void checkEquals(Iterator<Interval> itA, Iterator<Interval> itB) {
51
        while (itA.hasNext() && itB.hasNext()) {
52
            Interval a = itA.next();
53
            Interval b = itB.next();
54

  
55
            System.out.printf("%s %s%n", a, b);
56
            assertTrue(a.equals(b));
57
        }
58

  
59
        assertTrue(itA.hasNext() == itB.hasNext());
60
    }
61
}

Also available in: Unified diff