1 package org.opentrafficsim.road.network.factory.opendrive;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.Collections;
6 import java.util.List;
7 import java.util.NavigableMap;
8 import java.util.UUID;
9
10 import org.djunits.unit.LengthUnit;
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.opentrafficsim.core.geometry.OTSGeometryException;
13 import org.opentrafficsim.core.geometry.OTSLine3D;
14 import org.opentrafficsim.core.geometry.OTSPoint3D;
15 import org.opentrafficsim.core.network.NetworkException;
16 import org.w3c.dom.Node;
17 import org.w3c.dom.NodeList;
18 import org.xml.sax.SAXException;
19
20
21
22
23
24
25
26
27
28
29 class PlanViewTag implements Serializable
30 {
31
32
33 private static final long serialVersionUID = 20150723L;
34
35
36 @SuppressWarnings("checkstyle:visibilitymodifier")
37 List<GeometryTag> geometryTags = new ArrayList<GeometryTag>();
38
39
40
41
42
43
44
45
46
47
48 @SuppressWarnings("checkstyle:needbraces")
49 static void parsePlanView(final NodeList nodeList, final OpenDriveNetworkLaneParser parser, final RoadTag roadTag)
50 throws SAXException, OTSGeometryException, NetworkException
51 {
52 int geometryCount = 0;
53 PlanViewTag planViewTag = new PlanViewTag();
54 roadTag.planViewTag = planViewTag;
55
56 for (Node node0 : XMLParser.getNodes(nodeList, "planView"))
57 {
58 for (Node node : XMLParser.getNodes(node0.getChildNodes(), "geometry"))
59 {
60
61 GeometryTag geometryTag = GeometryTag.parseGeometry(node, parser);
62 geometryTag.id = roadTag.id + "." + String.valueOf(geometryCount);
63 geometryCount++;
64
65 geometryTag.z = assignHeight(geometryTag, roadTag.elevationProfileTag.elevationTags);
66
67 GeometryTag.makeOTSNode(parser.network, geometryTag);
68
69 planViewTag.geometryTags.add(geometryTag);
70 }
71
72 GeometryTag lastGeometryTag = new GeometryTag();
73
74 GeometryTag previousTag = planViewTag.geometryTags.get(planViewTag.geometryTags.size() - 1);
75
76 lastGeometryTag.id = roadTag.id + "." + String.valueOf(geometryCount);
77
78 lastGeometryTag.length = new Length(0.0, LengthUnit.METER);
79
80
81 lastGeometryTag.s = roadTag.length;
82
83 lastGeometryTag.x = new Length(0.0, LengthUnit.METER);
84
85 lastGeometryTag.x = previousTag.x.plus(previousTag.length.multiplyBy(Math.cos(previousTag.hdg.si)));
86
87 lastGeometryTag.y = new Length(0.0, LengthUnit.METER);
88
89 lastGeometryTag.y = previousTag.y.plus(previousTag.length.multiplyBy(Math.sin(previousTag.hdg.si)));
90
91 lastGeometryTag.hdg = previousTag.hdg;
92
93 lastGeometryTag.z = previousTag.z;
94
95 GeometryTag.makeOTSNode(parser.network, lastGeometryTag);
96
97 planViewTag.geometryTags.add(lastGeometryTag);
98
99 geometryCount = 0;
100 for (GeometryTag geometryTag : planViewTag.geometryTags)
101 {
102 if (geometryTag.spiralTag != null)
103 interpolateSpiral(parser, planViewTag, geometryTag, geometryCount);
104 if (geometryTag.arcTag != null)
105 interpolateArc(planViewTag, geometryTag, geometryCount, roadTag);
106 geometryCount++;
107 }
108 }
109 roadTag.designLine = buildDesignLine(planViewTag, roadTag);
110
111 }
112
113
114
115
116
117
118 private static Length assignHeight(GeometryTag geometryTag, NavigableMap<Double, ElevationTag> elevationTags)
119 {
120 Double key = geometryTag.s.si;
121 if (elevationTags.containsKey(key))
122 return elevationTags.get(key).elevation;
123 else
124 {
125 Double before = elevationTags.floorKey(key);
126 Double after = elevationTags.ceilingKey(key);
127
128 if (after == null)
129 return elevationTags.get(before).elevation;
130
131 Double factor = (key - before) / (after - before);
132
133 Length beHeight = elevationTags.get(before).elevation;
134 Length afHeight = elevationTags.get(after).elevation;
135
136 return afHeight.minus(beHeight).multiplyBy(factor).plus(beHeight);
137 }
138 }
139
140
141
142
143
144
145
146
147 private static void interpolateSpiral(OpenDriveNetworkLaneParser parser, PlanViewTag planViewTag, GeometryTag geometryTag,
148 int geometryCount) throws OTSGeometryException
149 {
150 double startCurvature = geometryTag.spiralTag.curvStart.doubleValue();
151 double endCurvature = geometryTag.spiralTag.curvEnd.doubleValue();
152 OTSPoint3D start = geometryTag.node.getPoint();
153 Length length = geometryTag.length;
154
155
156
157
158
159
160
161 List<OTSPoint3D> pOutPut = new ArrayList<OTSPoint3D>();
162
163 Length x1 = geometryTag.x.plus(geometryTag.length.multiplyBy(Math.cos(geometryTag.hdg.si)).multiplyBy(0.5));
164 Length y1 = geometryTag.y.plus(geometryTag.length.multiplyBy(Math.sin(geometryTag.hdg.si)).multiplyBy(0.5));
165
166 OTSPoint3D p = new OTSPoint3D(x1.si, y1.si, 0);
167 pOutPut.add(p);
168
169 Length x2 = geometryTag.x.plus(geometryTag.length.multiplyBy(Math.cos(geometryTag.hdg.si)).multiplyBy(0.66));
170 Length y2 = geometryTag.y.plus(geometryTag.length.multiplyBy(Math.sin(geometryTag.hdg.si)).multiplyBy(0.66));
171
172 OTSPoint3D q = new OTSPoint3D(x2.si, y2.si, 0);
173 pOutPut.add(q);
174
175 OTSLine3D otsLine = new OTSLine3D(pOutPut);
176
177
178
179 }
180
181
182
183
184
185
186
187
188 private static void interpolateArc(PlanViewTag planViewTag, GeometryTag geometryTag, int geometryCount, RoadTag roadTag)
189 throws OTSGeometryException
190 {
191 double curvature = geometryTag.arcTag.curvature.doubleValue();
192 OTSPoint3D start = geometryTag.node.getPoint();
193 OTSPoint3D end = planViewTag.geometryTags.get(geometryCount + 1).node.getPoint();
194
195 Length length = geometryTag.length;
196
197 double radius = 1 / curvature;
198 boolean side = false;
199
200 List<OTSPoint3D> line = null;
201
202 if (curvature < 0)
203 {
204
205 line = generateCurve(end, start, Math.abs(radius), 0.05, true, side);
206 Collections.reverse(line);
207 }
208 else
209 {
210 line = generateCurve(start, end, Math.abs(radius), 0.05, true, side);
211 }
212
213 OTSLine3D otsLine = new OTSLine3D(line);
214
215 geometryTag.interLine = otsLine;
216
217 }
218
219
220
221
222
223
224
225
226
227
228 private static List<OTSPoint3D> generateCurve(OTSPoint3D pFrom, OTSPoint3D pTo, double pRadius, double pMinDistance,
229 boolean shortest, boolean side)
230 {
231
232 List<OTSPoint3D> pOutPut = new ArrayList<OTSPoint3D>();
233
234
235 OTSPoint3D mPoint = new OTSPoint3D((pFrom.x + pTo.x) / 2, (pFrom.y + pTo.y) / 2, (pFrom.z + pTo.z) / 2);
236
237
238
239
240 double xDiff = pTo.x - pFrom.x;
241 double yDiff = pTo.y - pFrom.y;
242 double distance = Math.sqrt(xDiff * xDiff + yDiff * yDiff);
243
244
245 if (pRadius * 2.0f < distance)
246 {
247 throw new IllegalArgumentException("The radius is too small! The given points wont fall on the circle.");
248 }
249
250
251 double factor = Math.sqrt(
252 (pRadius * pRadius) / ((pTo.x - pFrom.x) * (pTo.x - pFrom.x) + (pTo.y - pFrom.y) * (pTo.y - pFrom.y)) - 0.25f);
253 double x = 0;
254 double y = 0;
255 if (side)
256 {
257 x = 0.5f * (pFrom.x + pTo.x) + factor * (pTo.y - pFrom.y);
258 y = 0.5f * (pFrom.y + pTo.y) + factor * (pFrom.x - pTo.x);
259 }
260 else
261 {
262 x = 0.5f * (pFrom.x + pTo.x) - factor * (pTo.y - pFrom.y);
263 y = 0.5f * (pFrom.y + pTo.y) - factor * (pFrom.x - pTo.x);
264 }
265 OTSPoint3D circleMiddlePoint = new OTSPoint3D(x, y, 0);
266
267
268
269
270 double angle1 = Math.atan2(pFrom.y - circleMiddlePoint.y, pFrom.x - circleMiddlePoint.x);
271 double angle2 = Math.atan2(pTo.y - circleMiddlePoint.y, pTo.x - circleMiddlePoint.x);
272
273
274 double step = pMinDistance / pRadius;
275
276
277
278 if (angle1 > angle2)
279 {
280 double temp = angle1;
281 angle1 = angle2;
282 angle2 = temp;
283
284 }
285
286 if ((pTo.x - circleMiddlePoint.x < 0) && (pTo.y - circleMiddlePoint.y < 0) && (pFrom.y - circleMiddlePoint.y > 0)
287 && (pFrom.x - circleMiddlePoint.x < 0))
288 {
289 double temp = angle1;
290 angle1 = angle2;
291 angle2 = temp;
292
293 angle1 = angle1 - Math.PI * 2;
294
295 }
296 else if ((pTo.x - circleMiddlePoint.x > 0) && (pTo.y - circleMiddlePoint.y < 0) && (pFrom.y - circleMiddlePoint.y > 0)
297 && (pFrom.x - circleMiddlePoint.x < 0))
298 {
299 double temp = angle1;
300 angle1 = angle2;
301 angle2 = temp;
302
303 angle1 = angle1 - Math.PI * 2;
304 }
305 else if ((pTo.x - circleMiddlePoint.x < 0) && (pTo.y - circleMiddlePoint.y < 0) && (pFrom.y - circleMiddlePoint.y > 0)
306 && (pFrom.x - circleMiddlePoint.x > 0))
307 {
308 double temp = angle1;
309 angle1 = angle2;
310 angle2 = temp;
311
312 angle1 = angle1 - Math.PI * 2;
313
314 }
315
316
317
318
319
320
321 boolean flipped = false;
322 if (!shortest)
323 {
324 if (angle2 - angle1 < Math.PI)
325 {
326 double temp = angle1;
327 angle1 = angle2;
328 angle2 = temp;
329 angle2 += Math.PI * 2.0f;
330 flipped = true;
331 }
332 }
333
334 double zStep = (pTo.z - pFrom.z) / ((angle2 - angle1) / step);
335 double zFirst = pFrom.z;
336 for (double f = angle1; f < angle2; f += step)
337 {
338 zFirst = zFirst + zStep;
339 OTSPoint3D p = new OTSPoint3D(Math.cos(f) * pRadius + circleMiddlePoint.x,
340 Math.sin(f) * pRadius + circleMiddlePoint.y, zFirst);
341
342 pOutPut.add(p);
343 }
344
345
346
347
348 return pOutPut;
349 }
350
351
352
353
354
355
356
357
358 static OTSLine3D buildDesignLine(final PlanViewTag planViewTag, final RoadTag roadTag) throws OTSGeometryException
359 {
360 int points = planViewTag.geometryTags.size();
361
362 if (points < 2)
363 System.err.println("No enough nodes");
364
365 GeometryTag from = planViewTag.geometryTags.get(0);
366 GeometryTag to = planViewTag.geometryTags.get(points - 1);
367
368 List<OTSPoint3D> coordinates = new ArrayList<OTSPoint3D>();
369
370 for (GeometryTag geometryTag : planViewTag.geometryTags)
371 {
372
373
374 if (coordinates.size() == 0)
375 {
376 coordinates.add(geometryTag.node.getPoint());
377 }
378 else
379 {
380 if (geometryTag.node.getPoint().x != coordinates.get(coordinates.size() - 1).x
381 && geometryTag.node.getPoint().y != coordinates.get(coordinates.size() - 1).y)
382 {
383 coordinates.add(geometryTag.node.getPoint());
384 }
385 }
386 OTSPoint3D lastPoint = new OTSPoint3D(coordinates.get(coordinates.size() - 1));
387
388 if (geometryTag.interLine != null)
389 {
390 for (OTSPoint3D point : geometryTag.interLine.getPoints())
391 {
392
393
394
395
396 if (lastPoint.x != point.x && lastPoint.y != point.y)
397 {
398 coordinates.add(point);
399 lastPoint = point;
400 }
401 }
402 }
403
404 }
405
406 OTSLine3D designLine = new OTSLine3D(coordinates);
407 if (roadTag.id == null)
408 {
409 roadTag.id = UUID.randomUUID().toString();
410 }
411 roadTag.startNode = from.node;
412 roadTag.endNode = to.node;
413
414
415 return designLine;
416
417 }
418
419
420 @Override
421 public final String toString()
422 {
423 return "PlanViewTag [geometryTags=" + this.geometryTags + "]";
424 }
425 }