1 package org.opentrafficsim.road.gtu.lane.tactical.lmrs;
2
3 import org.djunits.value.vdouble.scalar.Acceleration;
4 import org.djunits.value.vdouble.scalar.Length;
5 import org.djunits.value.vdouble.scalar.Speed;
6 import org.opentrafficsim.base.parameters.ParameterException;
7 import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
8 import org.opentrafficsim.base.parameters.ParameterTypeDouble;
9 import org.opentrafficsim.base.parameters.ParameterTypes;
10 import org.opentrafficsim.base.parameters.Parameters;
11 import org.opentrafficsim.core.gtu.perception.EgoPerception;
12 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
13 import org.opentrafficsim.core.network.LateralDirectionality;
14 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
15 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
16 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
17 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
18 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
19 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
20 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
21 import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Desire;
22 import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.LmrsParameters;
23 import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.LmrsUtil;
24 import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.VoluntaryIncentive;
25 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40 public class IncentiveCourtesy implements VoluntaryIncentive
41 {
42
43
44 protected static final ParameterTypeAcceleration B = ParameterTypes.B;
45
46
47 protected static final ParameterTypeDouble SOCIO = LmrsParameters.SOCIO;
48
49
50 protected static final ParameterTypeDouble DLEFT = LmrsParameters.DLEFT;
51
52
53 protected static final ParameterTypeDouble DRIGHT = LmrsParameters.DRIGHT;
54
55 @Override
56 public final Desire determineDesire(final Parameters parameters, final LanePerception perception,
57 final CarFollowingModel carFollowingModel, final Desire mandatoryDesire, final Desire voluntaryDesire)
58 throws ParameterException, OperationalPlanException
59 {
60
61 double dLeftYes = 0;
62 double dRightYes = 0;
63 double dLeftNo = 0;
64 double dRightNo = 0;
65 double socio = parameters.getParameter(SOCIO);
66 Acceleration b = parameters.getParameter(B);
67 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
68 Speed ownSpeed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
69 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
70 SpeedLimitInfo sli = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
71 boolean leftLane = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.LEFT).si > 0.0;
72 boolean rightLane = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.RIGHT).si > 0.0;
73 for (LateralDirectionality dir : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT})
74 {
75 Iterable<HeadwayGtu> leaders = neighbors.getLeaders(new RelativeLane(dir, 1));
76 if (leaders != null)
77 {
78 for (HeadwayGtu leader : leaders)
79 {
80 Parameters params = leader.getParameters();
81 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
82 if (desire > 0)
83 {
84
85 Acceleration a =
86 CarFollowingUtil.followSingleLeader(carFollowingModel, parameters, ownSpeed, sli, leader);
87 if (a.lt0())
88 {
89 double d = desire * Math.min(-a.si / b.si, 1.0);
90 if (dir.isLeft() && rightLane)
91 {
92
93 dRightYes = dRightYes > d ? dRightYes : d;
94 }
95 else if (leftLane)
96 {
97
98 dLeftYes = dLeftYes > d ? dLeftYes : d;
99 }
100 }
101 }
102 }
103 }
104
105 Iterable<HeadwayGtu> followers = neighbors.getFollowers(new RelativeLane(dir, 2));
106 if (followers != null)
107 {
108 for (HeadwayGtu follower : followers)
109 {
110 Parameters params = follower.getParameters();
111 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
112 Acceleration a = follower.getDistance().lt0() ? b.neg()
113 : LmrsUtil.singleAcceleration(follower.getDistance(), follower.getSpeed(), ownSpeed, desire, params,
114 follower.getSpeedLimitInfo(), follower.getCarFollowingModel());
115 if (a.lt0())
116 {
117 if (desire > 0)
118 {
119 double d = desire * Math.min(-a.si / b.si, 1.0);
120 if (dir.isLeft() && leftLane)
121 {
122
123 dLeftNo = dLeftNo > d ? dLeftNo : d;
124 }
125 else if (rightLane)
126 {
127
128 dRightNo = dRightNo > d ? dRightNo : d;
129 }
130 }
131 }
132 else
133 {
134
135 break;
136 }
137 }
138 }
139 leaders = neighbors.getLeaders(new RelativeLane(dir, 2));
140 if (leaders != null)
141 {
142 for (HeadwayGtu leader : leaders)
143 {
144 Parameters params = leader.getParameters();
145 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
146 if (desire > 0)
147 {
148 Acceleration a = LmrsUtil.singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire,
149 params, sli, carFollowingModel);
150 if (a.lt0())
151 {
152 double d = desire * Math.min(-a.si / b.si, 1.0);
153 if (dir.isLeft() && leftLane)
154 {
155
156 dLeftNo = dLeftNo > d ? dLeftNo : d;
157 }
158 else if (rightLane)
159 {
160
161 dRightNo = dRightNo > d ? dRightNo : d;
162 }
163 }
164 }
165 }
166 }
167 }
168
169 dLeftYes *= socio;
170 dRightYes *= socio;
171 return new Desire(dLeftYes - dLeftNo, dRightYes - dRightNo);
172
173 }
174
175 @Override
176 public final String toString()
177 {
178 return "IncentiveCourtesy";
179 }
180
181 }