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