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.gtu.GTUDirectionality;
8 import org.opentrafficsim.core.gtu.GTUException;
9 import org.opentrafficsim.core.gtu.GTUType;
10 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
11 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
12 import org.opentrafficsim.core.network.Link;
13 import org.opentrafficsim.core.network.LinkDirection;
14 import org.opentrafficsim.core.network.NetworkException;
15 import org.opentrafficsim.core.network.Node;
16 import org.opentrafficsim.core.network.route.Route;
17 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
18 import org.opentrafficsim.road.gtu.strategical.AbstractLaneBasedStrategicalPlanner;
19 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
20 import org.opentrafficsim.road.network.lane.CrossSectionElement;
21 import org.opentrafficsim.road.network.lane.CrossSectionLink;
22 import org.opentrafficsim.road.network.lane.Lane;
23
24 import nl.tudelft.simulation.language.Throw;
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40 public class LaneBasedStrategicalRoutePlanner extends AbstractLaneBasedStrategicalPlanner
41 implements 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 if (node.getLinks().size() == 1 && previousLink != null)
121 {
122
123 throw new NetworkException(
124 "LaneBasedStrategicalRoutePlanner is asked for a next link, but node " + node + " has no successors");
125 }
126 if (node.getLinks().size() == 1 && previousLink == null)
127 {
128
129 Link link = node.getLinks().iterator().next();
130 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
131 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
132 }
133 if (node.getLinks().size() == 2)
134 {
135
136
137
138
139 for (Link link : node.getLinks())
140 {
141 if (!link.equals(previousLink))
142 {
143 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
144 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
145 }
146 }
147 }
148
149
150 Set<Link> links = node.getLinks().toSet();
151 for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
152 {
153 Link link = linkIterator.next();
154 if (link.equals(previousLink))
155 {
156
157 linkIterator.remove();
158 }
159 else
160 {
161
162 if ((link.getStartNode().equals(node) && !link.getDirectionality(gtuType).isForwardOrBoth())
163 || (link.getEndNode().equals(node) && !link.getDirectionality(gtuType).isBackwardOrBoth()))
164 {
165 linkIterator.remove();
166 }
167 else
168 {
169
170 boolean out = false;
171 CrossSectionLink csLink = (CrossSectionLink) link;
172 for (CrossSectionElement cse : csLink.getCrossSectionElementList())
173 {
174 if (cse instanceof Lane)
175 {
176 Lane lane = (Lane) cse;
177 if ((link.getStartNode().equals(node) && lane.getDirectionality(gtuType).isForwardOrBoth())
178 || (link.getEndNode().equals(node) && lane.getDirectionality(gtuType).isBackwardOrBoth()))
179 {
180 out = true;
181 }
182 }
183 }
184 if (!out)
185 {
186 linkIterator.remove();
187 }
188 }
189 }
190 }
191
192 if (links.size() == 1)
193 {
194 Link link = links.iterator().next();
195 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
196 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
197 }
198
199
200 if (this.route == null)
201 {
202 throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
203 }
204 int i = this.route.getNodes().indexOf(node);
205 if (i == -1)
206 {
207 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
208 + ", but node " + node + " not in route " + this.route);
209 }
210 if (i == this.route.getNodes().size() - 1)
211 {
212 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
213 + ", but the GTU reached the last node for route " + this.route);
214 }
215 Node nextNode = this.route.getNode(i + 1);
216
217
218
219
220
221
222 for (Link link : links)
223 {
224 if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
225 {
226
227
228
229
230 return new LinkDirection(link, GTUDirectionality.DIR_MINUS);
231 }
232 if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
233 {
234
235
236
237
238 return new LinkDirection(link, GTUDirectionality.DIR_PLUS);
239 }
240 }
241 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
242 + previousLink.getId() + ", but no link could be found connecting node " + node + " and node " + nextNode
243 + " for route " + this.route);
244 }
245
246
247 @Override
248 public final Route getRoute()
249 {
250 return this.route;
251 }
252
253
254 @Override
255 public final String toString()
256 {
257 return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner=" + this.fixedTacticalPlanner
258 + "]";
259 }
260
261 }