1 /* ===================================================
2 * file: euler.h
3 * ---------------------------------------------------
4 * purpose: free method for computing spirals
5 * in OpenDRIVE applications
6 * ---------------------------------------------------
7 * using methods of CEPHES library
8 * ---------------------------------------------------
9 * first edit: 09.03.2010 by M. Dupuis @ VIRES GmbH
10 * last mod.: 02.05.2017 by Michael Scholz @ German Aerospace Center (DLR)
11 * last mod.: 05.07.2017 by Jakob Erdmann @ German Aerospace Center (DLR)
12 * ===================================================
13 Copyright 2010 VIRES Simulationstechnologie GmbH
14 Copyright 2017 German Aerospace Center (DLR)
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 http://www.apache.org/licenses/LICENSE-2.0
19 Unless required by applicable law or agreed to in writing, software
20 distributed under the License is distributed on an "AS IS" BASIS,
21 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 See the License for the specific language governing permissions and
23 limitations under the License.
24
25
26 NOTE:
27 The methods have been realized using the CEPHES library
28 http://www.netlib.org/cephes/
29 and do neither constitute the only nor the exclusive way of implementing
30 spirals for OpenDRIVE applications. Their sole purpose is to facilitate
31 the interpretation of OpenDRIVE spiral data.
32 */
33
34 /* ====== INCLUSIONS ====== */
35 #include <stdio.h>
36 #include <math.h>
37
38 /* ====== LOCAL VARIABLES ====== */
39 #ifndef M_PI
40 #define M_PI 3.1415926535897932384626433832795
41 #endif
42
43 /* S(x) for small x */
44 static double sn[6] = {
45 -2.99181919401019853726E3,
46 7.08840045257738576863E5,
47 -6.29741486205862506537E7,
48 2.54890880573376359104E9,
49 -4.42979518059697779103E10,
50 3.18016297876567817986E11,
51 };
52 static double sd[6] = {
53 /* 1.00000000000000000000E0,*/
54 2.81376268889994315696E2,
55 4.55847810806532581675E4,
56 5.17343888770096400730E6,
57 4.19320245898111231129E8,
58 2.24411795645340920940E10,
59 6.07366389490084639049E11,
60 };
61
62 /* C(x) for small x */
63 static double cn[6] = {
64 -4.98843114573573548651E-8,
65 9.50428062829859605134E-6,
66 -6.45191435683965050962E-4,
67 1.88843319396703850064E-2,
68 -2.05525900955013891793E-1,
69 9.99999999999999998822E-1,
70 };
71 static double cd[7] = {
72 3.99982968972495980367E-12,
73 9.15439215774657478799E-10,
74 1.25001862479598821474E-7,
75 1.22262789024179030997E-5,
76 8.68029542941784300606E-4,
77 4.12142090722199792936E-2,
78 1.00000000000000000118E0,
79 };
80
81 /* Auxiliary function f(x) */
82 static double fn[10] = {
83 4.21543555043677546506E-1,
84 1.43407919780758885261E-1,
85 1.15220955073585758835E-2,
86 3.45017939782574027900E-4,
87 4.63613749287867322088E-6,
88 3.05568983790257605827E-8,
89 1.02304514164907233465E-10,
90 1.72010743268161828879E-13,
91 1.34283276233062758925E-16,
92 3.76329711269987889006E-20,
93 };
94 static double fd[10] = {
95 /* 1.00000000000000000000E0,*/
96 7.51586398353378947175E-1,
97 1.16888925859191382142E-1,
98 6.44051526508858611005E-3,
99 1.55934409164153020873E-4,
100 1.84627567348930545870E-6,
101 1.12699224763999035261E-8,
102 3.60140029589371370404E-11,
103 5.88754533621578410010E-14,
104 4.52001434074129701496E-17,
105 1.25443237090011264384E-20,
106 };
107
108 /* Auxiliary function g(x) */
109 static double gn[11] = {
110 5.04442073643383265887E-1,
111 1.97102833525523411709E-1,
112 1.87648584092575249293E-2,
113 6.84079380915393090172E-4,
114 1.15138826111884280931E-5,
115 9.82852443688422223854E-8,
116 4.45344415861750144738E-10,
117 1.08268041139020870318E-12,
118 1.37555460633261799868E-15,
119 8.36354435630677421531E-19,
120 1.86958710162783235106E-22,
121 };
122 static double gd[11] = {
123 /* 1.00000000000000000000E0,*/
124 1.47495759925128324529E0,
125 3.37748989120019970451E-1,
126 2.53603741420338795122E-2,
127 8.14679107184306179049E-4,
128 1.27545075667729118702E-5,
129 1.04314589657571990585E-7,
130 4.60680728146520428211E-10,
131 1.10273215066240270757E-12,
132 1.38796531259578871258E-15,
133 8.39158816283118707363E-19,
134 1.86958710162783236342E-22,
135 };
136
137
polevl(double x,double * coef,int n)138 static double polevl( double x, double* coef, int n )
139 {
140 double ans;
141 double *p = coef;
142 int i;
143
144 ans = *p++;
145 i = n;
146
147 do
148 {
149 ans = ans * x + *p++;
150 }
151 while (--i);
152
153 return ans;
154 }
155
p1evl(double x,double * coef,int n)156 static double p1evl( double x, double* coef, int n )
157 {
158 double ans;
159 double *p = coef;
160 int i;
161
162 ans = x + *p++;
163 i = n - 1;
164
165 do
166 {
167 ans = ans * x + *p++;
168 }
169 while (--i);
170
171 return ans;
172 }
173
174
fresnel(double xxa,double * ssa,double * cca)175 static void fresnel( double xxa, double *ssa, double *cca )
176 {
177 double f, g, cc, ss, c, s, t, u;
178 double x, x2;
179
180 x = fabs( xxa );
181 x2 = x * x;
182
183 if ( x2 < 2.5625 )
184 {
185 t = x2 * x2;
186 ss = x * x2 * polevl (t, sn, 5) / p1evl (t, sd, 6);
187 cc = x * polevl (t, cn, 5) / polevl (t, cd, 6);
188 }
189 else if ( x > 36974.0 )
190 {
191 cc = 0.5;
192 ss = 0.5;
193 }
194 else
195 {
196 x2 = x * x;
197 t = M_PI * x2;
198 u = 1.0 / (t * t);
199 t = 1.0 / t;
200 f = 1.0 - u * polevl (u, fn, 9) / p1evl(u, fd, 10);
201 g = t * polevl (u, gn, 10) / p1evl (u, gd, 11);
202
203 t = M_PI * 0.5 * x2;
204 c = cos (t);
205 s = sin (t);
206 t = M_PI * x;
207 cc = 0.5 + (f * s - g * c) / t;
208 ss = 0.5 - (f * c + g * s) / t;
209 }
210
211 if ( xxa < 0.0 )
212 {
213 cc = -cc;
214 ss = -ss;
215 }
216
217 *cca = cc;
218 *ssa = ss;
219 }
220
221
222 /**
223 * compute the actual "standard" spiral, starting with curvature 0
224 * @param s run-length along spiral
225 * @param cDot first derivative of curvature [1/m2]
226 * @param x resulting x-coordinate in spirals local co-ordinate system [m]
227 * @param y resulting y-coordinate in spirals local co-ordinate system [m]
228 * @param t tangent direction at s [rad]
229 */
230
odrSpiral(double s,double cDot,double * x,double * y,double * t)231 void odrSpiral( double s, double cDot, double *x, double *y, double *t )
232 {
233 double a;
234
235 a = 1.0 / sqrt( fabs( cDot ) );
236 a *= sqrt( M_PI );
237
238 fresnel( s / a, y, x );
239
240 *x *= a;
241 *y *= a;
242
243 if ( cDot < 0.0 )
244 *y *= -1.0;
245
246 *t = s * s * cDot * 0.5;
247 }
248