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