Eclipse SUMO - Simulation of Urban MObility
odrSpiral.cpp
Go to the documentation of this file.
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 
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 
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 
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 
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 }
polevl
static double polevl(double x, double *coef, int n)
Definition: odrSpiral.cpp:138
fresnel
static void fresnel(double xxa, double *ssa, double *cca)
Definition: odrSpiral.cpp:175
p1evl
static double p1evl(double x, double *coef, int n)
Definition: odrSpiral.cpp:156
cn
static double cn[6]
Definition: odrSpiral.cpp:63
odrSpiral
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
Definition: odrSpiral.cpp:231
fd
static double fd[10]
Definition: odrSpiral.cpp:94
sd
static double sd[6]
Definition: odrSpiral.cpp:52
sn
static double sn[6]
Definition: odrSpiral.cpp:44
M_PI
#define M_PI
Definition: odrSpiral.cpp:40
gn
static double gn[11]
Definition: odrSpiral.cpp:109
fn
static double fn[10]
Definition: odrSpiral.cpp:82
gd
static double gd[11]
Definition: odrSpiral.cpp:122
cd
static double cd[7]
Definition: odrSpiral.cpp:71