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