1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.function.Supplier;
4
5 import org.djunits.value.vdouble.scalar.Length;
6 import org.djunits.value.vdouble.scalar.Speed;
7 import org.opentrafficsim.core.gtu.GTU;
8 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
9 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable.Intermediate;
10 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable.PerceptionAccumulator;
11 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable.PerceptionCollector;
12 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable.PerceptionFinalizer;
13 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
14 import org.opentrafficsim.road.gtu.lane.perception.categories.AnticipationSpeed.SpeedSet;
15
16
17
18
19
20
21
22
23
24
25
26
27
28 public class AnticipationSpeed implements PerceptionCollector<SpeedSet, LaneBasedGTU, SpeedSet>
29 {
30
31
32 private double desiredSpeed;
33
34
35 private double x0;
36
37
38 private RelativeLane lane;
39
40
41
42
43
44
45
46 public AnticipationSpeed(final Speed desiredSpeed, final Length lookAhead, final RelativeLane lane)
47 {
48 this.desiredSpeed = desiredSpeed.si;
49 this.x0 = lookAhead.si;
50 this.lane = lane;
51 }
52
53
54 @Override
55 public Supplier<SpeedSet> getIdentity()
56 {
57 return new Supplier<SpeedSet>()
58 {
59 @SuppressWarnings("synthetic-access")
60 @Override
61 public SpeedSet get()
62 {
63 SpeedSet identity = new SpeedSet();
64 identity.left = AnticipationSpeed.this.desiredSpeed;
65 identity.current = AnticipationSpeed.this.desiredSpeed;
66 identity.right = AnticipationSpeed.this.desiredSpeed;
67 return identity;
68 }
69 };
70 }
71
72
73 @Override
74 public PerceptionAccumulator<LaneBasedGTU, SpeedSet> getAccumulator()
75 {
76 return new PerceptionAccumulator<LaneBasedGTU, SpeedSet>()
77 {
78 @SuppressWarnings("synthetic-access")
79 @Override
80 public Intermediate<SpeedSet> accumulate(final Intermediate<SpeedSet> intermediate, final LaneBasedGTU object,
81 final Length distance)
82 {
83 double v = anticipateSingle(object, distance);
84 if (AnticipationSpeed.this.lane.getNumLanes() < 2)
85 {
86 intermediate.getObject().current =
87 intermediate.getObject().current < v ? intermediate.getObject().current : v;
88 }
89 if (!AnticipationSpeed.this.lane.isCurrent())
90 {
91 if (AnticipationSpeed.this.lane.isRight())
92 {
93 if (object.getTurnIndicatorStatus().isLeft())
94 {
95 intermediate.getObject().left =
96 intermediate.getObject().left < v ? intermediate.getObject().left : v;
97 }
98 }
99 else
100 {
101 if (object.getTurnIndicatorStatus().isRight())
102 {
103 intermediate.getObject().right =
104 intermediate.getObject().right < v ? intermediate.getObject().right : v;
105 }
106 }
107 }
108 return intermediate;
109 }
110 };
111
112 }
113
114
115
116
117
118
119
120 final double anticipateSingle(final GTU gtu, final Length distance)
121 {
122 Speed speed = gtu.getSpeed();
123 double v = speed == null ? 0.0 : speed.si;
124 if (v > this.desiredSpeed || distance.si > this.x0)
125 {
126 return this.desiredSpeed;
127 }
128 double f = distance.si / this.x0;
129 f = f < 0.0 ? 0.0 : f > 1.0 ? 1.0 : f;
130 return (1 - f) * v + f * this.desiredSpeed;
131 }
132
133
134 @Override
135 public PerceptionFinalizer<SpeedSet, SpeedSet> getFinalizer()
136 {
137 return new PerceptionFinalizer<SpeedSet, SpeedSet>()
138 {
139 @Override
140 public SpeedSet collect(final SpeedSet intermediate)
141 {
142 return intermediate;
143 }
144 };
145 }
146
147
148
149
150
151
152
153
154
155
156
157
158
159 public static class SpeedSet
160 {
161
162 private double left = Double.POSITIVE_INFINITY;
163
164
165 private double current = Double.POSITIVE_INFINITY;
166
167
168 private double right = Double.POSITIVE_INFINITY;
169
170
171
172
173
174 public final Speed getLeft()
175 {
176 return Speed.createSI(this.left);
177 }
178
179
180
181
182
183 public final Speed getCurrent()
184 {
185 return Speed.createSI(this.current);
186 }
187
188
189
190
191
192 public final Speed getRight()
193 {
194 return Speed.createSI(this.right);
195 }
196 }
197
198 }