1 package org.opentrafficsim.road.gtu.strategical.route;
2
3 import java.io.Serializable;
4 import java.util.Iterator;
5 import java.util.Set;
6
7 import org.opentrafficsim.core.Throw;
8 import org.opentrafficsim.core.gtu.GTU;
9 import org.opentrafficsim.core.gtu.GTUDirectionality;
10 import org.opentrafficsim.core.gtu.GTUException;
11 import org.opentrafficsim.core.gtu.GTUType;
12 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
13 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
14 import org.opentrafficsim.core.network.Link;
15 import org.opentrafficsim.core.network.LinkDirection;
16 import org.opentrafficsim.core.network.NetworkException;
17 import org.opentrafficsim.core.network.Node;
18 import org.opentrafficsim.core.network.route.Route;
19 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
20 import org.opentrafficsim.road.gtu.strategical.AbstractLaneBasedStrategicalPlanner;
21 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
22 import org.opentrafficsim.road.network.lane.CrossSectionElement;
23 import org.opentrafficsim.road.network.lane.CrossSectionLink;
24 import org.opentrafficsim.road.network.lane.Lane;
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40 public class LaneBasedStrategicalRoutePlanner extends AbstractLaneBasedStrategicalPlanner implements
41 LaneBasedStrategicalPlanner, Serializable
42 {
43
44 private static final long serialVersionUID = 20150724L;
45
46
47 private final Route route;
48
49
50 private final TacticalPlanner fixedTacticalPlanner;
51
52
53
54
55
56
57
58 public LaneBasedStrategicalRoutePlanner(final BehavioralCharacteristics behavioralCharacteristics,
59 final TacticalPlanner fixedTacticalPlanner, final LaneBasedGTU gtu) throws GTUException
60 {
61 this(behavioralCharacteristics, fixedTacticalPlanner, null, gtu);
62 }
63
64
65
66
67
68
69
70
71 public LaneBasedStrategicalRoutePlanner(final BehavioralCharacteristics behavioralCharacteristics,
72 final TacticalPlanner fixedTacticalPlanner, final Route route, final LaneBasedGTU gtu) throws GTUException
73 {
74 super(behavioralCharacteristics, gtu);
75 this.route = route;
76 this.fixedTacticalPlanner = fixedTacticalPlanner;
77 Throw.when(fixedTacticalPlanner == null, GTUException.class,
78 "Fixed Tactical Planner for a Strategical planner is null");
79 }
80
81
82 @Override
83 public TacticalPlanner generateTacticalPlanner()
84 {
85 return this.fixedTacticalPlanner;
86 }
87
88
89 @Override
90 public Node nextNode(final Link link, final GTUDirectionality direction, final GTUType gtuType) throws NetworkException
91 {
92 LinkDirection linkDirection = nextLinkDirection(link, direction, gtuType);
93 return linkDirection.getNodeTo();
94 }
95
96
97 @Override
98 public LinkDirection nextLinkDirection(final Link link, final GTUDirectionality direction, final GTUType gtuType)
99 throws NetworkException
100 {
101 Node nextNode = direction.equals(GTUDirectionality.DIR_PLUS) ? link.getEndNode() : link.getStartNode();
102 return nextLinkDirection(nextNode, link, gtuType);
103 }
104
105
106 @Override
107 public Node nextNode(final Node node, final Link previousLink, final GTUType gtuType) throws NetworkException
108 {
109 LinkDirection linkDirection = nextLinkDirection(node, previousLink, gtuType);
110 return linkDirection.getNodeTo();
111 }
112
113
114 @Override
115 public LinkDirection nextLinkDirection(final Node node, final Link previousLink, final GTUType gtuType)
116 throws NetworkException
117 {
118
119
120
121
122
123
124 if (node.getLinks().size() == 1 && previousLink != null)
125 {
126
127 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link, but node " + node
128 + " has no successors");
129 }
130 if (node.getLinks().size() == 1 && previousLink == null)
131 {
132
133 Link link = node.getLinks().iterator().next();
134 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
135 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
136 }
137 if (node.getLinks().size() == 2)
138 {
139
140
141
142
143 for (Link link : node.getLinks())
144 {
145 if (!link.equals(previousLink))
146 {
147 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
148 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
149 }
150 }
151 }
152
153
154 Set<Link> links = node.getLinks();
155 for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
156 {
157 Link link = linkIterator.next();
158 if (link.equals(previousLink))
159 {
160
161 linkIterator.remove();
162 }
163 else
164 {
165
166 if ((link.getStartNode().equals(node) && link.getDirectionality(gtuType).isBackward())
167 || (link.getEndNode().equals(node) && link.getDirectionality(gtuType).isForward()))
168 {
169 linkIterator.remove();
170 }
171 else
172 {
173
174 boolean out = false;
175 CrossSectionLink csLink = (CrossSectionLink) link;
176 for (CrossSectionElement cse : csLink.getCrossSectionElementList())
177 {
178 if (cse instanceof Lane)
179 {
180 Lane lane = (Lane) cse;
181 if ((link.getStartNode().equals(node) && lane.getDirectionality(gtuType).isForwardOrBoth())
182 || (link.getEndNode().equals(node) && lane.getDirectionality(gtuType).isBackwardOrBoth()))
183 {
184 out = true;
185 }
186 }
187 }
188 if (!out)
189 {
190 linkIterator.remove();
191 }
192 }
193 }
194 }
195
196
197
198
199
200
201 if (links.size() == 1)
202 {
203 Link link = links.iterator().next();
204 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
205 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
206 }
207
208
209 if (this.route == null)
210 {
211 throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
212 }
213 int i = this.route.getNodes().indexOf(node);
214 if (i == -1)
215 {
216 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
217 + previousLink + ", but node " + node + " not in route " + this.route);
218 }
219 if (i == this.route.getNodes().size() - 1)
220 {
221 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
222 + previousLink + ", but the GTU reached the last node for route " + this.route);
223 }
224 Node nextNode = this.route.getNode(i + 1);
225
226
227
228
229
230
231 for (Link link : links)
232 {
233 if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
234 {
235
236
237
238
239 return new LinkDirection(link, GTUDirectionality.DIR_MINUS);
240 }
241 if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
242 {
243
244
245
246
247 return new LinkDirection(link, GTUDirectionality.DIR_PLUS);
248 }
249 }
250 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next linkcoming from " + previousLink
251 + ", but no link could be found connecting node " + node + " and node " + nextNode + " for route " + this.route);
252 }
253
254
255 @Override
256 public final Route getRoute()
257 {
258 return this.route;
259 }
260
261
262 @Override
263 public final String toString()
264 {
265 return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner="
266 + this.fixedTacticalPlanner + "]";
267 }
268
269 }