1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.util.Collections;
4 import java.util.Iterator;
5 import java.util.Set;
6
7 import org.djunits.value.vdouble.scalar.Length;
8 import org.opentrafficsim.base.parameters.ParameterException;
9 import org.opentrafficsim.base.parameters.ParameterTypes;
10 import org.opentrafficsim.core.gtu.GTUException;
11 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
12 import org.opentrafficsim.core.network.LateralDirectionality;
13 import org.opentrafficsim.core.network.Link;
14 import org.opentrafficsim.core.network.Node;
15 import org.opentrafficsim.core.network.route.Route;
16 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
17 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
18 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
19 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
20 import org.opentrafficsim.road.network.lane.Lane;
21 import org.opentrafficsim.road.network.lane.LaneDirection;
22
23
24
25
26
27
28
29
30
31
32
33
34 public interface LaneBasedTacticalPlanner extends TacticalPlanner<LaneBasedGTU, LanePerception>
35 {
36
37
38
39
40 CarFollowingModel getCarFollowingModel();
41
42
43
44
45
46
47
48
49 default LaneDirectioneDirection.html#LaneDirection">LaneDirection chooseLaneAtSplit(final LaneDirection from, final Set<LaneDirection> lanes) throws ParameterException
50 {
51 if (getGtu().getOperationalPlan() instanceof LaneBasedOperationalPlan
52 && ((LaneBasedOperationalPlan) getGtu().getOperationalPlan()).isDeviative())
53 {
54
55 LateralDirectionality forceSide = LateralDirectionality.NONE;
56 try
57 {
58 Set<Lane> leftLanes = from.getLane().accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT,
59 getGtu().getGTUType(), from.getDirection());
60 if (!Collections.disjoint(getGtu().positions(getGtu().getReference()).keySet(), leftLanes))
61 {
62 forceSide = LateralDirectionality.LEFT;
63 }
64 else
65 {
66 Set<Lane> rightLanes = from.getLane().accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT,
67 getGtu().getGTUType(), from.getDirection());
68 if (!Collections.disjoint(getGtu().positions(getGtu().getReference()).keySet(), rightLanes))
69 {
70 forceSide = LateralDirectionality.RIGHT;
71 }
72 }
73 }
74 catch (GTUException exception)
75 {
76 throw new RuntimeException("Exception obtaining reference position.", exception);
77 }
78 if (!forceSide.isNone())
79 {
80 if (lanes.isEmpty())
81 {
82
83 return null;
84 }
85 else
86 {
87 Iterator<LaneDirection> iter = lanes.iterator();
88 LaneDirection next = iter.next();
89 while (iter.hasNext())
90 {
91 LaneDirection candidate = iter.next();
92 next = LaneBasedTacticalPlanner.mostOnSide(next, candidate, forceSide);
93 }
94 return next;
95 }
96 }
97 }
98 Route route = getGtu().getStrategicalPlanner().getRoute();
99 if (route == null)
100 {
101
102 LaneDirection rightMost = null;
103 for (LaneDirection lane : lanes)
104 {
105 rightMost = rightMost == null ? lane : mostOnSide(rightMost, lane, LateralDirectionality.RIGHT);
106 }
107 return rightMost;
108 }
109 Length maxDistance = Length.NEGATIVE_INFINITY;
110 LaneDirection best = null;
111 for (LaneDirection lane : lanes)
112 {
113 LaneDirection next = lane.getNextLaneDirection(getGtu());
114 if (next != null)
115 {
116 Length okDistance = okDistance(next, lane.getLength(), route,
117 getGtu().getParameters().getParameter(ParameterTypes.PERCEPTION));
118 if (maxDistance.eq(okDistance))
119 {
120 best = mostOnSide(best, lane, LateralDirectionality.RIGHT);
121 }
122 else if (okDistance.gt(maxDistance))
123 {
124 maxDistance = okDistance;
125 best = lane;
126 }
127 }
128 }
129 return best;
130 }
131
132
133
134
135
136
137
138
139
140
141
142 default Length okDistance(final LaneDirection lane, final Length distance, final Route route, final Length maxDistance)
143 {
144 if (distance.gt(maxDistance))
145 {
146 return maxDistance;
147 }
148 LaneDirection next = lane.getNextLaneDirection(getGtu());
149 if (next == null)
150 {
151 Node endNode = lane.getDirection().isPlus() ? lane.getLane().getParentLink().getEndNode()
152 : lane.getLane().getParentLink().getStartNode();
153 Set<Link> links = endNode.getLinks().toSet();
154 links.remove(lane.getLane().getParentLink());
155 if (route.contains(endNode) && (links.isEmpty() || links.iterator().next().getLinkType().isConnector()))
156 {
157
158 return maxDistance;
159 }
160
161 return distance.plus(lane.getLength());
162 }
163 return okDistance(next, distance.plus(lane.getLength()), route, maxDistance);
164 }
165
166
167
168
169
170
171
172
173 static LaneDirectionrk/lane/LaneDirection.html#LaneDirection">LaneDirectionane/LaneDirection.html#LaneDirection">LaneDirection mostOnSide(final LaneDirectionrk/lane/LaneDirection.html#LaneDirection">LaneDirection lane1, final LaneDirection lane2, final LateralDirectionality lat)
174 {
175 Length offset1 = lane1.getLane().getDesignLineOffsetAtBegin().plus(lane1.getLane().getDesignLineOffsetAtEnd());
176 offset1 = lane1.getDirection().isPlus() ? offset1 : offset1.neg();
177 Length offset2 = lane2.getLane().getDesignLineOffsetAtBegin().plus(lane2.getLane().getDesignLineOffsetAtEnd());
178 offset2 = lane2.getDirection().isPlus() ? offset2 : offset2.neg();
179 if (lat.isLeft())
180 {
181 return offset1.gt(offset2) ? lane1 : lane2;
182 }
183 return offset1.gt(offset2) ? lane2 : lane1;
184 }
185
186 }