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 X double[]; ?
356 * @param Y double[]; ?
357 */
358 private static void evalXYaLarge(final int nk, final double a, final double b, double[] X, double[] Y)
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 X[0] = cg * dC0 - s * sg * dS0;
382 Y[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 X[1] = cg * DC - s * sg * DS;
392 Y[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 X[2] = cg * DC - s * sg * DS;
402 Y[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 X double[]; ?
435 * @param Y double[]; ?
436 */
437 private static void evalXYazero(final int nk, final double b, double X[], double Y[])
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 X[0] = 1 - (b2 / 6) * (1 - (b2 / 20) * (1 - (b2 / 42)));
445 Y[0] = (b / 2) * (1 - (b2 / 12) * (1 - (b2 / 30)));
446 }
447 else
448 {
449 X[0] = sb / b;
450 Y[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 X[k] = (sb - k * Y[k - 1]) / b;
465 Y[k] = (k * X[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 X[k] = (k * A * rLa + B * rLb + cb) / (1 + k);
481 Y[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 }