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