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 =
37 new AccelerationStep(new Acceleration(Double.NEGATIVE_INFINITY, AccelerationUnit.SI),
38 new Time(Double.NaN, TimeUnit.SI), new Duration(Double.NaN, TimeUnit.SI));
39
40
41 public static final DualAccelerationStep TOODANGEROUS =
42 new DualAccelerationStep(PROHIBITIVEACCELERATIONSTEP, 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, final Duration stepSize)
56 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 = (GTUFollowingModelOld) ((AbstractLaneBasedTacticalPlanner) referenceGTU.getTacticalPlanner())
80 .getCarFollowingModel();
81
82 for (Headway headway : otherHeadways)
83 {
84 if (headway.getId().equals(referenceGTU.getId()))
85 {
86 continue;
87 }
88 if (headway.getDistance().si < 0)
89 {
90
91 AccelerationStep as = gfm.computeAccelerationStep(headway.getSpeed(), referenceGTU.getSpeed(),
92 new Length(-headway.getDistance().si, LengthUnit.SI), speedLimit,
93 referenceGTU.getSimulator().getSimulatorTime().getTime(), stepSize);
94 if (null == followerAccelerationStep || as.getAcceleration().lt(followerAccelerationStep.getAcceleration()))
95 {
96 followerAccelerationStep = as;
97 }
98 }
99 else
100 {
101
102 AccelerationStep as = gfm.computeAccelerationStep(referenceGTU, headway.getSpeed(), headway.getDistance(),
103 maxDistance, speedLimit, stepSize);
104 if (null == referenceGTUAccelerationStep
105 || as.getAcceleration().lt(referenceGTUAccelerationStep.getAcceleration()))
106 {
107 referenceGTUAccelerationStep = as;
108 }
109 }
110 }
111 if (null == followerAccelerationStep)
112 {
113 followerAccelerationStep = gfm.computeAccelerationStepWithNoLeader(referenceGTU, maxDistance, speedLimit, stepSize);
114 }
115 if (null == referenceGTUAccelerationStep)
116 {
117 referenceGTUAccelerationStep =
118 gfm.computeAccelerationStepWithNoLeader(referenceGTU, maxDistance, speedLimit, stepSize);
119 }
120 return new DualAccelerationStep(referenceGTUAccelerationStep, followerAccelerationStep);
121 }
122
123
124 @Override
125 public final AccelerationStep computeAccelerationStep(final LaneBasedGTU gtu, final Speed leaderSpeed, final Length headway,
126 final Length maxDistance, final Speed speedLimit) throws GTUException
127 {
128 return computeAccelerationStep(gtu, leaderSpeed, headway, maxDistance, speedLimit, getStepSize());
129 }
130
131
132 @Override
133 public final AccelerationStep computeAccelerationStep(final LaneBasedGTU gtu, final Speed leaderSpeed, final Length headway,
134 final Length maxDistance, final Speed speedLimit, final Duration stepSize) throws GTUException
135 {
136 Length distance;
137 Speed leaderOrBlockSpeed;
138 if (maxDistance.lt(headway) || headway == null)
139 {
140 distance = maxDistance;
141 leaderOrBlockSpeed = Speed.ZERO;
142 }
143 else
144 {
145 distance = headway;
146 leaderOrBlockSpeed = leaderSpeed;
147 }
148 final Speed followerSpeed = gtu.getSpeed();
149 final Speed followerMaximumSpeed = gtu.getMaximumSpeed();
150 Acceleration newAcceleration =
151 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderOrBlockSpeed, distance, speedLimit, stepSize);
152 Time nextEvaluationTime = gtu.getSimulator().getSimulatorTime().getTime().plus(stepSize);
153 return new AccelerationStep(newAcceleration, nextEvaluationTime, stepSize);
154 }
155
156
157 @Override
158 public final AccelerationStep computeAccelerationStep(final Speed followerSpeed, final Speed leaderSpeed,
159 final Length headway, final Speed speedLimit, final Time currentTime)
160 {
161 return computeAccelerationStep(followerSpeed, leaderSpeed, headway, speedLimit, currentTime, getStepSize());
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, final Duration stepSize)
168 {
169 final Speed followerMaximumSpeed = speedLimit;
170 Acceleration newAcceleration =
171 computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed, headway, speedLimit, stepSize);
172 Time nextEvaluationTime = currentTime.plus(stepSize);
173 return new AccelerationStep(newAcceleration, nextEvaluationTime, stepSize);
174 }
175
176
177 @Override
178 public final AccelerationStep computeAccelerationStepWithNoLeader(final LaneBasedGTU gtu, final Length maxDistance,
179 final Speed speedLimit) throws GTUException
180 {
181 return computeAccelerationStepWithNoLeader(gtu, maxDistance, speedLimit, getStepSize());
182 }
183
184
185 @Override
186 public final AccelerationStep computeAccelerationStepWithNoLeader(final LaneBasedGTU gtu, final Length maxDistance,
187 final Speed speedLimit, final Duration stepSize) throws GTUException
188 {
189 Length stopDistance = new Length(
190 gtu.getMaximumSpeed().si * gtu.getMaximumSpeed().si / (2.0 * getMaximumSafeDeceleration().si), LengthUnit.SI);
191 return computeAccelerationStep(gtu, gtu.getSpeed(), stopDistance, maxDistance, speedLimit, stepSize);
192
193
194
195
196 }
197
198
199 @Override
200 public final Length minimumHeadway(final Speed followerSpeed, final Speed leaderSpeed, final Length precision,
201 final Length maxDistance, final Speed speedLimit, final Speed followerMaximumSpeed)
202 {
203 if (precision.getSI() <= 0)
204 {
205 throw new Error("Precision has bad value (must be > 0; got " + precision + ")");
206 }
207 double maximumDeceleration = -getMaximumSafeDeceleration().getSI();
208
209 double minimumSI = 0;
210 double minimumSIDeceleration =
211 computeAcceleration(followerSpeed, followerSpeed, leaderSpeed, new Length(minimumSI, LengthUnit.SI), speedLimit)
212 .getSI();
213 if (minimumSIDeceleration >= maximumDeceleration)
214 {
215
216 return Length.ZERO;
217 }
218 double maximumSI = 1;
219 double maximumSIDeceleration = Double.NaN;
220
221 for (int step = 0; step < 20; step++)
222 {
223 maximumSIDeceleration = computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed,
224 new Length(maximumSI, LengthUnit.SI), speedLimit).getSI();
225 if (maximumSIDeceleration > maximumDeceleration)
226 {
227 break;
228 }
229 maximumSI *= 2;
230 }
231 if (maximumSIDeceleration < maximumDeceleration)
232 {
233 System.out.println();
234 maximumSIDeceleration = computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed,
235 new Length(maximumSI, LengthUnit.SI), speedLimit).getSI();
236 throw new Error("Cannot find headway that results in an acceptable deceleration");
237 }
238
239 final int maximumStep = (int) Math.ceil(Math.log((maximumSI - minimumSI) / precision.getSI()) / Math.log(2));
240 for (int step = 0; step < maximumStep; step++)
241 {
242 double midSI = (minimumSI + maximumSI) / 2;
243 double midSIAcceleration = computeAcceleration(followerSpeed, followerMaximumSpeed, leaderSpeed,
244 new Length(midSI, LengthUnit.SI), speedLimit).getSI();
245 if (midSIAcceleration < maximumDeceleration)
246 {
247 minimumSI = midSI;
248 }
249 else
250 {
251 maximumSI = midSI;
252 }
253 }
254 Length result = new Length(Math.min((minimumSI + maximumSI) / 2, maxDistance.si), LengthUnit.SI);
255 return result;
256 }
257
258 }