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