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