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