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
56 @Override
57 public final Desire determineDesire(final Parameters parameters, final LanePerception perception,
58 final CarFollowingModel carFollowingModel, final Desire mandatoryDesire, final Desire voluntaryDesire)
59 throws ParameterException, OperationalPlanException
60 {
61
62 double dLeftYes = 0;
63 double dRightYes = 0;
64 double dLeftNo = 0;
65 double dRightNo = 0;
66 double socio = parameters.getParameter(SOCIO);
67 Acceleration b = parameters.getParameter(B);
68 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
69 Speed ownSpeed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
70 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
71 SpeedLimitInfo sli = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
72 boolean leftLane = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.LEFT).si > 0.0;
73 boolean rightLane = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.RIGHT).si > 0.0;
74 for (LateralDirectionality dir : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT})
75 {
76 Iterable<HeadwayGtu> leaders = neighbors.getLeaders(new RelativeLane(dir, 1));
77 if (leaders != null)
78 {
79 for (HeadwayGtu leader : leaders)
80 {
81 Parameters params = leader.getParameters();
82 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
83 if (desire > 0)
84 {
85
86 Acceleration a =
87 CarFollowingUtil.followSingleLeader(carFollowingModel, parameters, ownSpeed, sli, leader);
88 if (a.lt0())
89 {
90 double d = desire * Math.min(-a.si / b.si, 1.0);
91 if (dir.isLeft() && rightLane)
92 {
93
94 dRightYes = dRightYes > d ? dRightYes : d;
95 }
96 else if (leftLane)
97 {
98
99 dLeftYes = dLeftYes > d ? dLeftYes : d;
100 }
101 }
102 }
103 }
104 }
105
106 Iterable<HeadwayGtu> followers = neighbors.getFollowers(new RelativeLane(dir, 2));
107 if (followers != null)
108 {
109 for (HeadwayGtu follower : followers)
110 {
111 Parameters params = follower.getParameters();
112 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
113 Acceleration a = follower.getDistance().lt0() ? b.neg()
114 : LmrsUtil.singleAcceleration(follower.getDistance(), follower.getSpeed(), ownSpeed, desire, params,
115 follower.getSpeedLimitInfo(), follower.getCarFollowingModel());
116 if (a.lt0())
117 {
118 if (desire > 0)
119 {
120 double d = desire * Math.min(-a.si / b.si, 1.0);
121 if (dir.isLeft() && leftLane)
122 {
123
124 dLeftNo = dLeftNo > d ? dLeftNo : d;
125 }
126 else if (rightLane)
127 {
128
129 dRightNo = dRightNo > d ? dRightNo : d;
130 }
131 }
132 }
133 else
134 {
135
136 break;
137 }
138 }
139 }
140 leaders = neighbors.getLeaders(new RelativeLane(dir, 2));
141 if (leaders != null)
142 {
143 for (HeadwayGtu leader : leaders)
144 {
145 Parameters params = leader.getParameters();
146 double desire = dir.isLeft() ? params.getParameter(DRIGHT) : params.getParameter(DLEFT);
147 if (desire > 0)
148 {
149 Acceleration a = LmrsUtil.singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire,
150 params, sli, carFollowingModel);
151 if (a.lt0())
152 {
153 double d = desire * Math.min(-a.si / b.si, 1.0);
154 if (dir.isLeft() && leftLane)
155 {
156
157 dLeftNo = dLeftNo > d ? dLeftNo : d;
158 }
159 else if (rightLane)
160 {
161
162 dRightNo = dRightNo > d ? dRightNo : d;
163 }
164 }
165 }
166 }
167 }
168 }
169
170 dLeftYes *= socio;
171 dRightYes *= socio;
172 return new Desire(dLeftYes - dLeftNo, dRightYes - dRightNo);
173
174 }
175
176
177 @Override
178 public final String toString()
179 {
180 return "IncentiveCourtesy";
181 }
182
183 }