1 package org.opentrafficsim.road.gtu.lane.tactical.toledo;
2
3 import java.io.Serializable;
4 import java.util.Iterator;
5 import java.util.Random;
6
7 import org.djunits.unit.AccelerationUnit;
8 import org.djunits.unit.LengthUnit;
9 import org.djunits.unit.LinearDensityUnit;
10 import org.djunits.value.vdouble.scalar.Acceleration;
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djunits.value.vdouble.scalar.LinearDensity;
13 import org.djunits.value.vdouble.scalar.Speed;
14 import org.djunits.value.vdouble.scalar.Time;
15 import org.djutils.draw.point.OrientedPoint2d;
16 import org.djutils.exceptions.Try;
17 import org.opentrafficsim.base.parameters.ParameterException;
18 import org.opentrafficsim.base.parameters.ParameterTypeLength;
19 import org.opentrafficsim.base.parameters.ParameterTypes;
20 import org.opentrafficsim.base.parameters.Parameters;
21 import org.opentrafficsim.core.gtu.GtuException;
22 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
23 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
24 import org.opentrafficsim.core.network.LateralDirectionality;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
27 import org.opentrafficsim.road.gtu.lane.perception.CategoricalLanePerception;
28 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
29 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
30 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
31 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.DirectNeighborsPerception;
32 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
33 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
34 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
35 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneChange;
36 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder;
37 import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
38 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
39 import org.opentrafficsim.road.network.LaneChangeInfo;
40 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
41 import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56 public class Toledo extends AbstractLaneBasedTacticalPlanner
57 {
58
59
60 private static final long serialVersionUID = 20160711L;
61
62
63 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
64
65
66 static final Length MAX_LIGHT_VEHICLE_LENGTH = new Length(9.14, LengthUnit.METER);
67
68
69 static final Length TAILGATE_LENGTH = new Length(10, LengthUnit.METER);
70
71
72 static final LinearDensity LOS_DENSITY = new LinearDensity(16, LinearDensityUnit.PER_KILOMETER);
73
74
75 public static final Random RANDOM = new Random();
76
77
78 private final LaneChange laneChange;
79
80
81
82
83
84
85 public Toledo(final CarFollowingModel carFollowingModel, final LaneBasedGtu gtu)
86 {
87 super(carFollowingModel, gtu, new CategoricalLanePerception(gtu));
88 this.laneChange = Try.assign(() -> new LaneChange(gtu), "Parameter LCDUR is required.", GtuException.class);
89 getPerception().addPerceptionCategory(new ToledoPerception(getPerception()));
90 getPerception().addPerceptionCategory(new DirectNeighborsPerception(getPerception(), HeadwayGtuType.WRAP));
91 }
92
93 @Override
94 public final OperationalPlan generateOperationalPlan(final Time startTime, final OrientedPoint2d locationAtStartTime)
95 throws GtuException, NetworkException, ParameterException
96 {
97
98
99 LanePerception perception = getPerception();
100 SpeedLimitProspect slp =
101 perception.getPerceptionCategory(ToledoPerception.class).getSpeedLimitProspect(RelativeLane.CURRENT);
102 SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
103 Parameters params = getGtu().getParameters();
104
105 Acceleration acceleration = null;
106
107
108 LateralDirectionality initiatedLaneChange;
109 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
110 if (!this.laneChange.isChangingLane())
111 {
112
113
114
115
116 GapInfo gapFwdL = getGapInfo(params, perception, Gap.FORWARD, RelativeLane.LEFT);
117 GapInfo gapAdjL = getGapInfo(params, perception, Gap.ADJACENT, RelativeLane.LEFT);
118 GapInfo gapBckL = getGapInfo(params, perception, Gap.BACKWARD, RelativeLane.LEFT);
119 GapInfo gapFwdR = getGapInfo(params, perception, Gap.FORWARD, RelativeLane.RIGHT);
120 GapInfo gapAdjR = getGapInfo(params, perception, Gap.ADJACENT, RelativeLane.RIGHT);
121 GapInfo gapBckR = getGapInfo(params, perception, Gap.BACKWARD, RelativeLane.RIGHT);
122 double emuTgL =
123 Math.log(Math.exp(gapFwdL.getUtility()) + Math.exp(gapAdjL.getUtility()) + Math.exp(gapBckL.getUtility()));
124 double emuTgR =
125 Math.log(Math.exp(gapFwdR.getUtility()) + Math.exp(gapAdjR.getUtility()) + Math.exp(gapBckR.getUtility()));
126
127
128
129 double eLead = RANDOM.nextGaussian() * Math.pow(params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD), 2);
130 double eLag = RANDOM.nextGaussian() * Math.pow(params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG), 2);
131 GapAcceptanceInfo gapAcceptL =
132 getGapAcceptanceInfo(getGtu(), params, perception, emuTgL, eLead, eLag, RelativeLane.LEFT);
133 GapAcceptanceInfo gapAcceptR =
134 getGapAcceptanceInfo(getGtu(), params, perception, emuTgR, eLead, eLag, RelativeLane.RIGHT);
135
136
137 double vL = laneUtility(getGtu(), params, perception, gapAcceptL.getEmu(), sli, RelativeLane.LEFT);
138 double vC = laneUtility(getGtu(), params, perception, 0, sli, RelativeLane.CURRENT);
139 double vR = laneUtility(getGtu(), params, perception, gapAcceptR.getEmu(), sli, RelativeLane.RIGHT);
140
141
142 if (vL > vR && vL > vC && gapAcceptL.isAcceptable())
143 {
144 initiatedLaneChange = LateralDirectionality.LEFT;
145 }
146 else if (vR > vL && vR > vC && gapAcceptR.isAcceptable())
147 {
148 initiatedLaneChange = LateralDirectionality.RIGHT;
149 }
150 else
151 {
152 initiatedLaneChange = LateralDirectionality.NONE;
153 }
154
155
156 if (initiatedLaneChange.isNone())
157 {
158 if ((vC > vR && vC > vL)
159 || (!neighbors.getLeaders(RelativeLane.CURRENT).isEmpty() && neighbors.getLeaders(RelativeLane.CURRENT)
160 .first().getDistance().lt(getCarFollowingModel().desiredHeadway(params, getGtu().getSpeed()))))
161 {
162 acceleration = getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
163 neighbors.getLeaders(RelativeLane.CURRENT));
164 }
165 else
166 {
167 GapInfo gapAdj;
168 GapInfo gapFwd;
169 GapInfo gapBck;
170 if (vL > vR && vL > vC)
171 {
172 gapAdj = gapAdjL;
173 gapFwd = gapFwdL;
174 gapBck = gapBckL;
175 }
176 else
177 {
178 gapAdj = gapAdjR;
179 gapFwd = gapFwdR;
180 gapBck = gapBckR;
181 }
182 if (gapAdj.getUtility() > gapFwd.getUtility() && gapAdj.getUtility() > gapBck.getUtility())
183 {
184
185 double eadj = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ)
186 * params.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ);
187 acceleration =
188 new Acceleration(
189 params.getParameter(ToledoLaneChangeParameters.C_ADJ_ACC)
190 * (params.getParameter(ToledoLaneChangeParameters.BETA_DP)
191 * gapAdj.getLength().si - gapAdj.getDistance().si)
192 + eadj,
193 AccelerationUnit.SI);
194 }
195 else if (gapFwd.getUtility() > gapAdj.getUtility() && gapFwd.getUtility() > gapBck.getUtility())
196 {
197
198 Length desiredPosition = new Length(
199 gapFwd.getDistance().si
200 + params.getParameter(ToledoLaneChangeParameters.BETA_DP) * gapFwd.getLength().si,
201 LengthUnit.SI);
202 double deltaV = gapFwd.getSpeed().si > getGtu().getSpeed().si
203 ? params.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_POS)
204 * (gapFwd.getSpeed().si - getGtu().getSpeed().si)
205 : params.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_NEG)
206 * (getGtu().getSpeed().si - gapFwd.getSpeed().si);
207 double efwd = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_FWD)
208 * params.getParameter(ToledoLaneChangeParameters.SIGMA_FWD);
209 acceleration = new Acceleration(params.getParameter(ToledoLaneChangeParameters.C_FWD_ACC)
210 * Math.pow(desiredPosition.si, params.getParameter(ToledoLaneChangeParameters.BETA_FWD))
211 * Math.exp(deltaV) + efwd, AccelerationUnit.SI);
212 }
213 else
214 {
215
216 Length desiredPosition = new Length(
217 gapBck.getDistance().si
218 + (1 - params.getParameter(ToledoLaneChangeParameters.BETA_DP)) * gapBck.getLength().si,
219 LengthUnit.SI);
220 double deltaV = gapBck.getSpeed().si > getGtu().getSpeed().si
221 ? params.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_POS)
222 * (gapBck.getSpeed().si - getGtu().getSpeed().si)
223 : params.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_NEG)
224 * (getGtu().getSpeed().si - gapBck.getSpeed().si);
225 double ebck = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_BCK)
226 * params.getParameter(ToledoLaneChangeParameters.SIGMA_BCK);
227 acceleration = new Acceleration(params.getParameter(ToledoLaneChangeParameters.C_BCK_ACC)
228 * Math.pow(desiredPosition.si, params.getParameter(ToledoLaneChangeParameters.BETA_BCK))
229 * Math.exp(deltaV) + ebck, AccelerationUnit.SI);
230 }
231 }
232 }
233 }
234 else
235 {
236 initiatedLaneChange = LateralDirectionality.NONE;
237 }
238
239 if (initiatedLaneChange.isLeft())
240 {
241
242 getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
243 neighbors.getLeaders(RelativeLane.LEFT));
244 }
245 else if (initiatedLaneChange.isRight())
246 {
247
248 getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
249 neighbors.getLeaders(RelativeLane.RIGHT));
250 }
251
252 if (acceleration == null)
253 {
254 throw new Error("Acceleration from toledo model is null.");
255 }
256
257
258 if (initiatedLaneChange.isNone())
259 {
260 return LaneOperationalPlanBuilder.buildAccelerationPlan(getGtu(), startTime, getGtu().getSpeed(), acceleration,
261 params.getParameter(ToledoLaneChangeParameters.DT), false);
262 }
263
264 OperationalPlan plan = LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(getGtu(), initiatedLaneChange,
265 getGtu().getLocation(), startTime, getGtu().getSpeed(), acceleration,
266 params.getParameter(ToledoLaneChangeParameters.DT), this.laneChange);
267 return plan;
268 }
269
270
271
272
273
274
275
276
277
278
279
280 private GapInfo getGapInfo(final Parameters params, final LanePerception perception, final Gap gap, final RelativeLane lane)
281 throws ParameterException, OperationalPlanException
282 {
283
284
285 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
286 if (gap.equals(Gap.FORWARD) && (neighbors.getLeaders(lane) == null || neighbors.getLeaders(lane).isEmpty()))
287 {
288
289 return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
290 }
291 if (gap.equals(Gap.BACKWARD) && (neighbors.getFollowers(lane) == null || neighbors.getFollowers(lane).isEmpty()))
292 {
293
294 return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
295 }
296
297 double constant;
298 double alpha;
299 Length distanceToGap;
300 int deltaFrontVehicle;
301 Length putativeLength;
302 Length putativeDistance;
303 Speed putativeSpeed;
304 Length leaderDist;
305 Speed leaderSpeed;
306 Length followerDist;
307 Speed followerSpeed;
308
309 if (gap.equals(Gap.FORWARD))
310 {
311 constant = params.getParameter(ToledoLaneChangeParameters.C_FWD_TG);
312 alpha = 0;
313 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> itAble = neighbors.getLeaders(lane);
314 HeadwayGtu second = null;
315 if (!itAble.isEmpty())
316 {
317 Iterator<HeadwayGtu> it = itAble.iterator();
318 it.next();
319 second = it.next();
320 }
321 if (second != null)
322 {
323
324 Iterator<HeadwayGtu> it = neighbors.getLeaders(lane).iterator();
325 it.next();
326 HeadwayGtu leader = it.next();
327 leaderDist = leader.getDistance();
328 leaderSpeed = leader.getSpeed();
329 putativeLength = leaderDist.minus(neighbors.getLeaders(lane).first().getDistance())
330 .minus(neighbors.getLeaders(lane).first().getLength());
331 }
332 else
333 {
334
335
336 leaderDist = Length.POSITIVE_INFINITY;
337 leaderSpeed = Speed.POSITIVE_INFINITY;
338 putativeLength = leaderDist;
339 }
340
341 followerDist =
342 neighbors.getLeaders(lane).first().getDistance().plus(neighbors.getLeaders(lane).first().getLength());
343 followerSpeed = neighbors.getLeaders(lane).first().getSpeed();
344 distanceToGap = followerDist;
345 putativeDistance = distanceToGap;
346 putativeSpeed = followerSpeed;
347 }
348 else if (gap.equals(Gap.ADJACENT))
349 {
350 constant = 0;
351 alpha = params.getParameter(ToledoLaneChangeParameters.ALPHA_ADJ);
352 distanceToGap = Length.ZERO;
353 if (neighbors.getLeaders(lane) != null && !neighbors.getLeaders(lane).isEmpty())
354 {
355 leaderDist = neighbors.getLeaders(lane).first().getDistance();
356 leaderSpeed = neighbors.getLeaders(lane).first().getSpeed();
357 }
358 else
359 {
360 leaderDist = Length.POS_MAXVALUE;
361 leaderSpeed = Speed.POS_MAXVALUE;
362 }
363 if (neighbors.getFollowers(lane) != null && !neighbors.getFollowers(lane).isEmpty())
364 {
365
366 followerDist = neighbors.getFollowers(lane).first().getDistance().plus(getGtu().getLength());
367 followerSpeed = neighbors.getFollowers(lane).first().getSpeed();
368 putativeDistance = followerDist;
369 }
370 else
371 {
372 followerDist = Length.NEG_MAXVALUE;
373 followerSpeed = Speed.NEG_MAXVALUE;
374 putativeDistance = Length.POS_MAXVALUE;
375 }
376 putativeSpeed = null;
377 putativeLength = leaderDist.plus(followerDist).plus(getGtu().getLength());
378 }
379 else
380 {
381 constant = params.getParameter(ToledoLaneChangeParameters.C_BCK_TG);
382 alpha = params.getParameter(ToledoLaneChangeParameters.ALPHA_BCK);
383 deltaFrontVehicle = 0;
384 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> itAble = neighbors.getFollowers(lane);
385 HeadwayGtu second = null;
386 if (!itAble.isEmpty())
387 {
388 Iterator<HeadwayGtu> it = itAble.iterator();
389 it.next();
390 second = it.next();
391 }
392 if (second != null)
393 {
394
395 Iterator<HeadwayGtu> it = neighbors.getFollowers(lane).iterator();
396 it.next();
397 HeadwayGtu follower = it.next();
398 followerDist = follower.getDistance();
399 followerSpeed = follower.getSpeed();
400 putativeLength = followerDist.minus(neighbors.getFollowers(lane).first().getDistance())
401 .minus(neighbors.getFollowers(lane).first().getLength());
402 }
403 else
404 {
405
406 followerDist = Length.NEGATIVE_INFINITY;
407 followerSpeed = Speed.NEGATIVE_INFINITY;
408 putativeLength = followerDist;
409 }
410
411 leaderDist =
412 neighbors.getFollowers(lane).first().getDistance().plus(neighbors.getLeaders(lane).first().getLength());
413 leaderSpeed = neighbors.getFollowers(lane).first().getSpeed();
414 distanceToGap = neighbors.getFollowers(lane).first().getDistance().plus(getGtu().getLength());
415 putativeDistance = distanceToGap.plus(neighbors.getFollowers(lane).first().getLength());
416 putativeSpeed = leaderSpeed;
417 }
418
419
420 if (!gap.equals(Gap.BACKWARD) && !neighbors.getLeaders(RelativeLane.CURRENT).isEmpty()
421 && neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance().lt(leaderDist))
422 {
423 leaderDist = neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance();
424 leaderSpeed = neighbors.getLeaders(RelativeLane.CURRENT).first().getSpeed();
425 deltaFrontVehicle = 1;
426 }
427 else
428 {
429 deltaFrontVehicle = 0;
430 }
431
432
433 Speed dV = followerSpeed.minus(leaderSpeed);
434 Length effectiveGap = leaderDist.minus(followerDist);
435
436
437 double util = constant
438 + params.getParameter(ToledoLaneChangeParameters.BETA_DTG) * distanceToGap.si
439 + params.getParameter(ToledoLaneChangeParameters.BETA_EG) * effectiveGap.si
440 + params.getParameter(ToledoLaneChangeParameters.BETA_FV) * deltaFrontVehicle
441 + params.getParameter(ToledoLaneChangeParameters.BETA_RGS) * dV.si
442 + alpha * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
443
444 return new GapInfo(util, putativeLength, putativeDistance, putativeSpeed);
445
446 }
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461 private GapAcceptanceInfo getGapAcceptanceInfo(final LaneBasedGtu gtu, final Parameters params,
462 final LanePerception perception, final double emuTg, final double eLead, final double eLag, final RelativeLane lane)
463 throws ParameterException, OperationalPlanException
464 {
465
466
467 double vLead;
468 double vLag;
469 boolean acceptLead;
470 boolean acceptLag;
471
472 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
473 if (neighbors.getLeaders(lane) != null && !neighbors.getLeaders(lane).isEmpty())
474 {
475 Speed dVLead = neighbors.getLeaders(lane).first().getSpeed().minus(gtu.getSpeed());
476 Length sLead = neighbors.getLeaders(lane).first().getDistance();
477
478 Length gLead =
479 new Length(
480 Math.exp(params.getParameter(ToledoLaneChangeParameters.C_LEAD)
481 + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD)
482 * Speed.max(dVLead, Speed.ZERO).si
483 + params.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD)
484 * Speed.min(dVLead, Speed.ZERO).si
485 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg
486 + params.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LEAD)
487 * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
488 + eLead),
489 LengthUnit.SI);
490 vLead = Math.log(gLead.si) - (params.getParameter(ToledoLaneChangeParameters.C_LEAD)
491 + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD) * Speed.max(dVLead, Speed.ZERO).si
492 + params.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD) * Speed.min(dVLead, Speed.ZERO).si
493 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg);
494 acceptLead = gLead.lt(sLead);
495 }
496 else
497 {
498 vLead = 0;
499 acceptLead = false;
500 }
501
502 if (neighbors.getFollowers(lane) != null && !neighbors.getFollowers(lane).isEmpty())
503 {
504 Speed dVLag = neighbors.getFollowers(lane).first().getSpeed().minus(gtu.getSpeed());
505 Length sLag = neighbors.getFollowers(lane).first().getDistance();
506
507 Length gLag =
508 new Length(
509 Math.exp(params.getParameter(ToledoLaneChangeParameters.C_LAG)
510 + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG)
511 * Speed.max(dVLag, Speed.ZERO).si
512 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg
513 + params.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LAG)
514 * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
515 + eLag),
516 LengthUnit.SI);
517 vLag = Math.log(gLag.si) - (params.getParameter(ToledoLaneChangeParameters.C_LAG)
518 + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG) * Speed.max(dVLag, Speed.ZERO).si
519 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg);
520 acceptLag = gLag.lt(sLag);
521 }
522 else
523 {
524 vLag = 0;
525 acceptLag = false;
526 }
527
528
529 double vRatLead = vLead / params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD);
530 double vRatLag = vLag / params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG);
531 double emuGa = vLead * cumNormDist(vRatLead)
532 + params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD) * normDist(vRatLead) + vLag * cumNormDist(vRatLag)
533 + params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG) * normDist(vRatLag);
534
535
536 return new GapAcceptanceInfo(emuGa, acceptLead && acceptLag);
537
538 }
539
540
541
542
543
544
545
546
547
548
549
550
551
552 private double laneUtility(final LaneBasedGtu gtu, final Parameters params, final LanePerception perception,
553 final double emuGa, final SpeedLimitInfo sli, final RelativeLane lane)
554 throws ParameterException, OperationalPlanException
555 {
556
557 ToledoPerception toledo = perception.getPerceptionCategory(ToledoPerception.class);
558 if (!perception.getLaneStructure().exists(lane))
559 {
560 return 0.0;
561 }
562
563
564 boolean takeNextOffRamp = false;
565
566
567
568
569
570
571
572 int deltaNextExit = takeNextOffRamp ? 1 : 0;
573
574 Length dExit;
575 if (!toledo.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT).isEmpty())
576 {
577 dExit = toledo.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT).first().remainingDistance();
578 }
579 else
580 {
581 dExit = Length.POSITIVE_INFINITY;
582 }
583
584 int[] delta = new int[3];
585 int deltaAdd = 0;
586 if (!toledo.getInfrastructureLaneChangeInfo(lane).isEmpty())
587 {
588 LaneChangeInfo ilciLef = toledo.getInfrastructureLaneChangeInfo(lane).first();
589 if (ilciLef.numberOfLaneChanges() > 1 && ilciLef.numberOfLaneChanges() < 5)
590 {
591 deltaAdd = ilciLef.numberOfLaneChanges() - 2;
592 delta[deltaAdd] = 1;
593 }
594 }
595
596
597 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
598 Length leaderLength = !lane.isCurrent() && !neighbors.getFirstLeaders(lane.getLateralDirectionality()).isEmpty()
599 ? neighbors.getFirstLeaders(lane.getLateralDirectionality()).first().getLength() : Length.ZERO;
600 Length followerLength = !lane.isCurrent() && !neighbors.getFirstFollowers(lane.getLateralDirectionality()).isEmpty()
601 ? neighbors.getFirstFollowers(lane.getLateralDirectionality()).first().getDistance() : Length.ZERO;
602 int deltaHeavy = leaderLength.gt(MAX_LIGHT_VEHICLE_LENGTH) || followerLength.gt(MAX_LIGHT_VEHICLE_LENGTH) ? 1 : 0;
603
604
605 LinearDensity d = getDensityInLane(gtu, perception, lane);
606
607
608 int deltaTailgate = 0;
609 if (lane.equals(RelativeLane.CURRENT) && !neighbors.getFollowers(RelativeLane.CURRENT).isEmpty()
610 && neighbors.getFollowers(RelativeLane.CURRENT).first().getDistance().le(TAILGATE_LENGTH))
611 {
612 LinearDensity dL = getDensityInLane(gtu, perception, RelativeLane.LEFT);
613 LinearDensity dR = getDensityInLane(gtu, perception, RelativeLane.RIGHT);
614 LinearDensity dRoad = new LinearDensity((dL.si + d.si + dR.si) / 3, LinearDensityUnit.SI);
615 if (dRoad.le(LOS_DENSITY))
616 {
617 deltaTailgate = 1;
618 }
619 }
620
621
622
623 int deltaRightMost = 0;
624 if (lane.equals(RelativeLane.CURRENT) && !toledo.getCrossSection().contains(RelativeLane.RIGHT))
625 {
626 deltaRightMost = 1;
627 }
628 else if (lane.equals(RelativeLane.RIGHT) && toledo.getCrossSection().contains(RelativeLane.RIGHT)
629 && !toledo.getCrossSection().contains(RelativeLane.SECOND_RIGHT))
630 {
631 deltaRightMost = 1;
632 }
633
634
635 Speed vFront;
636 Length sFront;
637 if (lane.equals(RelativeLane.CURRENT))
638 {
639 if (!neighbors.getLeaders(RelativeLane.CURRENT).isEmpty())
640 {
641 vFront = neighbors.getLeaders(RelativeLane.CURRENT).first().getSpeed();
642 sFront = neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance();
643 }
644 else
645 {
646 vFront = getCarFollowingModel().desiredSpeed(params, sli);
647 sFront = getCarFollowingModel().desiredHeadway(params, gtu.getSpeed());
648 }
649 }
650 else
651 {
652 vFront = Speed.ZERO;
653 sFront = Length.ZERO;
654 }
655
656
657 double constant = 0;
658 double error = 0;
659 if (lane.equals(RelativeLane.CURRENT))
660 {
661 constant = params.getParameter(ToledoLaneChangeParameters.C_CL);
662 error = params.getParameter(ToledoLaneChangeParameters.ALPHA_CL)
663 * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
664 }
665 else if (lane.equals(RelativeLane.RIGHT))
666 {
667 constant = params.getParameter(ToledoLaneChangeParameters.C_RL);
668 error = params.getParameter(ToledoLaneChangeParameters.ALPHA_RL)
669 * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
670 }
671
672 return constant
673 + params.getParameter(ToledoLaneChangeParameters.BETA_RIGHT_MOST) * deltaRightMost
674 + params.getParameter(ToledoLaneChangeParameters.BETA_VFRONT) * vFront.si
675 + params.getParameter(ToledoLaneChangeParameters.BETA_SFRONT) * sFront.si
676 + params.getParameter(ToledoLaneChangeParameters.BETA_DENSITY) * d.getInUnit(LinearDensityUnit.PER_KILOMETER)
677 + params.getParameter(ToledoLaneChangeParameters.BETA_HEAVY_NEIGHBOUR) * deltaHeavy
678 + params.getParameter(ToledoLaneChangeParameters.BETA_TAILGATE) * deltaTailgate
679 + Math.pow(dExit.getInUnit(LengthUnit.KILOMETER), params.getParameter(ToledoLaneChangeParameters.THETA_MLC))
680 * (params.getParameter(ToledoLaneChangeParameters.BETA1) * delta[0]
681 + params.getParameter(ToledoLaneChangeParameters.BETA2) * delta[1]
682 + params.getParameter(ToledoLaneChangeParameters.BETA3) * delta[2])
683 + params.getParameter(ToledoLaneChangeParameters.BETA_NEXT_EXIT) * deltaNextExit
684 + params.getParameter(ToledoLaneChangeParameters.BETA_ADD) * deltaAdd
685 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_GA) * emuGa
686 + error;
687
688 }
689
690
691
692
693
694
695
696
697
698 private LinearDensity getDensityInLane(final LaneBasedGtu gtu, final LanePerception perception, final RelativeLane lane)
699 throws OperationalPlanException
700 {
701 int nVehicles = 0;
702 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
703 Length up = Length.ZERO;
704 Length down = Length.ZERO;
705 for (HeadwayGtu neighbor : neighbors.getFollowers(lane))
706 {
707 nVehicles++;
708 down = neighbor.getDistance();
709 }
710 for (HeadwayGtu neighbor : neighbors.getLeaders(lane))
711 {
712 nVehicles++;
713 up = neighbor.getDistance();
714 }
715 if (nVehicles > 0)
716 {
717 return new LinearDensity(nVehicles / up.plus(down).getInUnit(LengthUnit.KILOMETER),
718 LinearDensityUnit.PER_KILOMETER);
719 }
720 return LinearDensity.ZERO;
721 }
722
723
724
725
726
727
728 private static double cumNormDist(final double x)
729 {
730 return .5 * (1 + erf(x / Math.sqrt(2)));
731 }
732
733
734
735
736
737
738 private static double erf(final double x)
739 {
740 double t = 1.0 / (1.0 + 0.5 * Math.abs(x));
741
742
743 double tau = t * Math.exp(-x * x - 1.26551223 + t * (1.00002368 + t * (0.37409196
744 + t * (0.09678418 + t * (0.18628806 + t * (0.27886807 + t * (-1.13520398
745 + t * (1.48851587 + t * (-0.82215223 + t * (0.17087277))))))))));
746
747 return x >= 0 ? 1 - tau : tau - 1;
748 }
749
750
751
752
753
754
755 private static double normDist(final double x)
756 {
757 return Math.exp(-x * x / 2) / Math.sqrt(2 * Math.PI);
758 }
759
760
761
762
763
764
765
766
767
768
769
770
771 private enum Gap
772 {
773
774 FORWARD,
775
776
777 ADJACENT,
778
779
780 BACKWARD;
781 }
782
783
784
785
786
787
788
789
790
791
792
793
794 private class GapInfo implements Serializable
795 {
796
797
798 private static final long serialVersionUID = 20160811L;
799
800
801 private final double utility;
802
803
804 private final Length length;
805
806
807 private final Length distance;
808
809
810 private final Speed speed;
811
812
813
814
815
816
817
818 GapInfo(final double utility, final Length length, final Length distance, final Speed speed)
819 {
820 this.utility = utility;
821 this.length = length;
822 this.distance = distance;
823 this.speed = speed;
824 }
825
826
827
828
829 public final double getUtility()
830 {
831 return this.utility;
832 }
833
834
835
836
837 public final Length getLength()
838 {
839 return this.length;
840 }
841
842
843
844
845 public final Length getDistance()
846 {
847 return this.distance;
848 }
849
850
851
852
853 public final Speed getSpeed()
854 {
855 return this.speed;
856 }
857
858 @Override
859 public String toString()
860 {
861 return "GapAcceptanceInfo [utility = " + this.utility + ", length = " + this.length + ", distance = "
862 + this.distance + ", speed = " + this.speed + "]";
863 }
864
865 }
866
867
868
869
870
871
872
873
874
875
876
877
878 private class GapAcceptanceInfo implements Serializable
879 {
880
881
882 private static final long serialVersionUID = 20160811L;
883
884
885 private final double emu;
886
887
888 private final boolean acceptable;
889
890
891
892
893
894 GapAcceptanceInfo(final double emu, final boolean acceptable)
895 {
896 this.emu = emu;
897 this.acceptable = acceptable;
898 }
899
900
901
902
903 public final double getEmu()
904 {
905 return this.emu;
906 }
907
908
909
910
911 public final boolean isAcceptable()
912 {
913 return this.acceptable;
914 }
915
916 @Override
917 public String toString()
918 {
919 return "GapAcceptanceInfo [emu = " + this.emu + ", acceptable = " + this.acceptable + "]";
920 }
921
922 }
923
924 @Override
925 public final String toString()
926 {
927 return "Toledo tactical planner.";
928 }
929
930 }