1 package org.opentrafficsim.core.geometry;
2
3 import org.djunits.unit.AngleUnit;
4 import org.djunits.unit.LengthUnit;
5 import org.djunits.unit.LinearDensityUnit;
6 import org.djunits.value.vdouble.scalar.Angle;
7 import org.djunits.value.vdouble.scalar.Length;
8 import org.djunits.value.vdouble.scalar.LinearDensity;
9
10 /**
11 * Generate an OTSLine3D for a clothoid. <br>
12 * Derived from odrSpiral.c by M. Dupuis @ VIRES GmbH <br>
13 *
14 * <pre>
15 * Licensed under the Apache License, Version 2.0 (the "License");
16 * you may not use this file except in compliance with the License.
17 * You may obtain a copy of the License at
18 *
19 * http://www.apache.org/licenses/LICENSE-2.0
20 *
21 * Unless required by applicable law or agreed to in writing, software
22 * distributed under the License is distributed on an "AS IS" BASIS,
23 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
24 * See the License for the specific language governing permissions and
25 * limitations under the License.
26 * </pre>
27 * <p>
28 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
29 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
30 * <p>
31 * @version $Revision$, $LastChangedDate$, by $Author$, initial version Nov 2, 2015 <br>
32 * @author M. Dupuis @ VIRES GmbH
33 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
34 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
35 */
36 public final class Clothoid
37 {
38 /** Utility class. */
39 private Clothoid()
40 {
41 // do not instantiate
42 }
43
44 /*- ===================================================
45 * file: odrSpiral.c
46 * ---------------------------------------------------
47 * purpose: free method for computing spirals
48 * in OpenDRIVE applications
49 * ---------------------------------------------------
50 * using methods of CEPHES library
51 * ---------------------------------------------------
52 * first edit: 09.03.2010 by M. Dupuis @ VIRES GmbH
53 * last mod.: 09.03.2010 by M. Dupuis @ VIRES GmbH
54 * ===================================================
55 Copyright 2010 VIRES Simulationstechnologie GmbH
56
57 Licensed under the Apache License, Version 2.0 (the "License");
58 you may not use this file except in compliance with the License.
59 You may obtain a copy of the License at
60
61 http://www.apache.org/licenses/LICENSE-2.0
62
63 Unless required by applicable law or agreed to in writing, software
64 distributed under the License is distributed on an "AS IS" BASIS,
65 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
66 See the License for the specific language governing permissions and
67 limitations under the License.
68
69
70 NOTE:
71 The methods have been realized using the CEPHES library
72
73 http://www.netlib.org/cephes/
74
75 and do neither constitute the only nor the exclusive way of implementing
76 spirals for OpenDRIVE applications. Their sole purpose is to facilitate
77 the interpretation of OpenDRIVE spiral data.
78 */
79
80 //@formatter:off
81 /** S(x) for small x numerator. */
82 static final double[] SN = {
83 -2.99181919401019853726E3,
84 7.08840045257738576863E5,
85 -6.29741486205862506537E7,
86 2.54890880573376359104E9,
87 -4.42979518059697779103E10,
88 3.18016297876567817986E11
89 };
90
91 /** S(x) for small x denominator. */
92 static final double[] SD = {
93 2.81376268889994315696E2,
94 4.55847810806532581675E4,
95 5.17343888770096400730E6,
96 4.19320245898111231129E8,
97 2.24411795645340920940E10,
98 6.07366389490084639049E11
99 };
100
101 /** C(x) for small x numerator. */
102 static final double[] CN = {
103 -4.98843114573573548651E-8,
104 9.50428062829859605134E-6,
105 -6.45191435683965050962E-4,
106 1.88843319396703850064E-2,
107 -2.05525900955013891793E-1,
108 9.99999999999999998822E-1
109 };
110
111 /** C(x) for small x denominator. */
112 static final double[] CD = {
113 3.99982968972495980367E-12,
114 9.15439215774657478799E-10,
115 1.25001862479598821474E-7,
116 1.22262789024179030997E-5,
117 8.68029542941784300606E-4,
118 4.12142090722199792936E-2,
119 1.00000000000000000118E0
120 };
121
122 /** Auxiliary function f(x) numerator. */
123 static final double[] FN = {
124 4.21543555043677546506E-1,
125 1.43407919780758885261E-1,
126 1.15220955073585758835E-2,
127 3.45017939782574027900E-4,
128 4.63613749287867322088E-6,
129 3.05568983790257605827E-8,
130 1.02304514164907233465E-10,
131 1.72010743268161828879E-13,
132 1.34283276233062758925E-16,
133 3.76329711269987889006E-20
134 };
135
136 /** Auxiliary function f(x) denominator. */
137 static final double[] FD = {
138 7.51586398353378947175E-1,
139 1.16888925859191382142E-1,
140 6.44051526508858611005E-3,
141 1.55934409164153020873E-4,
142 1.84627567348930545870E-6,
143 1.12699224763999035261E-8,
144 3.60140029589371370404E-11,
145 5.88754533621578410010E-14,
146 4.52001434074129701496E-17,
147 1.25443237090011264384E-20
148 };
149
150 /** Auxiliary function g(x) numerator. */
151 static final double[] GN = {
152 5.04442073643383265887E-1,
153 1.97102833525523411709E-1,
154 1.87648584092575249293E-2,
155 6.84079380915393090172E-4,
156 1.15138826111884280931E-5,
157 9.82852443688422223854E-8,
158 4.45344415861750144738E-10,
159 1.08268041139020870318E-12,
160 1.37555460633261799868E-15,
161 8.36354435630677421531E-19,
162 1.86958710162783235106E-22
163 };
164
165 /** Auxiliary function g(x) denominator. */
166 static final double[] GD = {
167 1.47495759925128324529E0,
168 3.37748989120019970451E-1,
169 2.53603741420338795122E-2,
170 8.14679107184306179049E-4,
171 1.27545075667729118702E-5,
172 1.04314589657571990585E-7,
173 4.60680728146520428211E-10,
174 1.10273215066240270757E-12,
175 1.38796531259578871258E-15,
176 8.39158816283118707363E-19,
177 1.86958710162783236342E-22
178 };
179 //@formatter:on
180
181 /**
182 * Compute a polynomial in x.
183 * @param x double; x
184 * @param coef double[]; coefficients
185 * @return polynomial in x
186 */
187 private static double polevl(final double x, final double[] coef)
188 {
189 double result = coef[0];
190 for (int i = 0; i < coef.length; i++)
191 {
192 result = result * x + coef[i];
193 }
194 return result;
195 }
196
197 /**
198 * Compute a polynomial in x.
199 * @param x double; x
200 * @param coef double[]; coefficients
201 * @return polynomial in x
202 */
203 private static double p1evl(final double x, final double[] coef)
204 {
205 double result = x + coef[0];
206 for (int i = 0; i < coef.length; i++)
207 {
208 result = result * x + coef[i];
209 }
210 return result;
211 }
212
213 /**
214 * Approximate the Fresnel function.
215 * @param xxa double; the xxa parameter
216 * @return double[]; array with two double values c and s
217 */
218 private static double[] fresnel(final double xxa)
219 {
220 final double x = Math.abs(xxa);
221 final double x2 = x * x;
222 double cc, ss;
223
224 if (x2 < 2.5625)
225 {
226 final double t = x2 * x2;
227 ss = x * x2 * polevl(t, SN) / p1evl(t, SD);
228 cc = x * polevl(t, CN) / polevl(t, CD);
229 }
230 else if (x > 36974.0)
231 {
232 cc = 0.5;
233 ss = 0.5;
234 }
235 else
236 {
237 double t = Math.PI * x2;
238 final double u = 1.0 / (t * t);
239 t = 1.0 / t;
240 final double f = 1.0 - u * polevl(u, FN) / p1evl(u, FD);
241 final double g = t * polevl(u, GN) / p1evl(u, GD);
242
243 t = Math.PI * 0.5 * x2;
244 final double c = Math.cos(t);
245 final double s = Math.sin(t);
246 t = Math.PI * x;
247 cc = 0.5 + (f * s - g * c) / t;
248 ss = 0.5 - (f * c + g * s) / t;
249 }
250
251 if (xxa < 0.0)
252 {
253 cc = -cc;
254 ss = -ss;
255 }
256 return new double[]{cc, ss};
257 }
258
259 /**
260 * Approximate one point of the "standard" spiral (curvature at start is 0).
261 * @param s run-length along spiral
262 * @param cDot first derivative of curvature [1/m2]
263 * @param initialCurvature double; curvature at start
264 * @return double[]; array of three double values containing x, y, and tangent direction
265 */
266 private static double[] odrSpiral(final double s, final double cDot, final double initialCurvature)
267 {
268 double a = Math.sqrt(Math.PI / Math.abs(cDot));
269
270 double[] xy = fresnel(initialCurvature + s / a);
271 return new double[]{xy[0] * a, xy[1] * a * Math.signum(cDot), s * s * cDot * 0.5};
272 }
273
274 /**
275 * Approximate a clothoid that starts in the x-direction.
276 * @param initialCurvature double; curvature at start
277 * @param curvatureDerivative double; rate of curvature change along the clothoid
278 * @param length double; total length of the clothoid
279 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
280 * @return OTSLine3D; the line; the z-component of each point is set to 0
281 * @throws OTSGeometryException if the number of segments is too low
282 */
283 private static OTSLine3D clothoid(final double initialCurvature, final double curvatureDerivative,
284 final double length, final int numSegments) throws OTSGeometryException
285 {
286 OTSPoint3D[] points = new OTSPoint3D[numSegments + 1];
287 double[] offset = odrSpiral(initialCurvature / curvatureDerivative, curvatureDerivative, initialCurvature);
288 double sinRot = Math.sin(offset[2]);
289 double cosRot = Math.cos(offset[2]);
290 for (int i = 0; i <= numSegments; i++)
291 {
292 double[] xyd =
293 odrSpiral(i * length / numSegments + initialCurvature / curvatureDerivative, curvatureDerivative,
294 initialCurvature);
295 double dx = xyd[0] - offset[0];
296 double dy = xyd[1] - offset[1];
297 points[i] = new OTSPoint3D(dx * cosRot + dy * sinRot, dy * cosRot - dx * sinRot, 0);
298 }
299 return new OTSLine3D(points);
300 }
301
302 /**
303 * Approximate a clothoid that starts at a given point in the given direction and curvature. Elevation is linearly
304 * interpolated over the length of the clothoid.
305 * @param x1 double; x-coordinate of the start point
306 * @param y1 double; y-coordinate of the start point
307 * @param startElevation double; z-component at start of the curve
308 * @param startDirection double; rotation in radians at the start of the curve
309 * @param startCurvature double; curvature at the start of the clothoid
310 * @param endCurvature double; curvature at the end of the clothoid
311 * @param length double; length of the clothoid
312 * @param endElevation double; z-component at end of the curve
313 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
314 * @return OTSLine3D; the clothoid
315 * @throws OTSGeometryException if the number of segments is too low
316 */
317 @SuppressWarnings("checkstyle:parameternumber")
318 private static OTSLine3D clothoid(final double x1, final double y1, final double startElevation,
319 final double startDirection, final double startCurvature, final double endCurvature, final double length,
320 final double endElevation, final int numSegments) throws OTSGeometryException
321 {
322 OTSLine3D result = clothoid(startCurvature, (endCurvature - startCurvature) / length, length, numSegments);
323 double sinRot = Math.sin(startDirection);
324 double cosRot = Math.cos(startDirection);
325 OTSPoint3D[] list = new OTSPoint3D[result.size()];
326 double elevationPerStep = (endElevation - startElevation) / (result.size() - 1);
327 for (int i = 0; i < result.size(); i++)
328 {
329 try
330 {
331 OTSPoint3D p = result.get(i);
332 list[i] =
333 new OTSPoint3D(x1 + cosRot * p.x + sinRot * p.y, y1 + cosRot * p.y - sinRot * p.x, startElevation
334 + i * elevationPerStep);
335 }
336 catch (OTSGeometryException ge)
337 {
338 // cannot happen
339 System.err.println("CANNOT HAPPEN; if you see this; let us know what you did.");
340 }
341 }
342 return new OTSLine3D(list);
343 }
344
345 /**
346 * Approximate a clothoid with curvature 0 at start.
347 * @param start OTSPoint3D; starting point of the clothoid
348 * @param startDirection Angle.Abs; start direction of the clothoid
349 * @param endCurvature double; curvature at the end of the clothoid [1/m]
350 * @param length Length.Rel; length of the clothoid
351 * @param endElevation Length.Rel; elevation at end of the clothoid
352 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
353 * @return OTSLine3D; the clothoid
354 * @throws OTSGeometryException if the number of segments is too low
355 */
356 public static OTSLine3D clothoid(final OTSPoint3D start, final Angle.Abs startDirection, final double endCurvature,
357 final Length.Rel length, final Length.Rel endElevation, final int numSegments) throws OTSGeometryException
358 {
359 return clothoid(start.x, start.y, start.z, startDirection.si, 0, endCurvature, length.si, endElevation.si,
360 numSegments);
361 }
362
363 /**
364 * Approximate a clothoid.
365 * @param start OTSPoint3D; starting point of the clothoid
366 * @param startDirection Angle.Abs; start direction of the clothoid
367 * @param startCurvature double; curvature at the start of the clothoid [1/m]
368 * @param endCurvature double; curvature at the end of the clothoid [1/m]
369 * @param length Length.Rel; length of the clothoid
370 * @param endElevation Length.Rel; elevation at end of the clothoid
371 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
372 * @return OTSLine3D; the clothoid
373 * @throws OTSGeometryException if the number of segments is too low
374 */
375 public static OTSLine3D clothoid(final OTSPoint3D start, final Angle.Abs startDirection,
376 final double startCurvature, final double endCurvature, final Length.Rel length, final Length.Rel endElevation,
377 final int numSegments) throws OTSGeometryException
378 {
379 return clothoid(start.x, start.y, start.x, startDirection.si, startCurvature, endCurvature, length.si,
380 endElevation.si, numSegments);
381 }
382
383 /**
384 * Approximate a clothoid with curvature 0 at start.
385 * @param start OTSPoint3D; starting point of the clothoid
386 * @param startDirection Angle.Abs; start direction of the clothoid
387 * @param endCurvature LinearDensity; curvature at the end of the clothoid
388 * @param length Length.Rel; length of the clothoid
389 * @param endElevation Length.Rel; elevation at end of the clothoid
390 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
391 * @return OTSLine3D; the clothoid
392 * @throws OTSGeometryException if the number of segments is too low
393 */
394 public static OTSLine3D
395 clothoid(final OTSPoint3D start, final Angle.Abs startDirection, final LinearDensity endCurvature,
396 final Length.Rel length, final Length.Rel endElevation, final int numSegments) throws OTSGeometryException
397 {
398 return clothoid(start, startDirection, 0, endCurvature.si, length, endElevation, numSegments);
399 }
400
401 /**
402 * Approximate a clothoid.
403 * @param start OTSPoint3D; starting point of the clothoid
404 * @param startDirection Angle.Abs; start direction of the clothoid
405 * @param startCurvature double; curvature at the start of the clothoid [1/m]
406 * @param endCurvature double; curvature at the end of the clothoid [1/m]
407 * @param length Length.Rel; length of the clothoid
408 * @param endElevation Length.Rel; elevation at end of the clothoid
409 * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
410 * @return OTSLine3D; the clothoid
411 * @throws OTSGeometryException if the number of segments is too low
412 */
413 public static OTSLine3D clothoid(final OTSPoint3D start, final Angle.Abs startDirection,
414 final LinearDensity startCurvature, final LinearDensity endCurvature, final Length.Rel length,
415 final Length.Rel endElevation, final int numSegments) throws OTSGeometryException
416 {
417 return clothoid(start, startDirection, startCurvature.si, endCurvature.si, length, endElevation, numSegments);
418 }
419
420 /**
421 * Demonstrate / test the clothoid methods.
422 * @param args String[]; the command line arguments (not used)
423 * @throws OTSGeometryException if the number of segments is too low
424 */
425 public static void main(final String[] args) throws OTSGeometryException
426 {
427 OTSLine3D line;
428 // line = clothoid(104.1485, 89.037488, 0, 0, 0, -0.04841457, 0, 3.2, 100);
429 // System.out.println(line.toPlotterFormat());
430 // line = clothoid(10, 10, 5, Math.PI / 8, 0 * -0.03, 0.04, 100, 15, 100);
431 line =
432 clothoid(new OTSPoint3D(10, 10, 5), new Angle.Abs(Math.PI / 8, AngleUnit.RADIAN), new LinearDensity(
433 0 * -0.03, LinearDensityUnit.PER_METER), new LinearDensity(0.04, LinearDensityUnit.PER_METER),
434 new Length.Rel(100, LengthUnit.METER), new Length.Rel(15, LengthUnit.METER), 100);
435 System.out.println(OTSGeometryUtil.printCoordinates("#", line, "\n"));
436 }
437
438 }