1 package org.opentrafficsim.road.gtu.lane.tactical.following;
2
3 import java.util.Collection;
4
5 import org.djunits.unit.AccelerationUnit;
6 import org.djunits.unit.LengthUnit;
7 import org.djunits.unit.TimeUnit;
8 import org.djunits.value.vdouble.scalar.Acceleration;
9 import org.djunits.value.vdouble.scalar.Duration;
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.djunits.value.vdouble.scalar.Speed;
12 import org.djunits.value.vdouble.scalar.Time;
13 import org.opentrafficsim.core.gtu.GTUException;
14 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
15 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
16 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
17 import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32 public abstract class AbstractGTUFollowingModelMobil implements GTUFollowingModelOld
33 {
34
35
36 private static final AccelerationStep PROHIBITIVEACCELERATIONSTEP = new AccelerationStep(new Acceleration(
37 Double.NEGATIVE_INFINITY, AccelerationUnit.SI), new Time(Double.NaN, TimeUnit.SI), new Duration(Double.NaN,
38 TimeUnit.SI));
39
40
41 public static final DualAccelerationStep TOODANGEROUS = new DualAccelerationStep(PROHIBITIVEACCELERATIONSTEP,
42 PROHIBITIVEACCELERATIONSTEP);
43
44
45 @Override
46 public final DualAccelerationStep computeDualAccelerationStep(final LaneBasedGTU referenceGTU,
47 final Collection<Headway> otherGTUs, final Length maxDistance, final Speed speedLimit) throws GTUException
48 {
49 return computeDualAccelerationStep(referenceGTU, otherGTUs, maxDistance, speedLimit, getStepSize());
50 }
51
52
53 @Override
54 public final DualAccelerationStep computeDualAccelerationStep(final LaneBasedGTU referenceGTU,
55 final Collection<Headway> otherHeadways, final Length maxDistance, final Speed speedLimit,
56 final Duration stepSize) throws GTUException
57 {
58
59 for (Headway headway : otherHeadways)
60 {
61
62 if (headway.getDistance() == null)
63 {
64 return TOODANGEROUS;
65 }
66 if (!headway.getId().equals(referenceGTU.getId()) && Double.isNaN(headway.getDistance().si))
67 {
68 return TOODANGEROUS;
69 }
70 }
71 AccelerationStep followerAccelerationStep = null;
72 AccelerationStep referenceGTUAccelerationStep = null;
73 TacticalPlanner tp = referenceGTU.getTacticalPlanner();
74 if (null == tp)
75 {
76 referenceGTU.getTacticalPlanner();
77 System.err.println ("tactical planner is null");
78 }
79 GTUFollowingModelOld gfm =
80 (GTUFollowingModelOld) ((AbstractLaneBasedTacticalPlanner) referenceGTU.getTacticalPlanner())
81 .getCarFollowingModel();
82
83 for (Headway headway : otherHeadways)
84 {
85 if (headway.getId().equals(referenceGTU.getId()))
86 {
87 continue;
88 }
89 if (headway.getDistance().si < 0)
90 {
91
92 AccelerationStep as =
93 gfm.computeAccelerationStep(headway.getSpeed(), referenceGTU.getSpeed(), new Length(
94 -headway.getDistance().si, LengthUnit.SI), speedLimit, referenceGTU.getSimulator()
95 .getSimulatorTime().getTime(), stepSize);
96 if (null == followerAccelerationStep || as.getAcceleration().lt(followerAccelerationStep.getAcceleration()))
97 {
98 followerAccelerationStep = as;
99 }
100 }
101 else
102 {
103
104 AccelerationStep as =
105 gfm.computeAccelerationStep(referenceGTU, headway.getSpeed(), headway.getDistance(), maxDistance,
106 speedLimit, stepSize);
107 if (null == referenceGTUAccelerationStep
108 || as.getAcceleration().lt(referenceGTUAccelerationStep.getAcceleration()))
109 {
110 referenceGTUAccelerationStep = as;
111 }
112 }
113
114
115
116
117 }
118 if (null == followerAccelerationStep)
119 {
120 followerAccelerationStep = gfm.computeAccelerationStepWithNoLeader(referenceGTU, maxDistance, speedLimit, stepSize);
121 }
122 if (null == referenceGTUAccelerationStep)
123 {
124 referenceGTUAccelerationStep =
125 gfm.computeAccelerationStepWithNoLeader(referenceGTU, maxDistance, speedLimit, stepSize);
126 }
127 return new DualAccelerationStep(referenceGTUAccelerationStep, followerAccelerationStep);
128 }
129
130
131 @Override
132 public final AccelerationStep computeAccelerationStep(final LaneBasedGTU gtu, final Speed leaderSpeed,
133 final Length headway, final Length maxDistance, final Speed speedLimit) throws GTUException
134 {
135 return computeAccelerationStep(gtu, leaderSpeed, headway, maxDistance, speedLimit, getStepSize());
136 }
137
138
139 @Override
140 public final AccelerationStep computeAccelerationStep(final LaneBasedGTU gtu, final Speed leaderSpeed,
141 final Length headway, final Length maxDistance, final Speed speedLimit, final Duration stepSize)
142 throws GTUException
143 {
144 Length distance;
145 Speed leaderOrBlockSpeed;
146 if (maxDistance.lt(headway) || headway == null)
147 {
148 distance = maxDistance;
149 leaderOrBlockSpeed = Speed.ZERO;
150 }
151 else
152 {
153 distance = headway;
154 leaderOrBlockSpeed = leaderSpeed;
155 }
156 final Speed followerSpeed = gtu.getSpeed();
157 final Speed followerMaximumSpeed = gtu.getMaximumSpeed();
158 Acceleration newAcceleration =
159 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderOrBlockSpeed, distance, speedLimit, stepSize);
160 Time nextEvaluationTime = gtu.getSimulator().getSimulatorTime().getTime().plus(stepSize);
161 return new AccelerationStep(newAcceleration, nextEvaluationTime, stepSize);
162 }
163
164
165 @Override
166 public final AccelerationStep computeAccelerationStep(final Speed followerSpeed, final Speed leaderSpeed,
167 final Length headway, final Speed speedLimit, final Time currentTime)
168 {
169 return computeAccelerationStep(followerSpeed, leaderSpeed, headway, speedLimit, currentTime, getStepSize());
170 }
171
172
173 @Override
174 public final AccelerationStep computeAccelerationStep(final Speed followerSpeed, final Speed leaderSpeed,
175 final Length headway, final Speed speedLimit, final Time currentTime, final Duration stepSize)
176 {
177 final Speed followerMaximumSpeed = speedLimit;
178 Acceleration newAcceleration =
179 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed, headway, speedLimit, stepSize);
180 Time nextEvaluationTime = currentTime.plus(stepSize);
181 return new AccelerationStep(newAcceleration, nextEvaluationTime, stepSize);
182 }
183
184
185 @Override
186 public final AccelerationStep computeAccelerationStepWithNoLeader(final LaneBasedGTU gtu, final Length maxDistance,
187 final Speed speedLimit) throws GTUException
188 {
189 return computeAccelerationStepWithNoLeader(gtu, maxDistance, speedLimit, getStepSize());
190 }
191
192
193 @Override
194 public final AccelerationStep computeAccelerationStepWithNoLeader(final LaneBasedGTU gtu, final Length maxDistance,
195 final Speed speedLimit, final Duration stepSize) throws GTUException
196 {
197 Length stopDistance =
198 new Length(gtu.getMaximumSpeed().si * gtu.getMaximumSpeed().si
199 / (2.0 * getMaximumSafeDeceleration().si), LengthUnit.SI);
200 return computeAccelerationStep(gtu, gtu.getSpeed(), stopDistance, maxDistance, speedLimit, stepSize);
201
202
203
204
205 }
206
207
208 @Override
209 public final Length minimumHeadway(final Speed followerSpeed, final Speed leaderSpeed, final Length precision,
210 final Length maxDistance, final Speed speedLimit, final Speed followerMaximumSpeed)
211 {
212 if (precision.getSI() <= 0)
213 {
214 throw new Error("Precision has bad value (must be > 0; got " + precision + ")");
215 }
216 double maximumDeceleration = -getMaximumSafeDeceleration().getSI();
217
218 double minimumSI = 0;
219 double minimumSIDeceleration =
220 computeAcceleration(followerSpeed, followerSpeed, leaderSpeed, new Length(minimumSI, LengthUnit.SI),
221 speedLimit).getSI();
222 if (minimumSIDeceleration >= maximumDeceleration)
223 {
224
225 return Length.ZERO;
226 }
227 double maximumSI = 1;
228 double maximumSIDeceleration = Double.NaN;
229
230 for (int step = 0; step < 20; step++)
231 {
232 maximumSIDeceleration =
233 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed,
234 new Length(maximumSI, LengthUnit.SI), speedLimit).getSI();
235 if (maximumSIDeceleration > maximumDeceleration)
236 {
237 break;
238 }
239 maximumSI *= 2;
240 }
241 if (maximumSIDeceleration < maximumDeceleration)
242 {
243 System.out.println();
244 maximumSIDeceleration =
245 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed,
246 new Length(maximumSI, LengthUnit.SI), speedLimit).getSI();
247 throw new Error("Cannot find headway that results in an acceptable deceleration");
248 }
249
250 final int maximumStep = (int) Math.ceil(Math.log((maximumSI - minimumSI) / precision.getSI()) / Math.log(2));
251 for (int step = 0; step < maximumStep; step++)
252 {
253 double midSI = (minimumSI + maximumSI) / 2;
254 double midSIAcceleration =
255 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed, new Length(midSI, LengthUnit.SI),
256 speedLimit).getSI();
257 if (midSIAcceleration < maximumDeceleration)
258 {
259 minimumSI = midSI;
260 }
261 else
262 {
263 maximumSI = midSI;
264 }
265 }
266 Length result = new Length(Math.min((minimumSI + maximumSI) / 2, maxDistance.si), LengthUnit.SI);
267 return result;
268 }
269 }