SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
euler.cpp
Go to the documentation of this file.
1 /*
2 #include "vcl_iostream.h"
3 #include <vcl_fstream.h>
4 #include "vcl_complex.h"
5 #include "vcl_cmath.h"
6 */
7 
8 #include "points.h"
9 #include "BiArc.h"
10 #include "euler.h"
11 #include <utils/common/StdDefs.h>
12 #include <complex>
13 #include <iostream>
14 
15 #define eGamma 1e-8
16 
17 //global lookup table: This is only instantiated when required and cleaned upon program exit
19 
20 //ensures single instantiation of the global variable
22 {
23  if (! globalEulerSpiralLookupTable)
24  globalEulerSpiralLookupTable = new EulerSpiralLookupTable();
25 
27 }
28 
30 {
31 }
32 
34 {
35 }
36 
37 //delta theta values for the table (tells you about the accuracy of the lookup)
39 {
40  return _dt;
41 }
42 
44 {
45  return _theta[N];
46 }
47 
48 double EulerSpiralLookupTable::k0(double start_angle, double end_angle)
49 {
50  //assume it is already 0-2Pi
51  double sangle, eangle;
52 
53  if (start_angle>M_PI) sangle = start_angle-2*M_PI;
54  else sangle = start_angle;
55 
56  if (end_angle>M_PI) eangle = end_angle-2*M_PI;
57  else eangle = end_angle;
58 
59  //output bilinear interpolated data from the tables
60  int ilow, ihigh, jlow, jhigh;
61 
62  ilow = (int)floor((sangle+M_PI)/_dt);
63  ihigh = (int)ceil((sangle+M_PI)/_dt);
64 
65  jlow = (int)floor((eangle+M_PI)/_dt);
66  jhigh = (int)ceil((eangle+M_PI)/_dt);
67 
68  double slow = _theta[ilow];
69  double elow = _theta[jlow];
70 
71  double a = (sangle - slow)/_dt;
72  double b = (eangle - elow)/_dt;
73 
74  double k0 = (1-a)*(1-b)*ES_k0[ilow][jlow] + a*(1-b)*ES_k0[ihigh][jlow] +
75  (1-a)*b*ES_k0[ilow][jhigh] + a*b*ES_k0[ihigh][jhigh];
76 
77  return k0;
78 }
79 
80 double EulerSpiralLookupTable::k1(double start_angle, double end_angle)
81 {
82  //output bilinear interpolated data from the tables
83 
84  //assume it is already 0-2Pi
85  double sangle, eangle;
86 
87  if (start_angle>M_PI) sangle = start_angle-2*M_PI;
88  else sangle = start_angle;
89 
90  if (end_angle>M_PI) eangle = end_angle-2*M_PI;
91  else eangle = end_angle;
92 
93  //output bilinear interpolated data from the tables
94  int ilow, ihigh, jlow, jhigh;
95 
96  ilow = (int)floor((sangle+M_PI)/_dt);
97  ihigh = (int)ceil((sangle+M_PI)/_dt);
98 
99  jlow = (int)floor((eangle+M_PI)/_dt);
100  jhigh = (int)ceil((eangle+M_PI)/_dt);
101 
102  double slow = _theta[ilow];
103  double elow = _theta[jlow];
104 
105  double a = (sangle - slow)/_dt;
106  double b = (eangle - elow)/_dt;
107 
108  double k1 = (1-a)*(1-b)*ES_k1[ilow][jlow] + a*(1-b)*ES_k1[ihigh][jlow] +
109  (1-a)*b*ES_k1[ilow][jhigh] + a*b*ES_k1[ihigh][jhigh];
110 
111  return k1;
112 }
113 
114 double EulerSpiralLookupTable::gamma(double start_angle, double end_angle)
115 {
116  //output bilinear interpolated data from the tables
117 
118  //assume it is already 0-2Pi
119  double sangle, eangle;
120 
121  if (start_angle>M_PI) sangle = start_angle-2*M_PI;
122  else sangle = start_angle;
123 
124  if (end_angle>M_PI) eangle = end_angle-2*M_PI;
125  else eangle = end_angle;
126 
127  //output bilinear interpolated data from the tables
128  int ilow, ihigh, jlow, jhigh;
129 
130  ilow = (int)floor((sangle+M_PI)/_dt);
131  ihigh = (int)ceil((sangle+M_PI)/_dt);
132 
133  jlow = (int)floor((eangle+M_PI)/_dt);
134  jhigh = (int)ceil((eangle+M_PI)/_dt);
135 
136  double slow = _theta[ilow];
137  double elow = _theta[jlow];
138 
139  double a = (sangle - slow)/_dt;
140  double b = (eangle - elow)/_dt;
141 
142  double gamma = (1-a)*(1-b)*ES_gamma[ilow][jlow] + a*(1-b)*ES_gamma[ihigh][jlow] +
143  (1-a)*b*ES_gamma[ilow][jhigh] + a*b*ES_gamma[ihigh][jhigh];
144 
145  return gamma;
146 }
147 
148 double EulerSpiralLookupTable::L(double start_angle, double end_angle)
149 {
150  //output bilinear interpolated data from the tables
151 
152  //assume it is already 0-2Pi
153  double sangle, eangle;
154 
155  if (start_angle>M_PI) sangle = start_angle-2*M_PI;
156  else sangle = start_angle;
157 
158  if (end_angle>M_PI) eangle = end_angle-2*M_PI;
159  else eangle = end_angle;
160 
161  //output bilinear interpolated data from the tables
162  int ilow, ihigh, jlow, jhigh;
163 
164  ilow = (int)floor((sangle+M_PI)/_dt);
165  ihigh = (int)ceil((sangle+M_PI)/_dt);
166 
167  jlow = (int)floor((eangle+M_PI)/_dt);
168  jhigh = (int)ceil((eangle+M_PI)/_dt);
169 
170  double slow = _theta[ilow];
171  double elow = _theta[jlow];
172 
173  double a = (sangle - slow)/_dt;
174  double b = (eangle - elow)/_dt;
175 
176  double L = (1-a)*(1-b)*ES_L[ilow][jlow] + a*(1-b)*ES_L[ihigh][jlow] +
177  (1-a)*b*ES_L[ilow][jhigh] + a*b*ES_L[ihigh][jhigh];
178 
179  return L;
180 }
181 
182 
183 // Computes the Euler spiral for the given params
184 //if the global lookup table is available, it looks up the ES params first and then optimizes them
185 //this should dramatically cut down in the time to optimize
187 {
188  //compute scaling distance
191 
192  //degeneracy check
193  if (d<eError)
194  return;
195 
196  //first compute a biarc estimate
200 
201  //get the total turning angle::This is an important parameter because
202  //it defines the one solution out of many possible solutions
205 
206  //From here on, normlize the parameters and use these to perform the optimization
207 
208  double k0_init_est = _bi_arc_estimate.params.K1*d;
209  double L_init_est = _bi_arc_estimate.params.L()/d;
210  double dstep = 0.1;
211 
212  //Alternately, we can get the initial values from the lookup table and perform
213  //the optimization from there
214  //double k0_init_est = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->k0(CCW(params.psi, params.start_angle), CCW(params.psi, params.end_angle));
215  //double L_init_est = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->L(CCW(params.psi, params.start_angle), CCW(params.psi, params.end_angle));
216  //double dstep = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->dt()/4;
217 
218  //then perform a simple gradient descent to find the real solution
219  double error = compute_error(k0_init_est, L_init_est);
220  double prev_error = error;
221 
222  double k0 = k0_init_est;
223  double L = L_init_est;
224 
225  double e1, e2, e3, e4 = 0;
226 
227  for (int i=0;i<MAX_NUM_ITERATIONS;i++)
228  {
229  if (error<eError)
230  break;
231 
232  e1 = compute_error(k0 + dstep, L);
233  e2 = compute_error(k0 - dstep, L);
234  e3 = compute_error(k0, L + dstep);
235  if (L>dstep) e4 = compute_error(k0, L - dstep);
236 
237  error = MIN2(MIN2(e1,e2),MIN2(e3,e4));
238 
239  if (error>prev_error)
240  {
241  dstep = dstep/2;
242  continue;
243  }
244 
245  if (error==e1) k0 = k0 + dstep;
246  else if (error==e2) k0 = k0 - dstep;
247  else if (error==e3) L = L + dstep;
248  else if (error==e4) L = L - dstep;
249 
250  prev_error = error;
251  }
252 
253  //store the parameters
254  params.K0 = k0/d;
255  params.L = L*d;
256  params.gamma = 2*(params.turningAngle - k0*L)/(L*L)/(d*d);
257  params.K2 = (k0 + params.gamma*L)/d;
258  params.error = error;
259 }
260 
261 //compute the extrinsic points
262 void EulerSpiral::computeSpiral(std::vector<Point2D<double> > &spiral, double ds, int NPts)
263 {
264  if (ds==0 && NPts==0){
265  //use default values
266  NPts = 100;
267  ds = params.L/NPts;
268  }
269 
270  spiral.clear();
271  spiral.push_back(params.start_pt);
272 
273  double s=ds;
274  if (NPts == 0)
275  NPts = (int) (params.L / ds);
276  for (int i=1; i<NPts; i++,s+=ds){
277  Point2D<double> cur_pt = compute_end_pt(s);
278  spiral.push_back(cur_pt);
279  }
280  spiral.push_back(params.end_pt);
281 }
282 
284 // Supporting functions
286 
287 Point2D<double> EulerSpiral::compute_es_point(EulerSpiralParams& es_params, double arclength, bool bNormalized)
288 {
289  params = es_params;
290  return compute_end_pt(params.K0, params.gamma, arclength, bNormalized);
291 }
292 
293 Point2D<double> EulerSpiral::compute_end_pt(double arclength, bool bNormalized)
294 {
295  return compute_end_pt(params.K0, params.gamma, arclength, bNormalized);
296 }
297 
298 Point2D<double> EulerSpiral::compute_end_pt(double k0, double gamma, double L, bool bNormalized)
299 {
300  Point2D<double> start_pt;
301  Point2D<double> end_pt;
302 
303  double theta;
304 
305  if (bNormalized){
306  start_pt = Point2D<double>(0,0);
307  theta = CCW(params.psi, params.start_angle);
308  }
309  else {
310  start_pt = params.start_pt;
311  theta = params.start_angle;
312  }
313 
314  if (L==0)
315  return start_pt;
316 
317  if (fabs(gamma)<eGamma)
318  {
319  if (fabs(k0)<eK)
320  {
321  //straight line
322  end_pt.setX(start_pt.getX()+L*cos(theta));
323  end_pt.setY(start_pt.getY()+L*sin(theta));
324  }
325  else
326  {
327  //circle
328  double const_term = 1.0/k0;
329  end_pt.setX(start_pt.getX()+const_term*(sin(k0*L+theta)-sin(theta)));
330  end_pt.setY(start_pt.getY()-const_term*(cos(k0*L+theta)-cos(theta)));
331  }
332  return end_pt;
333  }
334 
335  double const1 = sqrt(M_PI*fabs(gamma));
336  double const2 = sqrt(M_PI/fabs(gamma));
337 
338  Point2D<double> fresnel1 = get_fresnel_integral((k0+gamma*L)/const1);
339  Point2D<double> fresnel2 = get_fresnel_integral(k0/const1);
340 
341  double C = (fresnel1.getX() - fresnel2.getX());
342  if(gamma<0) {
343  C *= -1.;
344  }
345  double S = fresnel1.getY() - fresnel2.getY();
346 
347  double cos_term = cos(theta-((k0*k0)/(2.0*gamma)));
348  double sin_term = sin(theta-((k0*k0)/(2.0*gamma)));
349 
350  end_pt.setX(start_pt.getX() + const2*(C*cos_term - S*sin_term));
351  end_pt.setY(start_pt.getY() + const2*(C*sin_term + S*cos_term));
352 
353  return end_pt;
354 }
355 
356 inline double EulerSpiral::compute_error(double k0, double L)
357 {
358  //assumes normalized parameters
359 
360  //compute the endpoint of the Euler spiral with the given intrinsic parameters
361  double gamma = 2*(params.turningAngle - k0*L)/(L*L);
362  Point2D<double> cur_end_pt = compute_end_pt(k0, gamma, L, true);
363 
364  //the error is the distance between the current end point and the desired end point
365  return euc_distance(Point2D<double>(1,0), cur_end_pt);
366 }
367 
368 // Fresnel Integral code from Numerical recipes in C
369 // EPS is the relative error;
370 // MAXIT is the maximum number of iterations allowed;
371 // FPMIN is a number near the smallest representable floating-point number;
372 // XMIN is the dividing line between using the series and continued fraction.
373 
374 #define EPS 6.0e-8
375 #define MAXIT 100
376 #define FPMIN 1.0e-30
377 #define XMIN 1.5
378 
379 //Computes the Fresnel integrals S(x) and C(x) for all real x
381 {
382  bool odd;
383  int k,n;
384  double a,ax,fact,pix2,sign,sum,sumc,sums,term,test;
385 
386  std::complex<double> b,cc,d,h,del,cs;
387  Point2D<double> result;
388 
389  ax=fabs(x);
390  if (ax < sqrt(FPMIN))
391  {
392  //Special case: avoid failure of convergence test because of undeflow.
393  result.setY(0.0);
394  result.setX(ax);
395  }
396  else {
397  if (ax <= XMIN)
398  {
399  // Evaluate both series simultaneously.
400  sum=sums=0.0;
401  sumc=ax;
402  sign=1.0;
403  fact=(M_PI/2.0)*ax*ax;
404  odd=true;
405  term=ax;
406  n=3;
407 
408  for (k=1;k<=MAXIT;k++)
409  {
410  term *= fact/k;
411  sum += sign*term/n;
412  test=fabs(sum)*EPS;
413  if (odd)
414  {
415  sign = -sign;
416  sums=sum;
417  sum=sumc;
418  }
419  else {
420  sumc=sum;
421  sum=sums;
422  }
423 
424  if (term < test) break;
425  odd=!odd;
426  n +=2;
427  }
428 
429  if (k > MAXIT)
430  std::cout << "series failed in fresnel" << std::endl;
431 
432  result.setY(sums);
433  result.setX(sumc);
434  }
435  else {
436  // Evaluate continued fraction by modified Lentz's method
437  pix2=M_PI*ax*ax;
438  b = std::complex<double>(1.0,-pix2);
439  cc = std::complex<double>(1.0/FPMIN,0.0);
440  d=h = std::complex<double>(1.0,0.0)/b;
441  n = -1;
442 
443  for (k=2;k<=MAXIT;k++)
444  {
445  n +=2;
446  a = -n*(n+1);
447  b= b+std::complex<double>(4.0,0.0);
448  d=(std::complex<double>(1.0,0.0)/((a*d)+b));
449 
450  //Denominators cannot be zero
451  cc=(b+(std::complex<double>(a,0.0)/cc));
452 
453  del=(cc*d);
454  h=h*del;
455  if ((fabs(del.real()-1.0)+fabs(del.imag())) < EPS)
456  break;
457  }
458  if (k > MAXIT)
459  std::cout << "cf failed in frenel" << std::endl;
460 
461  h=std::complex<double>(ax,-ax)*h;
462  cs=std::complex<double>(0.5,0.5)*(std::complex<double>(1.0,0.0) - std::complex<double>(cos(0.5*pix2),sin(0.5*pix2))*h );
463 
464  result.setX(cs.real());
465  result.setY(cs.imag());
466  }
467  }
468 
469  if (x<0){ //use antisymmetry
470  result = -1*result;
471  }
472 
473  return result;
474 }
475 
double L()
Definition: BiArc.h:153
EulerSpiralParams params
Definition: euler.h:146
double L(double start_angle, double end_angle)
Definition: euler.cpp:148
#define FPMIN
Definition: euler.cpp:376
double error
Definition: euler.h:58
static EulerSpiralLookupTable * globalEulerSpiralLookupTable
Definition: euler.cpp:18
double K2
Definition: BiArc.h:50
void setY(coord_type ny)
Definition: points.h:100
#define M_PI
Definition: angles.h:37
void set_end_params(Point2D< double > end_pt, double end_angle)
Definition: BiArc.h:196
double compute_error(double k0, double L)
Definition: euler.cpp:356
void set_start_params(Point2D< double > start_pt, double start_angle)
Definition: BiArc.h:190
Point2D< double > end_pt
Definition: euler.h:49
#define EPS
Definition: euler.cpp:374
static EulerSpiralLookupTable * get_globalEulerSpiralLookupTable()
Definition: euler.cpp:21
double start_angle
Definition: euler.h:51
double L1
Definition: BiArc.h:52
double angle0To2Pi(double angle)
Definition: angles.h:40
#define eK
Definition: BiArc.h:35
double euc_distance(Point2D< point_type1 > pt1, Point2D< point_type2 > pt2)
Definition: points.h:245
coord_type getY() const
Definition: points.h:95
coord_type x() const
Definition: points.h:96
#define eError
Definition: euler.h:39
void compute_biarc_params()
Definition: BiArc.cpp:6
double L2
Definition: BiArc.h:53
double turningAngle
Definition: euler.h:57
double CCW(double reference, double angle1)
Definition: angles.h:57
double ** ES_gamma
Definition: euler.h:122
double K0
Definition: euler.h:53
double K2
Definition: euler.h:54
double K1
Definition: BiArc.h:49
void computeSpiral(std::vector< Point2D< double > > &spiral, double ds=0, int NPts=0)
Definition: euler.cpp:262
double ** ES_L
Definition: euler.h:123
T MIN2(T a, T b)
Definition: StdDefs.h:69
BiArcParams params
Definition: BiArc.h:168
double psi
Definition: euler.h:59
double ** ES_k0
Definition: euler.h:120
double k0(double start_angle, double end_angle)
Definition: euler.cpp:48
double gamma
Definition: euler.h:55
double L
Definition: euler.h:56
Point2D< double > get_fresnel_integral(double value)
Definition: euler.cpp:380
BiArc _bi_arc_estimate
Definition: euler.h:143
#define sign(a)
Definition: polyfonts.c:68
Point2D< double > compute_es_point(EulerSpiralParams &es_params, double arclength, bool bNormalized=false)
Definition: euler.cpp:287
Point2D< double > compute_end_pt(double arclength, bool bNormalized=false)
Definition: euler.cpp:293
double ** ES_k1
Definition: euler.h:121
void setX(coord_type nx)
Definition: points.h:99
double gamma(double start_angle, double end_angle)
Definition: euler.cpp:114
#define XMIN
Definition: euler.cpp:377
double end_angle
Definition: euler.h:52
coord_type y() const
Definition: points.h:97
void compute_es_params()
Definition: euler.cpp:186
Point2D< double > start_pt
Definition: euler.h:48
#define eGamma
Definition: euler.cpp:15
double k1(double start_angle, double end_angle)
Definition: euler.cpp:80
double theta(int N)
Definition: euler.cpp:43
coord_type getX() const
Definition: points.h:94
#define MAX_NUM_ITERATIONS
Definition: euler.h:38
#define MAXIT
Definition: euler.cpp:375