View Javadoc
1   package org.djutils.draw.line;
2   
3   import org.djutils.complex.Complex;
4   import org.djutils.draw.DrawRuntimeException;
5   import org.djutils.exceptions.Throw;
6   import org.djutils.polynomialroots.PolynomialRoots;
7   
8   /**
9    * Approximate a clothoid with a PolyLine3d. <br>
10   * Derived from https://github.com/ebertolazzi/G1fitting/blob/master/src/Clothoid.cc
11   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
12   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
13   */
14  public final class Clothoid
15  {
16      /** Utility class. */
17      private Clothoid()
18      {
19          // do not instantiate
20      }
21  
22      /** ??? */
23      static final double A_THRESOLD = 0.01;
24  
25      /** ??? */
26      static final int A_SERIE_SIZE = 3;
27  
28      //@formatter:off
29      /** Fresnel coefficients FN. */
30      static final double[] FN = 
31      { 
32          0.49999988085884732562,
33          1.3511177791210715095,
34          1.3175407836168659241,
35          1.1861149300293854992,
36          0.7709627298888346769,
37          0.4173874338787963957,
38          0.19044202705272903923,
39          0.06655998896627697537,
40          0.022789258616785717418,
41          0.0040116689358507943804,
42          0.0012192036851249883877 
43          };
44  
45      /** Fresnel coefficients FD. */
46      static final double[] FD = 
47      { 
48          1.0,
49          2.7022305772400260215,
50          4.2059268151438492767,
51          4.5221882840107715516,
52          3.7240352281630359588,
53          2.4589286254678152943,
54          1.3125491629443702962,
55          0.5997685720120932908,
56          0.20907680750378849485,
57          0.07159621634657901433,
58          0.012602969513793714191,
59          0.0038302423512931250065 
60      };
61  
62      /** Fresnel coefficients GN. */
63      static final double[] GN = 
64      { 
65          0.50000014392706344801,
66          0.032346434925349128728,
67          0.17619325157863254363,
68          0.038606273170706486252,
69          0.023693692309257725361,
70          0.007092018516845033662,
71          0.0012492123212412087428,
72          0.00044023040894778468486,
73          -8.80266827476172521e-6,
74          -1.4033554916580018648e-8,
75          2.3509221782155474353e-10 
76      };
77  
78      /** Fresnel coefficients GD. */
79      static final double[] GD = 
80      { 
81          1.0,
82          2.0646987497019598937,
83          2.9109311766948031235,
84          2.6561936751333032911,
85          2.0195563983177268073,
86          1.1167891129189363902,
87          0.57267874755973172715,
88          0.19408481169593070798,
89          0.07634808341431248904,
90          0.011573247407207865977,
91          0.0044099273693067311209,
92          -0.00009070958410429993314 
93      };
94  
95      /** Pi. */
96      static final double m_pi        = Math.PI;
97      /** Half Pi. */
98      static final double m_pi_2      = Math.PI / 2;
99      /** Two Pi. */
100     static final double m_2pi       = 2 * Math.PI;
101     /** One over Pi. */
102     static final double m_1_pi      = 1 / Math.PI;
103     /** One over square root of Pi. */
104     static final double m_1_sqrt_pi = 1 / Math.sqrt(Math.PI);
105 
106     /*
107      *  #######                                           
108      *  #       #####  ######  ####  #    # ###### #      
109      *  #       #    # #      #      ##   # #      #      
110      *  #####   #    # #####   ####  # #  # #####  #      
111      *  #       #####  #           # #  # # #      #      
112      *  #       #   #  #      #    # #   ## #      #      
113      *  #       #    # ######  ####  #    # ###### ###### 
114      */
115     //@formatter:on
116 
117     /**
118      * Purpose: This program computes the Fresnel integrals.
119      * 
120      * <pre>
121      * C(x) and S(x) using subroutine FCS
122      * Input :  x --- Argument of C(x) and S(x)
123      * Output:  C --- C(x)
124      * S --- S(x)
125      * Example:
126      * x          C(x)          S(x)
127      * -----------------------------------
128      * 0.0      .00000000      .00000000
129      * 0.5      .49234423      .06473243
130      * 1.0      .77989340      .43825915
131      * 1.5      .44526118      .69750496
132      * 2.0      .48825341      .34341568
133      * 2.5      .45741301      .61918176
134      * Purpose: Compute Fresnel integrals C(x) and S(x)
135      * Input :  x --- Argument of C(x) and S(x)
136      * Output:  C --- C(x)
137      * S --- S(x)
138      * </pre>
139      * 
140      * @param y double;
141      * @return double[]; double array with two elements; C is stored in the first, S in the second
142      */
143     private static double[] fresnelCS(final double y)
144     {
145 
146         final double eps = 1E-15;
147         final double x = y > 0 ? y : -y;
148         double resultC;
149         double resultS;
150 
151         if (x < 1.0)
152         {
153             double twofn, fact, denterm, numterm, sum, term;
154 
155             final double s = m_pi_2 * (x * x);
156             final double t = -s * s;
157 
158             // Cosine integral series
159             twofn = 0.0;
160             fact = 1.0;
161             denterm = 1.0;
162             numterm = 1.0;
163             sum = 1.0;
164             do
165             {
166                 twofn += 2.0;
167                 fact *= twofn * (twofn - 1.0);
168                 denterm += 4.0;
169                 numterm *= t;
170                 term = numterm / (fact * denterm);
171                 sum += term;
172             }
173             while (Math.abs(term) > eps * Math.abs(sum));
174 
175             resultC = x * sum;
176 
177             // Sine integral series
178             twofn = 1.0;
179             fact = 1.0;
180             denterm = 3.0;
181             numterm = 1.0;
182             sum = 1.0 / 3.0;
183             do
184             {
185                 twofn += 2.0;
186                 fact *= twofn * (twofn - 1.0);
187                 denterm += 4.0;
188                 numterm *= t;
189                 term = numterm / (fact * denterm);
190                 sum += term;
191             }
192             while (Math.abs(term) > eps * Math.abs(sum));
193 
194             resultS = m_pi_2 * sum * (x * x * x);
195         }
196         else if (x < 6.0)
197         {
198 
199             // Rational approximation for f
200             double sumn = 0.0;
201             double sumd = FD[11];
202             for (int k = 10; k >= 0; --k)
203             {
204                 sumn = FN[k] + x * sumn;
205                 sumd = FD[k] + x * sumd;
206             }
207             double f = sumn / sumd;
208 
209             // Rational approximation for g
210             sumn = 0.0;
211             sumd = GD[11];
212             for (int k = 10; k >= 0; --k)
213             {
214                 sumn = GN[k] + x * sumn;
215                 sumd = GD[k] + x * sumd;
216             }
217             double g = sumn / sumd;
218 
219             double u = m_pi_2 * (x * x);
220             double sinU = Math.sin(u);
221             double cosU = Math.cos(u);
222             resultC = 0.5 + f * sinU - g * cosU;
223             resultS = 0.5 - f * cosU - g * sinU;
224         }
225         else
226         {
227 
228             double absterm;
229 
230             // x >= 6; asymptotic expansions for f and g
231 
232             final double s = m_pi * x * x;
233             final double t = -1 / (s * s);
234 
235             // Expansion for f
236             double numterm = -1.0;
237             double term = 1.0;
238             double sum = 1.0;
239             double oldterm = 1.0;
240             double eps10 = 0.1 * eps;
241 
242             do
243             {
244                 numterm += 4.0;
245                 term *= numterm * (numterm - 2.0) * t;
246                 sum += term;
247                 absterm = Math.abs(term);
248                 Throw.when(false, oldterm >= absterm, DrawRuntimeException.class,
249                         "In FresnelCS f not converged to eps, x = " + x + " oldterm = " + oldterm + " absterm = " + absterm);
250                 oldterm = absterm;
251             }
252             while (absterm > eps10 * Math.abs(sum));
253 
254             double f = sum / (m_pi * x);
255 
256             // Expansion for g
257             numterm = -1.0;
258             term = 1.0;
259             sum = 1.0;
260             oldterm = 1.0;
261 
262             do
263             {
264                 numterm += 4.0;
265                 term *= numterm * (numterm + 2.0) * t;
266                 sum += term;
267                 absterm = Math.abs(term);
268                 Throw.when(oldterm >= absterm, DrawRuntimeException.class, "In FresnelCS g does not converge to eps, x = " + x
269                         + " oldterm = " + oldterm + " absterm = " + absterm);
270                 oldterm = absterm;
271             }
272             while (absterm > eps10 * Math.abs(sum));
273 
274             double g = m_pi * x;
275             g = sum / (g * g * x);
276 
277             double u = m_pi_2 * (x * x);
278             double sinU = Math.sin(u);
279             double cosU = Math.cos(u);
280             resultC = 0.5 + f * sinU - g * cosU;
281             resultS = 0.5 - f * cosU - g * sinU;
282 
283         }
284         if (y < 0)
285         {
286             resultC = -resultC;
287             resultS = -resultS;
288         }
289         return new double[] { resultC, resultS };
290     }
291 
292     /**
293      * ???
294      * @param nk int; size of the provided arrays
295      * @param t double;
296      * @param C double[]; should have length nk
297      * @param S double[]; shouldhave laength nk
298      */
299     private static void fresnelCS(final int nk, final double t, final double[] C, final double[] S)
300     {
301         double[] cs = fresnelCS(t);
302         C[0] = cs[0];
303         S[0] = cs[1];
304 
305         if (nk > 1)
306         {
307             double tt = m_pi_2 * (t * t);
308             double ss = Math.sin(tt);
309             double cc = Math.cos(tt);
310             C[1] = ss * m_1_pi;
311             S[1] = (1 - cc) * m_1_pi;
312             if (nk > 2)
313             {
314                 C[2] = (t * ss - S[0]) * m_1_pi;
315                 S[2] = (C[0] - t * cc) * m_1_pi;
316             }
317         }
318     }
319 
320     /**
321      * ???
322      * @param a double;
323      * @param b double;
324      * @return double[] with two elements set to X and Y
325      */
326     private static double[] evalXYaLarge(final double a, final double b)
327     {
328         double s = a > 0 ? +1 : -1;
329         double absa = Math.abs(a);
330         double z = m_1_sqrt_pi * Math.sqrt(absa);
331         double ell = s * b * m_1_sqrt_pi / Math.sqrt(absa);
332         double g = -0.5 * s * (b * b) / absa;
333         double cg = Math.cos(g) / z;
334         double sg = Math.sin(g) / z;
335 
336         // double Cl, Sl, Cz, Sz;
337         double[] resultL = fresnelCS(ell);
338         double[] resultZ = fresnelCS(ell + z);
339 
340         double dC0 = resultZ[0] - resultL[0];
341         double dS0 = resultZ[1] - resultL[1];
342 
343         double x = cg * dC0 - s * sg * dS0;
344         double y = sg * dC0 + s * cg * dS0;
345         return new double[] { x, y };
346     }
347 
348     // -------------------------------------------------------------------------
349     // nk max 3
350     /**
351      * ???
352      * @param nk int; minimum 0; maximum 3
353      * @param a double; ?
354      * @param b double; ?
355      * @param xArray double[]; ?
356      * @param yArray double[]; ?
357      */
358     private static void evalXYaLarge(final int nk, final double a, final double b, final double[] xArray, final double[] yArray)
359     {
360         Throw.when(nk <= 0 || nk >= 4, DrawRuntimeException.class,
361                 "In evalXYaLarge first argument nk must be in 1..3, nk " + nk);
362 
363         double s = a > 0 ? +1 : -1;
364         double absa = Math.abs(a);
365         double z = m_1_sqrt_pi * Math.sqrt(absa);
366         double ell = s * b * m_1_sqrt_pi / Math.sqrt(absa);
367         double g = -0.5 * s * (b * b) / absa;
368         double cg = Math.cos(g) / z;
369         double sg = Math.sin(g) / z;
370 
371         double[] cl = new double[3];
372         double[] sl = new double[3];
373         double[] cz = new double[3];
374         double[] sz = new double[3];
375 
376         fresnelCS(nk, ell, cl, sl);
377         fresnelCS(nk, ell + z, cz, sz);
378 
379         double dC0 = cz[0] - cl[0];
380         double dS0 = sz[0] - sl[0];
381         xArray[0] = cg * dC0 - s * sg * dS0;
382         yArray[0] = sg * dC0 + s * cg * dS0;
383         if (nk > 1)
384         {
385             cg /= z;
386             sg /= z;
387             double dC1 = cz[1] - cl[1];
388             double dS1 = sz[1] - sl[1];
389             double DC = dC1 - ell * dC0;
390             double DS = dS1 - ell * dS0;
391             xArray[1] = cg * DC - s * sg * DS;
392             yArray[1] = sg * DC + s * cg * DS;
393             if (nk > 2)
394             {
395                 double dC2 = cz[2] - cl[2];
396                 double dS2 = sz[2] - sl[2];
397                 DC = dC2 + ell * (ell * dC0 - 2 * dC1);
398                 DS = dS2 + ell * (ell * dS0 - 2 * dS1);
399                 cg = cg / z;
400                 sg = sg / z;
401                 xArray[2] = cg * DC - s * sg * DS;
402                 yArray[2] = sg * DC + s * cg * DS;
403             }
404         }
405     }
406 
407     /**
408      * LommelReduced.
409      * @param mu double; ?
410      * @param nu double; ?
411      * @param b double; ?
412      * @return double; ?
413      */
414     private static double LommelReduced(final double mu, final double nu, final double b)
415     {
416         double tmp = 1 / ((mu + nu + 1) * (mu - nu + 1));
417         double res = tmp;
418         for (int n = 1; n <= 100; ++n)
419         {
420             tmp *= (-b / (2 * n + mu - nu + 1)) * (b / (2 * n + mu + nu + 1));
421             res += tmp;
422             if (Math.abs(tmp) < Math.abs(res) * 1e-50)
423             {
424                 break;
425             }
426         }
427         return res;
428     }
429 
430     /**
431      * ???
432      * @param nk int; ?
433      * @param b double; ?
434      * @param zArray double[]; ?
435      * @param yArray double[]; ?
436      */
437     private static void evalXYazero(final int nk, final double b, final double[] zArray, final double[] yArray)
438     {
439         double sb = Math.sin(b);
440         double cb = Math.cos(b);
441         double b2 = b * b;
442         if (Math.abs(b) < 1e-3)
443         {
444             zArray[0] = 1 - (b2 / 6) * (1 - (b2 / 20) * (1 - (b2 / 42)));
445             yArray[0] = (b / 2) * (1 - (b2 / 12) * (1 - (b2 / 30)));
446         }
447         else
448         {
449             zArray[0] = sb / b;
450             yArray[0] = (1 - cb) / b;
451         }
452         // use recurrence in the stable part
453         int m = (int) Math.floor(2 * b);
454         if (m >= nk)
455         {
456             m = nk - 1;
457         }
458         if (m < 1)
459         {
460             m = 1;
461         }
462         for (int k = 1; k < m; ++k)
463         {
464             zArray[k] = (sb - k * yArray[k - 1]) / b;
465             yArray[k] = (k * zArray[k - 1] - cb) / b;
466         }
467         // use Lommel for the unstable part
468         if (m < nk)
469         {
470             double A = b * sb;
471             double D = sb - b * cb;
472             double B = b * D;
473             double C = -b2 * sb;
474             double rLa = LommelReduced(m + 0.5, 1.5, b);
475             double rLd = LommelReduced(m + 0.5, 0.5, b);
476             for (int k = m; k < nk; ++k)
477             {
478                 double rLb = LommelReduced(k + 1.5, 0.5, b);
479                 double rLc = LommelReduced(k + 1.5, 1.5, b);
480                 zArray[k] = (k * A * rLa + B * rLb + cb) / (1 + k);
481                 yArray[k] = (C * rLc + sb) / (2 + k) + D * rLd;
482                 rLa = rLc;
483                 rLd = rLb;
484             }
485         }
486     }
487 
488     /**
489      * ???
490      * @param a double; ?
491      * @param b double; ?
492      * @param p double; ?
493      * @return double[]; containing the two values; X and Y.
494      */
495     private static double[] evalXYaSmall(final double a, final double b, final int p)
496     {
497         Throw.when(p >= 11 && p <= 0, DrawRuntimeException.class, "In evalXYaSmall p = " + p + " must be in 1..10");
498 
499         double[] x0 = new double[43];
500         double[] y0 = new double[43];
501 
502         int nkk = 4 * p + 3; // max 43
503         evalXYazero(nkk, b, x0, y0);
504 
505         double x = x0[0] - (a / 2) * y0[2];
506         double y = y0[0] + (a / 2) * x0[2];
507 
508         double t = 1;
509         double aa = -a * a / 4; // controllare!
510         for (int n = 1; n <= p; ++n)
511         {
512             t *= aa / (2 * n * (2 * n - 1));
513             double bf = a / (4 * n + 2);
514             int jj = 4 * n;
515             x += t * (x0[jj] - bf * y0[jj + 2]);
516             y += t * (y0[jj] + bf * x0[jj + 2]);
517         }
518         return new double[] { x, y };
519     }
520 
521     /**
522      * ???
523      * @param nk int; ?
524      * @param a double; ?
525      * @param b double; ?
526      * @param p double; ?
527      * @param x double[]; ?
528      * @param y double[]; ?
529      */
530     private static void evalXYaSmall(final int nk, final double a, final double b, final int p, final double[] x,
531             final double[] y)
532     {
533 
534         int nkk = nk + 4 * p + 2; // max 45
535         double[] x0 = new double[45];
536         double[] y0 = new double[45];
537 
538         Throw.when(nkk >= 46, DrawRuntimeException.class,
539                 "In evalXYaSmall (nk,p) = (" + nk + "," + p + ")\n" + "nk + 4*p + 2 = " + nkk + " must be less than 46\n");
540 
541         evalXYazero(nkk, b, x0, y0);
542 
543         for (int j = 0; j < nk; ++j)
544         {
545             x[j] = x0[j] - (a / 2) * y0[j + 2];
546             y[j] = y0[j] + (a / 2) * x0[j + 2];
547         }
548 
549         double t = 1;
550         double aa = -a * a / 4; // controllare!
551         for (int n = 1; n <= p; ++n)
552         {
553             t *= aa / (2 * n * (2 * n - 1));
554             double bf = a / (4 * n + 2);
555             for (int j = 0; j < nk; ++j)
556             {
557                 int jj = 4 * n + j;
558                 x[j] += t * (x0[jj] - bf * y0[jj + 2]);
559                 y[j] += t * (y0[jj] + bf * x0[jj + 2]);
560             }
561         }
562     }
563 
564     /**
565      * ???
566      * @param a double; ?
567      * @param b double; ?
568      * @param c double; ?
569      * @return double[]; size two containing C and S
570      */
571     private static double[] GeneralizedFresnelCS(final double a, final double b, final double c)
572     {
573 
574         double[] xxyy = Math.abs(a) < A_THRESOLD ? evalXYaSmall(a, b, A_SERIE_SIZE) : evalXYaLarge(a, b);
575 
576         double cosC = Math.cos(c);
577         double sinC = Math.sin(c);
578 
579         // FIXME: Confusing names
580         double intC = xxyy[0] * cosC - xxyy[1] * sinC;
581         double intS = xxyy[0] * sinC + xxyy[1] * cosC;
582         return new double[] { intC, intS };
583     }
584 
585     /**
586      * ???
587      * @param nk int; ?
588      * @param a double; ?
589      * @param b double; ?
590      * @param c double; ?
591      * @param intC double[]; stores C results
592      * @param intS double[]; stores S results
593      */
594     static void GeneralizedFresnelCS(final int nk, final double a, final double b, final double c, final double[] intC,
595             final double[] intS)
596     {
597 
598         Throw.when(nk <= 0 || nk >= 4, DrawRuntimeException.class, "nk = " + nk + " must be in 1..3");
599 
600         if (Math.abs(a) < A_THRESOLD)
601         {
602             evalXYaSmall(nk, a, b, A_SERIE_SIZE, intC, intS);
603         }
604         else
605         {
606             evalXYaLarge(nk, a, b, intC, intS);
607         }
608 
609         double cosC = Math.cos(c);
610         double sinC = Math.sin(c);
611 
612         for (int k = 0; k < nk; ++k)
613         {
614             double xx = intC[k];
615             double yy = intS[k];
616             intC[k] = xx * cosC - yy * sinC;
617             intS[k] = xx * sinC + yy * cosC;
618         }
619     }
620 
621     /** CF coefficients. */
622     private static final double[] CF = { 2.989696028701907, 0.716228953608281, -0.458969738821509, -0.502821153340377,
623             0.261062141752652, -0.045854475238709 };
624 
625     /**
626      * Create a clothoid connecting (x0,y0) to (x1,y1) having direction theta0 at the start point and theta1 at the end point.
627      * @param x0 double; x coordinate of the start point
628      * @param y0 double; y coordinate of the start point
629      * @param theta0 double; direction at the start point (in radians)
630      * @param x1 double; x coordinate of the end point
631      * @param y1 double; y coordinate of the end point
632      * @param theta1 double; direction at the end point (in radians)
633      * @return int; the number of iterations
634      */
635     public static int buildClothoid(final double x0, final double y0, final double theta0, final double x1, final double y1,
636             final double theta1)
637     {
638         double k;
639         double dk;
640         double l;
641         // traslazione in (0,0)
642         double dx = x1 - x0;
643         double dy = y1 - y0;
644         double r = Math.hypot(dx, dy);
645         double phi = Math.atan2(dy, dx);
646 
647         double phi0 = theta0 - phi;
648         double phi1 = theta1 - phi;
649 
650         phi0 -= m_2pi * Math.rint(phi0 / m_2pi);
651         phi1 -= m_2pi * Math.rint(phi1 / m_2pi);
652 
653         if (phi0 > m_pi)
654         {
655             phi0 -= m_2pi;
656         }
657         if (phi0 < -m_pi)
658         {
659             phi0 += m_2pi;
660         }
661         if (phi1 > m_pi)
662         {
663             phi1 -= m_2pi;
664         }
665         if (phi1 < -m_pi)
666         {
667             phi1 += m_2pi;
668         }
669 
670         double delta = phi1 - phi0;
671 
672         // punto iniziale
673         double x = phi0 * m_1_pi;
674         double y = phi1 * m_1_pi;
675         double xy = x * y;
676         y *= y;
677         x *= x;
678         double a =
679                 (phi0 + phi1) * (CF[0] + xy * (CF[1] + xy * CF[2]) + (CF[3] + xy * CF[4]) * (x + y) + CF[5] * (x * x + y * y));
680 
681         // newton
682         double g = 0;
683         double dg;
684         double[] intC = new double[3];
685         double[] intS = new double[3];
686         int niter = 0;
687         do
688         {
689             GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
690             g = intS[0];
691             dg = intC[2] - intC[1];
692             a -= g / dg;
693         }
694         while (++niter <= 10 && Math.abs(g) > 1E-12);
695 
696         Throw.when(Math.abs(g) > 1E-8, DrawRuntimeException.class, "Newton did not converge, g = " + g + " niter = " + niter);
697         double[] cs = GeneralizedFresnelCS(2 * a, delta - a, phi0);
698         intC[0] = cs[0];
699         intS[0] = cs[1];
700         l = r / intC[0];
701 
702         Throw.when(l <= 0, DrawRuntimeException.class, "Negative length L = " + l);
703         k = (delta - a) / l;
704         dk = 2 * a / l / l;
705 
706         return niter;
707     }
708 
709     /**
710      * Create a clothoid connecting (x0,y0) to (x1,y1) having direction theta0 at the start point and theta1 at the end point.
711      * @param x0 double; x coordinate of the start point
712      * @param y0 double; y coordinate of the start point
713      * @param theta0 double; direction at the start point (in radians)
714      * @param x1 double; x coordinate of the end point
715      * @param y1 double; y coordinate of the end point
716      * @param theta1 double; direction at the end point (in radians)
717      * @return int; the number of iterations
718      */
719     public static int buildClothoidMoreResults(final double x0, final double y0, final double theta0, final double x1,
720             final double y1, final double theta1)
721     {
722         double k;
723         double dk;
724         double l;
725         double k_1;
726         double dk_1;
727         double l_1;
728         double k_2;
729         double dk_2;
730         double l_2;
731         // traslazione in (0,0)
732         double dx = x1 - x0;
733         double dy = y1 - y0;
734         double r = Math.hypot(dx, dy);
735         double phi = Math.atan2(dy, dx);
736 
737         double phi0 = theta0 - phi;
738         double phi1 = theta1 - phi;
739 
740         phi0 -= m_2pi * Math.rint(phi0 / m_2pi);
741         phi1 -= m_2pi * Math.rint(phi1 / m_2pi);
742 
743         if (phi0 > m_pi)
744         {
745             phi0 -= m_2pi;
746         }
747         if (phi0 < -m_pi)
748         {
749             phi0 += m_2pi;
750         }
751         if (phi1 > m_pi)
752         {
753             phi1 -= m_2pi;
754         }
755         if (phi1 < -m_pi)
756         {
757             phi1 += m_2pi;
758         }
759 
760         double delta = phi1 - phi0;
761 
762         // punto iniziale
763         double x = phi0 * m_1_pi;
764         double y = phi1 * m_1_pi;
765         double xy = x * y;
766         y *= y;
767         x *= x;
768         double a =
769                 (phi0 + phi1) * (CF[0] + xy * (CF[1] + xy * CF[2]) + (CF[3] + xy * CF[4]) * (x + y) + CF[5] * (x * x + y * y));
770 
771         // newton
772         double g = 0;
773         double dg;
774         double[] intC = new double[3];
775         double[] intS = new double[3];
776         int niter = 0;
777         do
778         {
779             GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
780             g = intS[0];
781             dg = intC[2] - intC[1];
782             a -= g / dg;
783         }
784         while (++niter <= 10 && Math.abs(g) > 1E-12);
785 
786         Throw.when(Math.abs(g) > 1E-8, DrawRuntimeException.class, "Newton do not converge, g = " + g + " niter = " + niter);
787         GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
788         l = r / intC[0];
789 
790         Throw.when(l <= 0, DrawRuntimeException.class, "Negative length L = " + l);
791         k = (delta - a) / l;
792         dk = 2 * a / l / l;
793 
794         double alpha = intC[0] * intC[1] + intS[0] * intS[1];
795         double beta = intC[0] * intC[2] + intS[0] * intS[2];
796         double gamma = intC[0] * intC[0] + intS[0] * intS[0];
797         double tx = intC[1] - intC[2];
798         double ty = intS[1] - intS[2];
799         double txy = l * (intC[1] * intS[2] - intC[2] * intS[1]);
800         double omega = l * (intS[0] * tx - intC[0] * ty) - txy;
801 
802         delta = intC[0] * tx + intS[0] * ty;
803 
804         l_1 = omega / delta;
805         l_2 = txy / delta;
806 
807         delta *= l;
808         k_1 = (beta - gamma - k * omega) / delta;
809         k_2 = -(beta + k * txy) / delta;
810 
811         delta *= l / 2;
812         dk_1 = (gamma - alpha - dk * omega * l) / delta;
813         dk_2 = (alpha - dk * txy * l) / delta;
814 
815         return niter;
816     }
817 
818     // void
819     // eval(final double s,
820     // double & theta,
821     // double & kappa,
822     // double & x,
823     // double & y ) const {
824     // double C, S ;
825     // GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
826     // x = x0 + s*C ;
827     // y = y0 + s*S ;
828     // theta = theta0 + s*(k+s*(dk/2)) ;
829     // kappa = k + s*dk ;
830     // }
831     //
832     // void
833     // ClothoidCurve::eval( double s, double & x, double & y ) const {
834     // double C, S ;
835     // GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
836     // x = x0 + s*C ;
837     // y = y0 + s*S ;
838     // }
839     //
840     // void
841     // ClothoidCurve::eval_D( double s, double & x_D, double & y_D ) const {
842     // double theta = theta0 + s*(k+s*(dk/2)) ;
843     // x_D = cos(theta) ;
844     // y_D = sin(theta) ;
845     // }
846     //
847     // void
848     // ClothoidCurve::eval_DD( double s, double & x_DD, double & y_DD ) const {
849     // double theta = theta0 + s*(k+s*(dk/2)) ;
850     // double theta_D = k+s*dk ;
851     // x_DD = -sin(theta)*theta_D ;
852     // y_DD = cos(theta)*theta_D ;
853     // }
854     //
855     // void
856     // ClothoidCurve::eval_DDD( double s, double & x_DDD, double & y_DDD ) const {
857     // double theta = theta0 + s*(k+s*(dk/2)) ;
858     // double theta_D = k+s*dk ;
859     // double C = cos(theta) ;
860     // double S = sin(theta) ;
861     // double th2 = theta_D*theta_D ;
862     // x_DDD = -C*th2-S*dk ;
863     // y_DDD = -S*th2+C*dk ;
864     // }
865     //
866     //// offset curve
867     // void
868     // ClothoidCurve::eval( double s, double offs, double & x, double & y ) const {
869     // double C, S ;
870     // GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
871     // double theta = theta0 + s*(k+s*(dk/2)) ;
872     // double nx = -sin(theta) ;
873     // double ny = cos(theta) ;
874     // x = x0 + s*C + offs * nx ;
875     // y = y0 + s*S + offs * ny ;
876     // }
877     //
878     // void
879     // ClothoidCurve::eval_D( double s, double offs, double & x_D, double & y_D ) const {
880     // double theta = theta0 + s*(k+s*(dk/2)) ;
881     // double theta_D = k+s*dk ;
882     // double scale = 1-offs*theta_D ;
883     // x_D = cos(theta)*scale ;
884     // y_D = sin(theta)*scale ;
885     // }
886     //
887     // void
888     // ClothoidCurve::eval_DD( double s, double offs, double & x_DD, double & y_DD ) const {
889     // double theta = theta0 + s*(k+s*(dk/2)) ;
890     // double theta_D = k+s*dk ;
891     // double C = cos(theta) ;
892     // double S = sin(theta) ;
893     // double tmp1 = theta_D*(1-theta_D*offs) ;
894     // double tmp2 = offs*dk ;
895     // x_DD = -tmp1*S - C*tmp2 ;
896     // y_DD = tmp1*C - S*tmp2 ;
897     // }
898     //
899     // void
900     // ClothoidCurve::eval_DDD( double s, double offs, double & x_DDD, double & y_DDD ) const {
901     // double theta = theta0 + s*(k+s*(dk/2)) ;
902     // double theta_D = k+s*dk ;
903     // double C = cos(theta) ;
904     // double S = sin(theta) ;
905     // double tmp1 = theta_D*theta_D*(theta_D*offs-1) ;
906     // double tmp2 = dk*(1-3*theta_D*offs) ;
907     // x_DDD = tmp1*C-tmp2*S ;
908     // y_DDD = tmp1*S+tmp2*C ;
909     // }
910 
911     /**
912      * ???
913      * @param theta0 double; theta0
914      * @param theta double; theta
915      * @return double; kappa
916      */
917     private static double kappa(final double theta0, final double theta)
918     {
919         double x = theta0 * theta0;
920         double a = -3.714 + x * 0.178;
921         double b = -1.913 - x * 0.0753;
922         double c = 0.999 + x * 0.03475;
923         double d = 0.191 - x * 0.00703;
924         double e = 0.500 - x * -0.00172;
925         double t = d * theta0 + e * theta;
926         return a * theta0 + b * theta + c * (t * t * t);
927     }
928 
929     /**
930      * theta_guess.
931      * <p>
932      * FIXME value parameter ok;
933      * @param theta0 double; theta0
934      * @param k0 double; k0
935      * @return double; theta
936      */
937     private static double theta_guess(final double theta0, final double k0)
938     {
939         double x = theta0 * theta0;
940         double a = -3.714 + x * 0.178;
941         double b = -1.913 - x * 0.0753;
942         double c = 0.999 + x * 0.03475;
943         double d = 0.191 - x * 0.00703;
944         double e = 0.500 - x * -0.00172;
945         double e2 = e * e;
946         double dt = d * theta0;
947         double dt2 = dt * dt;
948         double qA = c * e * e2;
949         double qB = 3 * (c * d * e2 * theta0);
950         double qC = 3 * c * e * dt2 + b;
951         double qD = c * (dt * dt2) + a * theta0 - k0;
952         boolean ok;
953 
954         Complex[] roots = PolynomialRoots.cubicRoots(qA, qB, qC, qD);
955         // Count the real roots
956         int nr = 0;
957         for (Complex root : roots)
958         {
959             if (root.isReal())
960             {
961                 nr++;
962             }
963         }
964         // cerco radice reale piu vicina
965         double theta;
966         switch (nr)
967         {
968             case 0:
969             default:
970                 ok = false;
971                 return 0;
972             case 1:
973                 theta = roots[0].re;
974                 break;
975             case 2:
976                 if (Math.abs(roots[0].re - theta0) < Math.abs(roots[1].re - theta0))
977                 {
978                     theta = roots[0].re;
979                 }
980                 else
981                 {
982                     theta = roots[1].re;
983                 }
984                 break;
985             case 3:
986                 theta = roots[0].re;
987                 for (int i = 1; i < 3; ++i)
988                 {
989                     if (Math.abs(theta - theta0) > Math.abs(roots[i].re - theta0))
990                     {
991                         theta = roots[i].re;
992                     }
993                 }
994                 break;
995         }
996         ok = Math.abs(theta - theta0) < m_pi;
997         return theta;
998     }
999 
1000     // bool
1001     // ClothoidCurve::setup_forward( double _x0,
1002     // double _y0,
1003     // double _theta0,
1004     // double _k,
1005     // double _x1,
1006     // double _y1,
1007     // double tol ) {
1008     //
1009     // x0 = _x0 ;
1010     // y0 = _y0 ;
1011     // theta0 = _theta0 ;
1012     // k = _k ;
1013     // s_min = 0 ;
1014     //
1015     //// Compute guess angles
1016     // double len = hypot( _y1-_y0, _x1-_x0 ) ;
1017     // double arot = atan2( _y1-_y0, _x1-_x0 ) ;
1018     // double th0 = theta0 - arot ;
1019     //// normalize angle
1020     // while ( th0 > m_pi ) th0 -= m_2pi ;
1021     // while ( th0 < -m_pi ) th0 += m_2pi ;
1022     //
1023     //// solve the problem from (0,0) to (1,0)
1024     // double k0 = k*len ;
1025     // double alpha = 2.6 ;
1026     // double thmin = max(-m_pi,-theta0/2-alpha) ;
1027     // double thmax = min( m_pi,-theta0/2+alpha) ;
1028     // double Kmin = kappa( th0, thmax ) ;
1029     // double Kmax = kappa( th0, thmin ) ;
1030     // bool ok ;
1031     // double th = theta_guess( th0, max(min(k0,Kmax),Kmin), ok ) ;
1032     // if ( ok ) {
1033     // for ( int iter = 0 ; iter < 10 ; ++iter ) {
1034     // double dk, L, k_1, dk_1, L_1, k_2, dk_2, L_2 ;
1035     // buildClothoid( 0, 0, th0,
1036     // 1, 0, th,
1037     // k, dk, L, k_1, dk_1, L_1, k_2, dk_2, L_2 ) ;
1038     // double f = k - k0 ;
1039     // double df = k_2 ;
1040     // double dth = f/df ;
1041     // th -= dth ;
1042     // if ( abs(dth) < tol && abs(f) < tol ) {
1043     //// transform solution
1044     // buildClothoid( x0, y0, theta0,
1045     // _x1, _y1, arot + th,
1046     // _k, dk, s_max ) ;
1047     // return true ;
1048     // }
1049     // }
1050     // }
1051     // return false ;
1052     // }
1053     //
1054     // void
1055     // ClothoidCurve::change_origin( double s0 ) {
1056     // double new_theta, new_kappa, new_x0, new_y0 ;
1057     // eval( s0, new_theta, new_kappa, new_x0, new_y0 ) ;
1058     // x0 = new_x0 ;
1059     // y0 = new_y0 ;
1060     // theta0 = new_theta ;
1061     // k = new_kappa ;
1062     // s_min -= s0 ;
1063     // s_max -= s0 ;
1064     // }
1065     //
1066     // bool
1067     // ClothoidCurve::bbTriangle( double offs,
1068     // double p0[2],
1069     // double p1[2],
1070     // double p2[2] ) const {
1071     // double theta_max = theta( s_max ) ;
1072     // double theta_min = theta( s_min ) ;
1073     // double dtheta = Math.abs( theta_max-theta_min ) ;
1074     // if ( dtheta < m_pi_2 ) {
1075     // double alpha, t0[2] ;
1076     // eval( s_min, offs, p0[0], p0[1] ) ;
1077     // eval_D( s_min, t0[0], t0[1] ) ; // no offset
1078     // if ( dtheta > 0.0001 * m_pi_2 ) {
1079     // double t1[2] ;
1080     // eval( s_max, offs, p1[0], p1[1] ) ;
1081     // eval_D( s_max, t1[0], t1[1] ) ; // no offset
1082     //// risolvo il sistema
1083     //// p0 + alpha * t0 = p1 + beta * t1
1084     //// alpha * t0 - beta * t1 = p1 - p0
1085     // double det = t1[0]*t0[1]-t0[0]*t1[1] ;
1086     // alpha = ((p1[1]-p0[1])*t1[0] - (p1[0]-p0[0])*t1[1])/det ;
1087     // } else {
1088     //// se angolo troppo piccolo uso approx piu rozza
1089     // alpha = s_max - s_min ;
1090     // }
1091     // p2[0] = p0[0] + alpha*t0[0] ;
1092     // p2[1] = p0[1] + alpha*t0[1] ;
1093     // return true ;
1094     // } else {
1095     // return false ;
1096     // }
1097     // }
1098     //
1099     // void
1100     // ClothoidCurve::bbSplit( double split_angle,
1101     // double split_size,
1102     // double split_offs,
1103     // vector<ClothoidCurve> & c,
1104     // vector<Triangle2D> & t ) const {
1105     //
1106     //// step 0: controllo se curvatura passa per 0
1107     // double k_min = theta_D( s_min ) ;
1108     // double k_max = theta_D( s_max ) ;
1109     // c.clear() ;
1110     // t.clear() ;
1111     // if ( k_min * k_max < 0 ) {
1112     //// risolvo (s-s_min)*dk+k_min = 0 --> s = s_min-k_min/dk
1113     // double s_med = s_min-k_min/dk ;
1114     //
1115     // ClothoidCurve tmp(*this) ;
1116     // tmp.trim(s_min,s_med) ;
1117     // tmp.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
1118     // tmp.trim(s_med,s_max) ;
1119     // tmp.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
1120     // }else
1121     //
1122     // {
1123     // bbSplit_internal(split_angle, split_size, split_offs, c, t);
1124     // }
1125     // }
1126     //
1127     // static double
1128     //
1129     // abs2pi( double a ) {
1130     // a = Math.abs(a) ;
1131     // while ( a > m_pi ) a -= m_2pi ;
1132     // return Math.abs(a) ;
1133     // }
1134     //
1135     // void
1136     // ClothoidCurve::bbSplit_internal( double split_angle,
1137     // double split_size,
1138     // double split_offs,
1139     // vector<ClothoidCurve> & c,
1140     // vector<Triangle2D> & t ) const {
1141     //
1142     // double theta_min, kappa_min, x_min, y_min,
1143     // theta_max, kappa_max, x_max, y_max ;
1144     //
1145     // eval( s_min, theta_min, kappa_min, x_min, y_min ) ;
1146     // eval( s_max, theta_max, kappa_max, x_max, y_max ) ;
1147     //
1148     // double dtheta = Math.abs( theta_max - theta_min ) ;
1149     // double dx = x_max - x_min ;
1150     // double dy = y_max - y_min ;
1151     // double len = hypot( dy, dx ) ;
1152     // double dangle = abs2pi(atan2( dy, dx )-theta_min) ;
1153     // if ( dtheta <= split_angle && len*tan(dangle) <= split_size ) {
1154     // Triangle2D tt ;
1155     // this->bbTriangle(split_offs,tt) ;
1156     // c.push_back(*this) ;
1157     // t.push_back(tt) ;
1158     // } else {
1159     //
1160     // ClothoidCurve cc(*this) ;
1161     // double s_med = (s_min+s_max)/2 ;
1162     // cc.trim(s_min,s_med) ;
1163     // cc.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
1164     // cc.trim(s_med,s_max) ;
1165     // cc.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
1166     // }}
1167     //
1168     // bool ClothoidCurve::intersect_internal(ClothoidCurve&c1,
1169     //
1170     // double c1_offs, double&s1,ClothoidCurve&c2,
1171     //
1172     // double c2_offs, double&s2,
1173     //
1174     // int max_iter,
1175     // double tolerance)const{
1176     //
1177     // double angle1a = c1.theta(c1.s_min);
1178     //
1179     // double angle1b = c1.theta(c1.s_max);
1180     //
1181     // double angle2a = c2.theta(c2.s_min);
1182     //
1183     // double angle2b = c2.theta(c2.s_max);
1184     //
1185     // // cerca angoli migliori per partire
1186     // double dmax = abs2pi(angle1a - angle2a);
1187     //
1188     // double dab = abs2pi(angle1a - angle2b);
1189     //
1190     // double dba = abs2pi(angle1b - angle2a);
1191     //
1192     // double dbb = abs2pi(angle1b - angle2b);s1=c1.s_min;s2=c2.s_min;if(dmax<dab)
1193     // {
1194     // dmax = dab;
1195     // s2 = c2.s_max;
1196     // }if(dmax<dba)
1197     // {
1198     // dmax = dba;
1199     // s1 = c1.s_min;
1200     // s2 = c2.s_min;
1201     // }if(dmax<dbb)
1202     // {
1203     // s1 = c1.s_min;
1204     // s2 = c2.s_max;
1205     // }for(
1206     //
1207     // int i = 0;i<max_iter;++i)
1208     // {
1209     // double t1[2], t2[2], p1[2], p2[2] ;
1210     // c1.eval( s1, c1_offs, p1[0], p1[1] ) ;
1211     // c1.eval_D( s1, c1_offs, t1[0], t1[1] ) ;
1212     // c2.eval( s2, c2_offs, p2[0], p2[1] ) ;
1213     // c2.eval_D( s2, c2_offs, t2[0], t2[1] ) ;
1214     /// *
1215     //// risolvo il sistema
1216     //// p1 + alpha * t1 = p2 + beta * t2
1217     //// alpha * t1 - beta * t2 = p2 - p1
1218     ////
1219     //// / t1[0] -t2[0] \ / alpha \ = / p2[0] - p1[0] \
1220     //// \ t1[1] -t2[1] / \ beta / \ p2[1] - p1[1] /
1221     // */
1222     // double det = t2[0]*t1[1]-t1[0]*t2[1] ;
1223     // double px = p2[0]-p1[0] ;
1224     // double py = p2[1]-p1[1] ;
1225     // s1 += (py*t2[0] - px*t2[1])/det ;
1226     // s2 += (t1[0]*py - t1[1]*px)/det ;
1227     // if ( s1 <= c1.s_min || s1 >= c1.s_max ||
1228     // s2 <= c2.s_min || s2 >= c2.s_max ) break ;
1229     // if ( Math.abs(px) <= tolerance ||
1230     // Math.abs(py) <= tolerance ) return true ;
1231     // }return false;}
1232     //
1233     // void ClothoidCurve::intersect(
1234     //
1235     // double offs, ClothoidCurve const&clot,
1236     //
1237     // double clot_offs, vector<double>&s1,vector<double>&s2,
1238     //
1239     // int max_iter, double tolerance)const
1240     // {
1241     // vector<ClothoidCurve> c0, c1;
1242     // vector<Triangle2D> t0, t1;
1243     // bbSplit(m_pi / 50, (s_max - s_min) / 3, offs, c0, t0);
1244     // clot.bbSplit(m_pi / 50, (clot.s_max - clot.s_min) / 3, clot_offs, c1, t1);
1245     // s1.clear();
1246     // s2.clear();
1247     // for (int i = 0; i < int(c0.size()); ++i)
1248     // {
1249     // for (int j = 0; j < int(c1.size()); ++j)
1250     // {
1251     // if (t0[i].overlap(t1[j]))
1252     // {
1253     // // uso newton per cercare intersezione
1254     // double tmp_s1, tmp_s2;
1255     // bool ok = intersect_internal(c0[i], offs, tmp_s1, c1[j], clot_offs, tmp_s2, max_iter, tolerance);
1256     // if (ok)
1257     // {
1258     // s1.push_back(tmp_s1);
1259     // s2.push_back(tmp_s2);
1260     // }
1261     // }
1262     // }
1263     // }
1264     // }
1265     //
1266     // // collision detection
1267     // bool
1268     // ClothoidCurve::approsimate_collision( double offs,
1269     // ClothoidCurve const & clot,
1270     // double clot_offs,
1271     // double max_angle,
1272     // double max_size ) const {
1273     // vector<ClothoidCurve> c0, c1 ;
1274     // vector<Triangle2D> t0, t1 ;
1275     // bbSplit( max_angle, max_size, offs, c0, t0 ) ;
1276     // clot.bbSplit( max_angle, max_size, clot_offs, c1, t1 ) ;
1277     // for ( int i = 0 ; i < int(c0.size()) ; ++i ) {
1278     // for ( int j = 0 ; j < int(c1.size()) ; ++j ) {
1279     // if ( t0[i].overlap(t1[j]) ) return true ;
1280     // }
1281     // }
1282     // return false ;
1283     // }
1284     //
1285     // void
1286     // ClothoidCurve::rotate( double angle, double cx, double cy ) {
1287     // double dx = x0 - cx ;
1288     // double dy = y0 - cy ;
1289     // double C = cos(angle) ;
1290     // double S = sin(angle) ;
1291     // double ndx = C*dx - S*dy ;
1292     // double ndy = C*dy + S*dx ;
1293     // x0 = cx + ndx ;
1294     // y0 = cy + ndy ;
1295     // theta0 += angle ;
1296     // }
1297     //
1298     // void
1299     // ClothoidCurve::scale( double s ) {
1300     // k /= s ;
1301     // dk /= s*s ;
1302     // s_min *= s ;
1303     // s_max *= s ;
1304     // }
1305     //
1306     // void
1307     // ClothoidCurve::reverse() {
1308     // theta0 = theta0 + m_pi ;
1309     // if ( theta0 > m_pi ) theta0 -= 2*m_pi ;
1310     // k = -k ;
1311     // double tmp = s_max ;
1312     // s_max = -s_min ;
1313     // s_min = -tmp ;
1314     // }
1315     //
1316     // std::ostream&operator<<(std::ostream&stream,ClothoidCurve const&c)
1317     //
1318     // {stream<<"x0 = "<<c.x0<<"\ny0 = "<<c.y0<<"\ntheta0 = "<<c.theta0<<"\nk = "<<c.k<<"\ndk = "<<c.dk<<"\nL =
1319     // "<<c.s_max-c.s_min<<"\ns_min = "<<c.s_min<<"\ns_max = "<<c.s_max<<"\n";return stream;}
1320     //
1321     // static inline bool
1322     //
1323     // power2( double a )
1324     // { return a*a ; }
1325     //
1326     // // **************************************************************************
1327     //
1328     // static
1329     // inline
1330     // bool
1331     //
1332     // solve2x2( double const b[2],
1333     // double A[2][2],
1334     // double x[2] ) {
1335     //// full pivoting
1336     // int ij = 0 ;
1337     // double Amax = Math.abs(A[0][0]) ;
1338     // double tmp = Math.abs(A[0][1]) ;
1339     // if ( tmp > Amax ) { ij = 1 ; Amax = tmp ; }
1340     // tmp = Math.abs(A[1][0]) ;
1341     // if ( tmp > Amax ) { ij = 2 ; Amax = tmp ; }
1342     // tmp = Math.abs(A[1][1]) ;
1343     // if ( tmp > Amax ) { ij = 3 ; Amax = tmp ; }
1344     // if ( Amax == 0 ) return false ;
1345     // int i[] = { 0, 1 } ;
1346     // int j[] = { 0, 1 } ;
1347     // if ( (ij&0x01) == 0x01 ) { j[0] = 1 ; j[1] = 0 ; }
1348     // if ( (ij&0x02) == 0x02 ) { i[0] = 1 ; i[1] = 0 ; }
1349     //// apply factorization
1350     // A[i[1]][j[0]] /= A[i[0]][j[0]] ;
1351     // A[i[1]][j[1]] -= A[i[1]][j[0]]*A[i[0]][j[1]] ;
1352     //// check for singularity
1353     // double epsi = 1e-10 ;
1354     // if ( Math.abs( A[i[1]][j[1]] ) < epsi ) {
1355     //// L^+ Pb
1356     // double tmp = (b[i[0]] + A[i[1]][j[0]]*b[i[1]]) /
1357     // ( (1+power2(A[i[1]][j[0]]) ) *
1358     // ( power2(A[i[0]][j[0]])+power2(A[i[0]][j[1]]) ) ) ;
1359     // x[j[0]] = tmp*A[i[0]][j[0]] ;
1360     // x[j[1]] = tmp*A[i[0]][j[1]] ;
1361     // } else { // non singular
1362     //// L^(-1) Pb
1363     // x[j[0]] = b[i[0]] ;
1364     // x[j[1]] = b[i[1]]-A[i[1]][j[0]]*x[j[0]] ;
1365     //// U^(-1) x
1366     // x[j[1]] /= A[i[1]][j[1]] ;
1367     // x[j[0]] = (x[j[0]]-A[i[0]][j[1]]*x[j[1]])/A[i[0]][j[0]] ;
1368     // }
1369     // return true ;
1370     // }
1371     //
1372     //// **************************************************************************
1373     //
1374     // void
1375     // G2data::setup( double _x0,
1376     // double _y0,
1377     // double _theta0,
1378     // double _kappa0,
1379     // double _x1,
1380     // double _y1,
1381     // double _theta1,
1382     // double _kappa1 ) {
1383     //
1384     // x0 = _x0 ;
1385     // y0 = _y0 ;
1386     // theta0 = _theta0;
1387     // kappa0 = _kappa0 ;
1388     // x1 = _x1 ;
1389     // y1 = _y1 ;
1390     // theta1 = _theta1 ;
1391     // kappa1 = _kappa1 ;
1392     //
1393     //// scale problem
1394     // double dx = x1 - x0 ;
1395     // double dy = y1 - y0 ;
1396     // phi = atan2( dy, dx ) ;
1397     // Lscale = 2/hypot( dx, dy ) ;
1398     //
1399     // th0 = theta0 - phi ;
1400     // th1 = theta1 - phi ;
1401     //
1402     // k0 = kappa0/Lscale ;
1403     // k1 = kappa1/Lscale ;
1404     //
1405     // DeltaK = k1 - k0 ;
1406     // DeltaTheta = th1 - th0 ;
1407     // }
1408     //
1409     // void
1410     // G2data::setTolerance( double tol ) {
1411     // CLOTHOID_ASSERT( tol > 0 && tol <= 0.1,
1412     // "setTolerance, tolerance = " << tol << " must be in (0,0.1]" ) ;
1413     // tolerance = tol ;
1414     // }
1415     //
1416     // void
1417     // G2data::setMaxIter( int miter ) {
1418     // CLOTHOID_ASSERT( miter > 0 && miter <= 1000,
1419     // "setMaxIter, maxIter = " << miter << " must be in [1,1000]" ) ;
1420     // maxIter = miter ;
1421     // }
1422     //
1423     // // **************************************************************************
1424     //
1425     // void
1426     // G2solve2arc::evalA( double alpha,
1427     // double L,
1428     // double & A,
1429     // double & A_1,
1430     // double & A_2 ) const {
1431     // double K = k0+k1 ;
1432     // double aK = alpha*DeltaK ;
1433     // A = alpha*(L*(aK-K)+2*DeltaTheta) ;
1434     // A_1 = (2*aK-K)*L+2*DeltaTheta;
1435     // A_2 = alpha*(aK-K) ;
1436     // }
1437     //
1438     // void
1439     // G2solve2arc::evalG( double alpha,
1440     // double L,
1441     // double th,
1442     // double k,
1443     // double G[2],
1444     // double G_1[2],
1445     // double G_2[2] ) const {
1446     //
1447     // double A, A_1, A_2, X[3], Y[3] ;
1448     // evalA( alpha, L, A, A_1, A_2 ) ;
1449     // double ak = alpha*k ;
1450     // double Lk = L*k ;
1451     // GeneralizedFresnelCS( 3, A, ak*L, th, X, Y );
1452     //
1453     // G[0] = alpha*X[0] ;
1454     // G_1[0] = X[0]-alpha*(Y[2]*A_1/2+Y[1]*Lk) ;
1455     // G_2[0] = -alpha*(Y[2]*A_2/2+Y[1]*ak) ;
1456     //
1457     // G[1] = alpha*Y[0] ;
1458     // G_1[1] = Y[0]+alpha*(X[2]*A_1/2+X[1]*Lk) ;
1459     // G_2[1] = alpha*(X[2]*A_2/2+X[1]*ak) ;
1460     //
1461     // }
1462     //
1463     // void
1464     // G2solve2arc::evalFJ( double const vars[2],
1465     // double F[2],
1466     // double J[2][2] ) const {
1467     //
1468     // double alpha = vars[0] ;
1469     // double L = vars[1] ;
1470     // double G[2], G_1[2], G_2[2] ;
1471     //
1472     // evalG( alpha, L, th0, k0, G, G_1, G_2 ) ;
1473     //
1474     // F[0] = G[0] - 2/L ; F[1] = G[1] ;
1475     // J[0][0] = G_1[0] ; J[1][0] = G_1[1] ;
1476     // J[0][1] = G_2[0] + 2/(L*L) ; J[1][1] = G_2[1] ;
1477     //
1478     // evalG( alpha-1, L, th1, k1, G, G_1, G_2 ) ;
1479     // F[0] -= G[0] ; F[1] -= G[1] ;
1480     // J[0][0] -= G_1[0] ; J[1][0] -= G_1[1] ;
1481     // J[0][1] -= G_2[0] ; J[1][1] -= G_2[1] ;
1482     // }
1483     //
1484     //// ---------------------------------------------------------------------------
1485     //
1486     // bool
1487     // G2solve2arc::solve() {
1488     // double X[2] = { 0.5, 2 } ;
1489     // bool converged = false ;
1490     // for ( int i = 0 ; i < maxIter && !converged ; ++i ) {
1491     // double F[2], J[2][2], d[2] ;
1492     // evalFJ( X, F, J ) ;
1493     // if ( !solve2x2( F, J, d ) ) break ;
1494     // double lenF = hypot(F[0],F[1]) ;
1495     // X[0] -= d[0] ;
1496     // X[1] -= d[1] ;
1497     // converged = lenF < tolerance ;
1498     // }
1499     // if ( converged ) converged = X[1] > 0 && X[0] > 0 && X[0] < 1 ;
1500     // if ( converged ) buildSolution( X[0], X[1] ) ;
1501     // return converged ;
1502     // }
1503     //
1504     // // **************************************************************************
1505     //
1506     // void
1507     // G2solve2arc::buildSolution( double alpha, double L ) {
1508     // double beta = 1-alpha ;
1509     // double LL = L/Lscale ;
1510     // double s0 = LL*alpha ;
1511     // double s1 = LL*beta ;
1512     //
1513     // double tmp = k0*alpha+k1*beta-2*DeltaTheta/L ;
1514     //
1515     // double dk0 = -Lscale*(k0+tmp)/s0 ;
1516     // double dk1 = Lscale*(k1+tmp)/s1 ;
1517     //
1518     // S0.setup( x0, y0, theta0, kappa0, dk0, 0, s0 ) ;
1519     // S1.setup( x1, y1, theta1, kappa1, dk1, -s1, 0 ) ;
1520     // S1.change_origin( -s1 ) ;
1521     // }
1522     //
1523     // // **************************************************************************
1524     //
1525     // void
1526     // G2solve3arc::setup( double _x0,
1527     // double _y0,
1528     // double _theta0,
1529     // double _kappa0,
1530     // double _frac0,
1531     // double _x1,
1532     // double _y1,
1533     // double _theta1,
1534     // double _kappa1,
1535     // double _frac1 ) {
1536     // G2data::setup( _x0, _y0, _theta0, _kappa0, _x1, _y1, _theta1, _kappa1 ) ;
1537     //
1538     // double tmp = 1/(2-(_frac0+_frac1)) ;
1539     // alpha = _frac0*tmp ;
1540     // beta = _frac1*tmp ;
1541     //
1542     // gamma = (1-alpha-beta)/4 ;
1543     // gamma2 = gamma*gamma ;
1544     //
1545     // a0 = alpha*k0 ;
1546     // b1 = beta*k1 ;
1547     //
1548     // double ab = alpha-beta ;
1549     //
1550     // dK0_0 = 2*alpha*DeltaTheta ;
1551     // dK0_1 = -alpha*(k0+a0+b1) ;
1552     // dK0_2 = -alpha*gamma*(beta-alpha+1) ;
1553     //
1554     // dK1_0 = -2*beta*DeltaTheta ;
1555     // dK1_1 = beta*(k1+a0+b1) ;
1556     // dK1_2 = beta*gamma*(beta-alpha-1) ;
1557     //
1558     // KM_0 = 2*gamma*DeltaTheta ;
1559     // KM_1 = -gamma*(a0+b1) ;
1560     // KM_2 = gamma2*(alpha-beta) ;
1561     //
1562     // thM_0 = (ab*DeltaTheta+(th0+th1))/2 ;
1563     // thM_1 = (a0-b1-ab*(a0+b1))/4 ;
1564     // thM_2 = (gamma*(2*ab*ab-alpha-beta-1))/8 ;
1565     // }
1566     //
1567     // void
1568     // G2solve3arc::evalFJ( double const vars[2],
1569     // double F[2],
1570     // double J[2][2] ) const {
1571     //
1572     // double eta = vars[0] ;
1573     // double zeta = vars[1] ;
1574     //
1575     // double dK0 = dK0_0 + eta*dK0_1 + zeta*dK0_2 ;
1576     // double dK1 = dK1_0 + eta*dK1_1 + zeta*dK1_2 ;
1577     // double KM = KM_0 + eta*KM_1 + zeta*KM_2 ;
1578     // double thM = thM_0 + eta*thM_1 + zeta*thM_2 ;
1579     //
1580     // double xa[3], ya[3], xb[3], yb[3], xM[3], yM[3], xP[3], yP[3] ;
1581     //
1582     // GeneralizedFresnelCS( 3, dK0, a0*eta, th0, xa, ya );
1583     // GeneralizedFresnelCS( 3, dK1, -b1*eta, th1, xb, yb );
1584     // GeneralizedFresnelCS( 3, gamma2*zeta, -KM, thM, xM, yM );
1585     // GeneralizedFresnelCS( 3, gamma2*zeta, KM, thM, xP, yP );
1586     //
1587     // F[0] = alpha*xa[0] + beta*xb[0] + gamma*(xM[0]+xP[0]) - 2/eta ;
1588     // F[1] = alpha*ya[0] + beta*yb[0] + gamma*(yM[0]+yP[0]) ;
1589     //
1590     //// D F[0] / D eta
1591     // J[0][0] = - alpha*(ya[2]*dK0_1/2+ya[1]*a0)
1592     // - beta*(yb[2]*dK1_1/2-yb[1]*b1)
1593     // + gamma * ((yM[1]-yP[1])*KM_1-(yM[0]+yP[0])*thM_1)
1594     // + 2/(eta*eta) ;
1595     //
1596     //// D F[0] / D zeta
1597     // J[0][1] = - alpha*(ya[2]*dK0_2/2) - beta*(yb[2]*dK1_2/2)
1598     // - gamma*( (yM[2]+yP[2])*gamma2/2+(yP[1]-yM[1])*KM_2+(yP[0]+yM[0])*thM_2 ) ;
1599     //
1600     //// D F[1] / D eta
1601     // J[1][0] = alpha*(xa[2]*dK0_1/2+xa[1]*a0) +
1602     // beta*(xb[2]*dK1_1/2-xb[1]*b1) +
1603     // gamma * ((xP[1]-xM[1])*KM_1+(xM[0]+xP[0])*thM_1) ;
1604     //
1605     //// D F[1] / D zeta
1606     // J[1][1] = alpha*(xa[2]*dK0_2/2) + beta*(xb[2]*dK1_2/2)
1607     // + gamma * ( (xM[2]+xP[2])*gamma2/2+(xP[1]-xM[1])*KM_2+(xP[0]+xM[0])*thM_2 ) ;
1608     //
1609     // }
1610     //
1611     //// ---------------------------------------------------------------------------
1612     //
1613     // bool
1614     // G2solve3arc::solve() {
1615     // double X[2] = { 2, 0 } ; // eta, zeta
1616     // bool converged = false ;
1617     // for ( int i = 0 ; i < maxIter && !converged ; ++i ) {
1618     // double F[2], J[2][2], d[2] ;
1619     // evalFJ( X, F, J ) ;
1620     // if ( !solve2x2( F, J, d ) ) break ;
1621     // double lenF = hypot(F[0],F[1]) ;
1622     // X[0] -= d[0] ;
1623     // X[1] -= d[1] ;
1624     // converged = lenF < tolerance ;
1625     // }
1626     // if ( converged ) converged = X[0] > 0 ; // eta > 0 !
1627     // if ( converged ) buildSolution( X[0], X[1] ) ;
1628     // return converged ;
1629     // }
1630     //
1631     // void
1632     // G2solve3arc::buildSolution( double eta, double zeta ) {
1633     //
1634     // double L0 = eta*alpha ;
1635     // double L1 = eta*beta ;
1636     // double LM = eta*gamma ;
1637     //
1638     // double dkappaM = zeta*gamma2 ; // /(eta*eta)*LM*LM ;
1639     // double dkappa0A = dK0_0 + eta*dK0_1 + zeta*dK0_2 ;
1640     // double dkappa1B = dK1_0 + eta*dK1_1 + zeta*dK1_2 ;
1641     // double kappaM = KM_0 + eta*KM_1 + zeta*KM_2 ;
1642     // double thetaM = thM_0 + eta*thM_1 + zeta*thM_2 ;
1643     //
1644     // double xa, ya, xmL, ymL ;
1645     // GeneralizedFresnelCS( dkappa0A, k0*L0, th0, xa, ya );
1646     // GeneralizedFresnelCS( dkappaM, -kappaM, thetaM, xmL, ymL );
1647     //
1648     // double xM = L0 * xa + LM * xmL - 1 ;
1649     // double yM = L0 * ya + LM * ymL ;
1650     //
1651     //// rovescia scalatura
1652     // L0 /= Lscale ;
1653     // L1 /= Lscale ;
1654     // LM /= Lscale ;
1655     //
1656     // dkappa0A /= L0*L0 ;
1657     // dkappa1B /= L1*L1 ;
1658     // dkappaM /= LM*LM ;
1659     // kappaM /= LM ;
1660     //
1661     // S0.setup( x0, y0, theta0, kappa0, dkappa0A, 0, L0 ) ;
1662     // S1.setup( x1, y1, theta1, kappa1, dkappa1B, -L1, 0 ) ;
1663     //
1664     //// la trasformazione inversa da [-1,1] a (x0,y0)-(x1,y1)
1665     //// g(x,y) = RotInv(phi)*(1/lambda*[X;Y] - [xbar;ybar]) = [x;y]
1666     //
1667     // double C = cos(phi) ;
1668     // double S = sin(phi) ;
1669     // double dx = (xM+1)/Lscale ;
1670     // double dy = yM/Lscale ;
1671     // SM.setup( x0 + C * dx - S * dy, y0 + C * dy + S * dx,
1672     // thetaM+phi, kappaM, dkappaM, -LM, LM ) ;
1673     //
1674     //// Sguess.setup_G1( x0_orig, y0_orig, theta0_orig,
1675     //// x1_orig, y1_orig, theta1_orig ) ;
1676     //
1677     // S1.change_origin( -L1 ) ;
1678     // SM.change_origin( -LM ) ;
1679     // }
1680     //
1681     // }
1682 }