Clothoid.java
package org.djutils.draw.line;
import org.djutils.complex.Complex;
import org.djutils.draw.DrawRuntimeException;
import org.djutils.exceptions.Throw;
import org.djutils.polynomialroots.PolynomialRoots;
/**
* Approximate a clothoid with a PolyLine3d. <br>
* Derived from https://github.com/ebertolazzi/G1fitting/blob/master/src/Clothoid.cc
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
*/
public final class Clothoid
{
/** Utility class. */
private Clothoid()
{
// do not instantiate
}
/** ??? */
static final double A_THRESOLD = 0.01;
/** ??? */
static final int A_SERIE_SIZE = 3;
//@formatter:off
/** Fresnel coefficients FN. */
static final double[] FN =
{
0.49999988085884732562,
1.3511177791210715095,
1.3175407836168659241,
1.1861149300293854992,
0.7709627298888346769,
0.4173874338787963957,
0.19044202705272903923,
0.06655998896627697537,
0.022789258616785717418,
0.0040116689358507943804,
0.0012192036851249883877
};
/** Fresnel coefficients FD. */
static final double[] FD =
{
1.0,
2.7022305772400260215,
4.2059268151438492767,
4.5221882840107715516,
3.7240352281630359588,
2.4589286254678152943,
1.3125491629443702962,
0.5997685720120932908,
0.20907680750378849485,
0.07159621634657901433,
0.012602969513793714191,
0.0038302423512931250065
};
/** Fresnel coefficients GN. */
static final double[] GN =
{
0.50000014392706344801,
0.032346434925349128728,
0.17619325157863254363,
0.038606273170706486252,
0.023693692309257725361,
0.007092018516845033662,
0.0012492123212412087428,
0.00044023040894778468486,
-8.80266827476172521e-6,
-1.4033554916580018648e-8,
2.3509221782155474353e-10
};
/** Fresnel coefficients GD. */
static final double[] GD =
{
1.0,
2.0646987497019598937,
2.9109311766948031235,
2.6561936751333032911,
2.0195563983177268073,
1.1167891129189363902,
0.57267874755973172715,
0.19408481169593070798,
0.07634808341431248904,
0.011573247407207865977,
0.0044099273693067311209,
-0.00009070958410429993314
};
/** Pi. */
static final double m_pi = Math.PI;
/** Half Pi. */
static final double m_pi_2 = Math.PI / 2;
/** Two Pi. */
static final double m_2pi = 2 * Math.PI;
/** One over Pi. */
static final double m_1_pi = 1 / Math.PI;
/** One over square root of Pi. */
static final double m_1_sqrt_pi = 1 / Math.sqrt(Math.PI);
/*
* #######
* # ##### ###### #### # # ###### #
* # # # # # ## # # #
* ##### # # ##### #### # # # ##### #
* # ##### # # # # # # #
* # # # # # # # ## # #
* # # # ###### #### # # ###### ######
*/
//@formatter:on
/**
* Purpose: This program computes the Fresnel integrals.
*
* <pre>
* C(x) and S(x) using subroutine FCS
* Input : x --- Argument of C(x) and S(x)
* Output: C --- C(x)
* S --- S(x)
* Example:
* x C(x) S(x)
* -----------------------------------
* 0.0 .00000000 .00000000
* 0.5 .49234423 .06473243
* 1.0 .77989340 .43825915
* 1.5 .44526118 .69750496
* 2.0 .48825341 .34341568
* 2.5 .45741301 .61918176
* Purpose: Compute Fresnel integrals C(x) and S(x)
* Input : x --- Argument of C(x) and S(x)
* Output: C --- C(x)
* S --- S(x)
* </pre>
*
* @param y double;
* @return double[]; double array with two elements; C is stored in the first, S in the second
*/
private static double[] fresnelCS(final double y)
{
final double eps = 1E-15;
final double x = y > 0 ? y : -y;
double resultC;
double resultS;
if (x < 1.0)
{
double twofn, fact, denterm, numterm, sum, term;
final double s = m_pi_2 * (x * x);
final double t = -s * s;
// Cosine integral series
twofn = 0.0;
fact = 1.0;
denterm = 1.0;
numterm = 1.0;
sum = 1.0;
do
{
twofn += 2.0;
fact *= twofn * (twofn - 1.0);
denterm += 4.0;
numterm *= t;
term = numterm / (fact * denterm);
sum += term;
}
while (Math.abs(term) > eps * Math.abs(sum));
resultC = x * sum;
// Sine integral series
twofn = 1.0;
fact = 1.0;
denterm = 3.0;
numterm = 1.0;
sum = 1.0 / 3.0;
do
{
twofn += 2.0;
fact *= twofn * (twofn - 1.0);
denterm += 4.0;
numterm *= t;
term = numterm / (fact * denterm);
sum += term;
}
while (Math.abs(term) > eps * Math.abs(sum));
resultS = m_pi_2 * sum * (x * x * x);
}
else if (x < 6.0)
{
// Rational approximation for f
double sumn = 0.0;
double sumd = FD[11];
for (int k = 10; k >= 0; --k)
{
sumn = FN[k] + x * sumn;
sumd = FD[k] + x * sumd;
}
double f = sumn / sumd;
// Rational approximation for g
sumn = 0.0;
sumd = GD[11];
for (int k = 10; k >= 0; --k)
{
sumn = GN[k] + x * sumn;
sumd = GD[k] + x * sumd;
}
double g = sumn / sumd;
double u = m_pi_2 * (x * x);
double sinU = Math.sin(u);
double cosU = Math.cos(u);
resultC = 0.5 + f * sinU - g * cosU;
resultS = 0.5 - f * cosU - g * sinU;
}
else
{
double absterm;
// x >= 6; asymptotic expansions for f and g
final double s = m_pi * x * x;
final double t = -1 / (s * s);
// Expansion for f
double numterm = -1.0;
double term = 1.0;
double sum = 1.0;
double oldterm = 1.0;
double eps10 = 0.1 * eps;
do
{
numterm += 4.0;
term *= numterm * (numterm - 2.0) * t;
sum += term;
absterm = Math.abs(term);
Throw.when(false, oldterm >= absterm, DrawRuntimeException.class,
"In FresnelCS f not converged to eps, x = " + x + " oldterm = " + oldterm + " absterm = " + absterm);
oldterm = absterm;
}
while (absterm > eps10 * Math.abs(sum));
double f = sum / (m_pi * x);
// Expansion for g
numterm = -1.0;
term = 1.0;
sum = 1.0;
oldterm = 1.0;
do
{
numterm += 4.0;
term *= numterm * (numterm + 2.0) * t;
sum += term;
absterm = Math.abs(term);
Throw.when(oldterm >= absterm, DrawRuntimeException.class, "In FresnelCS g does not converge to eps, x = " + x
+ " oldterm = " + oldterm + " absterm = " + absterm);
oldterm = absterm;
}
while (absterm > eps10 * Math.abs(sum));
double g = m_pi * x;
g = sum / (g * g * x);
double u = m_pi_2 * (x * x);
double sinU = Math.sin(u);
double cosU = Math.cos(u);
resultC = 0.5 + f * sinU - g * cosU;
resultS = 0.5 - f * cosU - g * sinU;
}
if (y < 0)
{
resultC = -resultC;
resultS = -resultS;
}
return new double[] { resultC, resultS };
}
/**
* ???
* @param nk int; size of the provided arrays
* @param t double;
* @param C double[]; should have length nk
* @param S double[]; shouldhave laength nk
*/
private static void fresnelCS(final int nk, final double t, final double[] C, final double[] S)
{
double[] cs = fresnelCS(t);
C[0] = cs[0];
S[0] = cs[1];
if (nk > 1)
{
double tt = m_pi_2 * (t * t);
double ss = Math.sin(tt);
double cc = Math.cos(tt);
C[1] = ss * m_1_pi;
S[1] = (1 - cc) * m_1_pi;
if (nk > 2)
{
C[2] = (t * ss - S[0]) * m_1_pi;
S[2] = (C[0] - t * cc) * m_1_pi;
}
}
}
/**
* ???
* @param a double;
* @param b double;
* @return double[] with two elements set to X and Y
*/
private static double[] evalXYaLarge(final double a, final double b)
{
double s = a > 0 ? +1 : -1;
double absa = Math.abs(a);
double z = m_1_sqrt_pi * Math.sqrt(absa);
double ell = s * b * m_1_sqrt_pi / Math.sqrt(absa);
double g = -0.5 * s * (b * b) / absa;
double cg = Math.cos(g) / z;
double sg = Math.sin(g) / z;
// double Cl, Sl, Cz, Sz;
double[] resultL = fresnelCS(ell);
double[] resultZ = fresnelCS(ell + z);
double dC0 = resultZ[0] - resultL[0];
double dS0 = resultZ[1] - resultL[1];
double x = cg * dC0 - s * sg * dS0;
double y = sg * dC0 + s * cg * dS0;
return new double[] { x, y };
}
// -------------------------------------------------------------------------
// nk max 3
/**
* ???
* @param nk int; minimum 0; maximum 3
* @param a double; ?
* @param b double; ?
* @param xArray double[]; ?
* @param yArray double[]; ?
*/
private static void evalXYaLarge(final int nk, final double a, final double b, final double[] xArray, final double[] yArray)
{
Throw.when(nk <= 0 || nk >= 4, DrawRuntimeException.class,
"In evalXYaLarge first argument nk must be in 1..3, nk " + nk);
double s = a > 0 ? +1 : -1;
double absa = Math.abs(a);
double z = m_1_sqrt_pi * Math.sqrt(absa);
double ell = s * b * m_1_sqrt_pi / Math.sqrt(absa);
double g = -0.5 * s * (b * b) / absa;
double cg = Math.cos(g) / z;
double sg = Math.sin(g) / z;
double[] cl = new double[3];
double[] sl = new double[3];
double[] cz = new double[3];
double[] sz = new double[3];
fresnelCS(nk, ell, cl, sl);
fresnelCS(nk, ell + z, cz, sz);
double dC0 = cz[0] - cl[0];
double dS0 = sz[0] - sl[0];
xArray[0] = cg * dC0 - s * sg * dS0;
yArray[0] = sg * dC0 + s * cg * dS0;
if (nk > 1)
{
cg /= z;
sg /= z;
double dC1 = cz[1] - cl[1];
double dS1 = sz[1] - sl[1];
double DC = dC1 - ell * dC0;
double DS = dS1 - ell * dS0;
xArray[1] = cg * DC - s * sg * DS;
yArray[1] = sg * DC + s * cg * DS;
if (nk > 2)
{
double dC2 = cz[2] - cl[2];
double dS2 = sz[2] - sl[2];
DC = dC2 + ell * (ell * dC0 - 2 * dC1);
DS = dS2 + ell * (ell * dS0 - 2 * dS1);
cg = cg / z;
sg = sg / z;
xArray[2] = cg * DC - s * sg * DS;
yArray[2] = sg * DC + s * cg * DS;
}
}
}
/**
* LommelReduced.
* @param mu double; ?
* @param nu double; ?
* @param b double; ?
* @return double; ?
*/
private static double LommelReduced(final double mu, final double nu, final double b)
{
double tmp = 1 / ((mu + nu + 1) * (mu - nu + 1));
double res = tmp;
for (int n = 1; n <= 100; ++n)
{
tmp *= (-b / (2 * n + mu - nu + 1)) * (b / (2 * n + mu + nu + 1));
res += tmp;
if (Math.abs(tmp) < Math.abs(res) * 1e-50)
{
break;
}
}
return res;
}
/**
* ???
* @param nk int; ?
* @param b double; ?
* @param zArray double[]; ?
* @param yArray double[]; ?
*/
private static void evalXYazero(final int nk, final double b, final double[] zArray, final double[] yArray)
{
double sb = Math.sin(b);
double cb = Math.cos(b);
double b2 = b * b;
if (Math.abs(b) < 1e-3)
{
zArray[0] = 1 - (b2 / 6) * (1 - (b2 / 20) * (1 - (b2 / 42)));
yArray[0] = (b / 2) * (1 - (b2 / 12) * (1 - (b2 / 30)));
}
else
{
zArray[0] = sb / b;
yArray[0] = (1 - cb) / b;
}
// use recurrence in the stable part
int m = (int) Math.floor(2 * b);
if (m >= nk)
{
m = nk - 1;
}
if (m < 1)
{
m = 1;
}
for (int k = 1; k < m; ++k)
{
zArray[k] = (sb - k * yArray[k - 1]) / b;
yArray[k] = (k * zArray[k - 1] - cb) / b;
}
// use Lommel for the unstable part
if (m < nk)
{
double A = b * sb;
double D = sb - b * cb;
double B = b * D;
double C = -b2 * sb;
double rLa = LommelReduced(m + 0.5, 1.5, b);
double rLd = LommelReduced(m + 0.5, 0.5, b);
for (int k = m; k < nk; ++k)
{
double rLb = LommelReduced(k + 1.5, 0.5, b);
double rLc = LommelReduced(k + 1.5, 1.5, b);
zArray[k] = (k * A * rLa + B * rLb + cb) / (1 + k);
yArray[k] = (C * rLc + sb) / (2 + k) + D * rLd;
rLa = rLc;
rLd = rLb;
}
}
}
/**
* ???
* @param a double; ?
* @param b double; ?
* @param p double; ?
* @return double[]; containing the two values; X and Y.
*/
private static double[] evalXYaSmall(final double a, final double b, final int p)
{
Throw.when(p >= 11 && p <= 0, DrawRuntimeException.class, "In evalXYaSmall p = " + p + " must be in 1..10");
double[] x0 = new double[43];
double[] y0 = new double[43];
int nkk = 4 * p + 3; // max 43
evalXYazero(nkk, b, x0, y0);
double x = x0[0] - (a / 2) * y0[2];
double y = y0[0] + (a / 2) * x0[2];
double t = 1;
double aa = -a * a / 4; // controllare!
for (int n = 1; n <= p; ++n)
{
t *= aa / (2 * n * (2 * n - 1));
double bf = a / (4 * n + 2);
int jj = 4 * n;
x += t * (x0[jj] - bf * y0[jj + 2]);
y += t * (y0[jj] + bf * x0[jj + 2]);
}
return new double[] { x, y };
}
/**
* ???
* @param nk int; ?
* @param a double; ?
* @param b double; ?
* @param p double; ?
* @param x double[]; ?
* @param y double[]; ?
*/
private static void evalXYaSmall(final int nk, final double a, final double b, final int p, final double[] x,
final double[] y)
{
int nkk = nk + 4 * p + 2; // max 45
double[] x0 = new double[45];
double[] y0 = new double[45];
Throw.when(nkk >= 46, DrawRuntimeException.class,
"In evalXYaSmall (nk,p) = (" + nk + "," + p + ")\n" + "nk + 4*p + 2 = " + nkk + " must be less than 46\n");
evalXYazero(nkk, b, x0, y0);
for (int j = 0; j < nk; ++j)
{
x[j] = x0[j] - (a / 2) * y0[j + 2];
y[j] = y0[j] + (a / 2) * x0[j + 2];
}
double t = 1;
double aa = -a * a / 4; // controllare!
for (int n = 1; n <= p; ++n)
{
t *= aa / (2 * n * (2 * n - 1));
double bf = a / (4 * n + 2);
for (int j = 0; j < nk; ++j)
{
int jj = 4 * n + j;
x[j] += t * (x0[jj] - bf * y0[jj + 2]);
y[j] += t * (y0[jj] + bf * x0[jj + 2]);
}
}
}
/**
* ???
* @param a double; ?
* @param b double; ?
* @param c double; ?
* @return double[]; size two containing C and S
*/
private static double[] GeneralizedFresnelCS(final double a, final double b, final double c)
{
double[] xxyy = Math.abs(a) < A_THRESOLD ? evalXYaSmall(a, b, A_SERIE_SIZE) : evalXYaLarge(a, b);
double cosC = Math.cos(c);
double sinC = Math.sin(c);
// FIXME: Confusing names
double intC = xxyy[0] * cosC - xxyy[1] * sinC;
double intS = xxyy[0] * sinC + xxyy[1] * cosC;
return new double[] { intC, intS };
}
/**
* ???
* @param nk int; ?
* @param a double; ?
* @param b double; ?
* @param c double; ?
* @param intC double[]; stores C results
* @param intS double[]; stores S results
*/
static void GeneralizedFresnelCS(final int nk, final double a, final double b, final double c, final double[] intC,
final double[] intS)
{
Throw.when(nk <= 0 || nk >= 4, DrawRuntimeException.class, "nk = " + nk + " must be in 1..3");
if (Math.abs(a) < A_THRESOLD)
{
evalXYaSmall(nk, a, b, A_SERIE_SIZE, intC, intS);
}
else
{
evalXYaLarge(nk, a, b, intC, intS);
}
double cosC = Math.cos(c);
double sinC = Math.sin(c);
for (int k = 0; k < nk; ++k)
{
double xx = intC[k];
double yy = intS[k];
intC[k] = xx * cosC - yy * sinC;
intS[k] = xx * sinC + yy * cosC;
}
}
/** CF coefficients. */
private static final double[] CF = { 2.989696028701907, 0.716228953608281, -0.458969738821509, -0.502821153340377,
0.261062141752652, -0.045854475238709 };
/**
* Create a clothoid connecting (x0,y0) to (x1,y1) having direction theta0 at the start point and theta1 at the end point.
* @param x0 double; x coordinate of the start point
* @param y0 double; y coordinate of the start point
* @param theta0 double; direction at the start point (in radians)
* @param x1 double; x coordinate of the end point
* @param y1 double; y coordinate of the end point
* @param theta1 double; direction at the end point (in radians)
* @return int; the number of iterations
*/
public static int buildClothoid(final double x0, final double y0, final double theta0, final double x1, final double y1,
final double theta1)
{
double k;
double dk;
double l;
// traslazione in (0,0)
double dx = x1 - x0;
double dy = y1 - y0;
double r = Math.hypot(dx, dy);
double phi = Math.atan2(dy, dx);
double phi0 = theta0 - phi;
double phi1 = theta1 - phi;
phi0 -= m_2pi * Math.rint(phi0 / m_2pi);
phi1 -= m_2pi * Math.rint(phi1 / m_2pi);
if (phi0 > m_pi)
{
phi0 -= m_2pi;
}
if (phi0 < -m_pi)
{
phi0 += m_2pi;
}
if (phi1 > m_pi)
{
phi1 -= m_2pi;
}
if (phi1 < -m_pi)
{
phi1 += m_2pi;
}
double delta = phi1 - phi0;
// punto iniziale
double x = phi0 * m_1_pi;
double y = phi1 * m_1_pi;
double xy = x * y;
y *= y;
x *= x;
double a =
(phi0 + phi1) * (CF[0] + xy * (CF[1] + xy * CF[2]) + (CF[3] + xy * CF[4]) * (x + y) + CF[5] * (x * x + y * y));
// newton
double g = 0;
double dg;
double[] intC = new double[3];
double[] intS = new double[3];
int niter = 0;
do
{
GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
g = intS[0];
dg = intC[2] - intC[1];
a -= g / dg;
}
while (++niter <= 10 && Math.abs(g) > 1E-12);
Throw.when(Math.abs(g) > 1E-8, DrawRuntimeException.class, "Newton did not converge, g = " + g + " niter = " + niter);
double[] cs = GeneralizedFresnelCS(2 * a, delta - a, phi0);
intC[0] = cs[0];
intS[0] = cs[1];
l = r / intC[0];
Throw.when(l <= 0, DrawRuntimeException.class, "Negative length L = " + l);
k = (delta - a) / l;
dk = 2 * a / l / l;
return niter;
}
/**
* Create a clothoid connecting (x0,y0) to (x1,y1) having direction theta0 at the start point and theta1 at the end point.
* @param x0 double; x coordinate of the start point
* @param y0 double; y coordinate of the start point
* @param theta0 double; direction at the start point (in radians)
* @param x1 double; x coordinate of the end point
* @param y1 double; y coordinate of the end point
* @param theta1 double; direction at the end point (in radians)
* @return int; the number of iterations
*/
public static int buildClothoidMoreResults(final double x0, final double y0, final double theta0, final double x1,
final double y1, final double theta1)
{
double k;
double dk;
double l;
double k_1;
double dk_1;
double l_1;
double k_2;
double dk_2;
double l_2;
// traslazione in (0,0)
double dx = x1 - x0;
double dy = y1 - y0;
double r = Math.hypot(dx, dy);
double phi = Math.atan2(dy, dx);
double phi0 = theta0 - phi;
double phi1 = theta1 - phi;
phi0 -= m_2pi * Math.rint(phi0 / m_2pi);
phi1 -= m_2pi * Math.rint(phi1 / m_2pi);
if (phi0 > m_pi)
{
phi0 -= m_2pi;
}
if (phi0 < -m_pi)
{
phi0 += m_2pi;
}
if (phi1 > m_pi)
{
phi1 -= m_2pi;
}
if (phi1 < -m_pi)
{
phi1 += m_2pi;
}
double delta = phi1 - phi0;
// punto iniziale
double x = phi0 * m_1_pi;
double y = phi1 * m_1_pi;
double xy = x * y;
y *= y;
x *= x;
double a =
(phi0 + phi1) * (CF[0] + xy * (CF[1] + xy * CF[2]) + (CF[3] + xy * CF[4]) * (x + y) + CF[5] * (x * x + y * y));
// newton
double g = 0;
double dg;
double[] intC = new double[3];
double[] intS = new double[3];
int niter = 0;
do
{
GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
g = intS[0];
dg = intC[2] - intC[1];
a -= g / dg;
}
while (++niter <= 10 && Math.abs(g) > 1E-12);
Throw.when(Math.abs(g) > 1E-8, DrawRuntimeException.class, "Newton do not converge, g = " + g + " niter = " + niter);
GeneralizedFresnelCS(3, 2 * a, delta - a, phi0, intC, intS);
l = r / intC[0];
Throw.when(l <= 0, DrawRuntimeException.class, "Negative length L = " + l);
k = (delta - a) / l;
dk = 2 * a / l / l;
double alpha = intC[0] * intC[1] + intS[0] * intS[1];
double beta = intC[0] * intC[2] + intS[0] * intS[2];
double gamma = intC[0] * intC[0] + intS[0] * intS[0];
double tx = intC[1] - intC[2];
double ty = intS[1] - intS[2];
double txy = l * (intC[1] * intS[2] - intC[2] * intS[1]);
double omega = l * (intS[0] * tx - intC[0] * ty) - txy;
delta = intC[0] * tx + intS[0] * ty;
l_1 = omega / delta;
l_2 = txy / delta;
delta *= l;
k_1 = (beta - gamma - k * omega) / delta;
k_2 = -(beta + k * txy) / delta;
delta *= l / 2;
dk_1 = (gamma - alpha - dk * omega * l) / delta;
dk_2 = (alpha - dk * txy * l) / delta;
return niter;
}
// void
// eval(final double s,
// double & theta,
// double & kappa,
// double & x,
// double & y ) const {
// double C, S ;
// GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
// x = x0 + s*C ;
// y = y0 + s*S ;
// theta = theta0 + s*(k+s*(dk/2)) ;
// kappa = k + s*dk ;
// }
//
// void
// ClothoidCurve::eval( double s, double & x, double & y ) const {
// double C, S ;
// GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
// x = x0 + s*C ;
// y = y0 + s*S ;
// }
//
// void
// ClothoidCurve::eval_D( double s, double & x_D, double & y_D ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// x_D = cos(theta) ;
// y_D = sin(theta) ;
// }
//
// void
// ClothoidCurve::eval_DD( double s, double & x_DD, double & y_DD ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double theta_D = k+s*dk ;
// x_DD = -sin(theta)*theta_D ;
// y_DD = cos(theta)*theta_D ;
// }
//
// void
// ClothoidCurve::eval_DDD( double s, double & x_DDD, double & y_DDD ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double theta_D = k+s*dk ;
// double C = cos(theta) ;
// double S = sin(theta) ;
// double th2 = theta_D*theta_D ;
// x_DDD = -C*th2-S*dk ;
// y_DDD = -S*th2+C*dk ;
// }
//
//// offset curve
// void
// ClothoidCurve::eval( double s, double offs, double & x, double & y ) const {
// double C, S ;
// GeneralizedFresnelCS( dk*s*s, k*s, theta0, C, S ) ;
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double nx = -sin(theta) ;
// double ny = cos(theta) ;
// x = x0 + s*C + offs * nx ;
// y = y0 + s*S + offs * ny ;
// }
//
// void
// ClothoidCurve::eval_D( double s, double offs, double & x_D, double & y_D ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double theta_D = k+s*dk ;
// double scale = 1-offs*theta_D ;
// x_D = cos(theta)*scale ;
// y_D = sin(theta)*scale ;
// }
//
// void
// ClothoidCurve::eval_DD( double s, double offs, double & x_DD, double & y_DD ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double theta_D = k+s*dk ;
// double C = cos(theta) ;
// double S = sin(theta) ;
// double tmp1 = theta_D*(1-theta_D*offs) ;
// double tmp2 = offs*dk ;
// x_DD = -tmp1*S - C*tmp2 ;
// y_DD = tmp1*C - S*tmp2 ;
// }
//
// void
// ClothoidCurve::eval_DDD( double s, double offs, double & x_DDD, double & y_DDD ) const {
// double theta = theta0 + s*(k+s*(dk/2)) ;
// double theta_D = k+s*dk ;
// double C = cos(theta) ;
// double S = sin(theta) ;
// double tmp1 = theta_D*theta_D*(theta_D*offs-1) ;
// double tmp2 = dk*(1-3*theta_D*offs) ;
// x_DDD = tmp1*C-tmp2*S ;
// y_DDD = tmp1*S+tmp2*C ;
// }
/**
* ???
* @param theta0 double; theta0
* @param theta double; theta
* @return double; kappa
*/
private static double kappa(final double theta0, final double theta)
{
double x = theta0 * theta0;
double a = -3.714 + x * 0.178;
double b = -1.913 - x * 0.0753;
double c = 0.999 + x * 0.03475;
double d = 0.191 - x * 0.00703;
double e = 0.500 - x * -0.00172;
double t = d * theta0 + e * theta;
return a * theta0 + b * theta + c * (t * t * t);
}
/**
* theta_guess.
* <p>
* FIXME value parameter ok;
* @param theta0 double; theta0
* @param k0 double; k0
* @return double; theta
*/
private static double theta_guess(final double theta0, final double k0)
{
double x = theta0 * theta0;
double a = -3.714 + x * 0.178;
double b = -1.913 - x * 0.0753;
double c = 0.999 + x * 0.03475;
double d = 0.191 - x * 0.00703;
double e = 0.500 - x * -0.00172;
double e2 = e * e;
double dt = d * theta0;
double dt2 = dt * dt;
double qA = c * e * e2;
double qB = 3 * (c * d * e2 * theta0);
double qC = 3 * c * e * dt2 + b;
double qD = c * (dt * dt2) + a * theta0 - k0;
boolean ok;
Complex[] roots = PolynomialRoots.cubicRoots(qA, qB, qC, qD);
// Count the real roots
int nr = 0;
for (Complex root : roots)
{
if (root.isReal())
{
nr++;
}
}
// cerco radice reale piu vicina
double theta;
switch (nr)
{
case 0:
default:
ok = false;
return 0;
case 1:
theta = roots[0].re;
break;
case 2:
if (Math.abs(roots[0].re - theta0) < Math.abs(roots[1].re - theta0))
{
theta = roots[0].re;
}
else
{
theta = roots[1].re;
}
break;
case 3:
theta = roots[0].re;
for (int i = 1; i < 3; ++i)
{
if (Math.abs(theta - theta0) > Math.abs(roots[i].re - theta0))
{
theta = roots[i].re;
}
}
break;
}
ok = Math.abs(theta - theta0) < m_pi;
return theta;
}
// bool
// ClothoidCurve::setup_forward( double _x0,
// double _y0,
// double _theta0,
// double _k,
// double _x1,
// double _y1,
// double tol ) {
//
// x0 = _x0 ;
// y0 = _y0 ;
// theta0 = _theta0 ;
// k = _k ;
// s_min = 0 ;
//
//// Compute guess angles
// double len = hypot( _y1-_y0, _x1-_x0 ) ;
// double arot = atan2( _y1-_y0, _x1-_x0 ) ;
// double th0 = theta0 - arot ;
//// normalize angle
// while ( th0 > m_pi ) th0 -= m_2pi ;
// while ( th0 < -m_pi ) th0 += m_2pi ;
//
//// solve the problem from (0,0) to (1,0)
// double k0 = k*len ;
// double alpha = 2.6 ;
// double thmin = max(-m_pi,-theta0/2-alpha) ;
// double thmax = min( m_pi,-theta0/2+alpha) ;
// double Kmin = kappa( th0, thmax ) ;
// double Kmax = kappa( th0, thmin ) ;
// bool ok ;
// double th = theta_guess( th0, max(min(k0,Kmax),Kmin), ok ) ;
// if ( ok ) {
// for ( int iter = 0 ; iter < 10 ; ++iter ) {
// double dk, L, k_1, dk_1, L_1, k_2, dk_2, L_2 ;
// buildClothoid( 0, 0, th0,
// 1, 0, th,
// k, dk, L, k_1, dk_1, L_1, k_2, dk_2, L_2 ) ;
// double f = k - k0 ;
// double df = k_2 ;
// double dth = f/df ;
// th -= dth ;
// if ( abs(dth) < tol && abs(f) < tol ) {
//// transform solution
// buildClothoid( x0, y0, theta0,
// _x1, _y1, arot + th,
// _k, dk, s_max ) ;
// return true ;
// }
// }
// }
// return false ;
// }
//
// void
// ClothoidCurve::change_origin( double s0 ) {
// double new_theta, new_kappa, new_x0, new_y0 ;
// eval( s0, new_theta, new_kappa, new_x0, new_y0 ) ;
// x0 = new_x0 ;
// y0 = new_y0 ;
// theta0 = new_theta ;
// k = new_kappa ;
// s_min -= s0 ;
// s_max -= s0 ;
// }
//
// bool
// ClothoidCurve::bbTriangle( double offs,
// double p0[2],
// double p1[2],
// double p2[2] ) const {
// double theta_max = theta( s_max ) ;
// double theta_min = theta( s_min ) ;
// double dtheta = Math.abs( theta_max-theta_min ) ;
// if ( dtheta < m_pi_2 ) {
// double alpha, t0[2] ;
// eval( s_min, offs, p0[0], p0[1] ) ;
// eval_D( s_min, t0[0], t0[1] ) ; // no offset
// if ( dtheta > 0.0001 * m_pi_2 ) {
// double t1[2] ;
// eval( s_max, offs, p1[0], p1[1] ) ;
// eval_D( s_max, t1[0], t1[1] ) ; // no offset
//// risolvo il sistema
//// p0 + alpha * t0 = p1 + beta * t1
//// alpha * t0 - beta * t1 = p1 - p0
// double det = t1[0]*t0[1]-t0[0]*t1[1] ;
// alpha = ((p1[1]-p0[1])*t1[0] - (p1[0]-p0[0])*t1[1])/det ;
// } else {
//// se angolo troppo piccolo uso approx piu rozza
// alpha = s_max - s_min ;
// }
// p2[0] = p0[0] + alpha*t0[0] ;
// p2[1] = p0[1] + alpha*t0[1] ;
// return true ;
// } else {
// return false ;
// }
// }
//
// void
// ClothoidCurve::bbSplit( double split_angle,
// double split_size,
// double split_offs,
// vector<ClothoidCurve> & c,
// vector<Triangle2D> & t ) const {
//
//// step 0: controllo se curvatura passa per 0
// double k_min = theta_D( s_min ) ;
// double k_max = theta_D( s_max ) ;
// c.clear() ;
// t.clear() ;
// if ( k_min * k_max < 0 ) {
//// risolvo (s-s_min)*dk+k_min = 0 --> s = s_min-k_min/dk
// double s_med = s_min-k_min/dk ;
//
// ClothoidCurve tmp(*this) ;
// tmp.trim(s_min,s_med) ;
// tmp.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
// tmp.trim(s_med,s_max) ;
// tmp.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
// }else
//
// {
// bbSplit_internal(split_angle, split_size, split_offs, c, t);
// }
// }
//
// static double
//
// abs2pi( double a ) {
// a = Math.abs(a) ;
// while ( a > m_pi ) a -= m_2pi ;
// return Math.abs(a) ;
// }
//
// void
// ClothoidCurve::bbSplit_internal( double split_angle,
// double split_size,
// double split_offs,
// vector<ClothoidCurve> & c,
// vector<Triangle2D> & t ) const {
//
// double theta_min, kappa_min, x_min, y_min,
// theta_max, kappa_max, x_max, y_max ;
//
// eval( s_min, theta_min, kappa_min, x_min, y_min ) ;
// eval( s_max, theta_max, kappa_max, x_max, y_max ) ;
//
// double dtheta = Math.abs( theta_max - theta_min ) ;
// double dx = x_max - x_min ;
// double dy = y_max - y_min ;
// double len = hypot( dy, dx ) ;
// double dangle = abs2pi(atan2( dy, dx )-theta_min) ;
// if ( dtheta <= split_angle && len*tan(dangle) <= split_size ) {
// Triangle2D tt ;
// this->bbTriangle(split_offs,tt) ;
// c.push_back(*this) ;
// t.push_back(tt) ;
// } else {
//
// ClothoidCurve cc(*this) ;
// double s_med = (s_min+s_max)/2 ;
// cc.trim(s_min,s_med) ;
// cc.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
// cc.trim(s_med,s_max) ;
// cc.bbSplit_internal( split_angle, split_size, split_offs, c, t ) ;
// }}
//
// bool ClothoidCurve::intersect_internal(ClothoidCurve&c1,
//
// double c1_offs, double&s1,ClothoidCurve&c2,
//
// double c2_offs, double&s2,
//
// int max_iter,
// double tolerance)const{
//
// double angle1a = c1.theta(c1.s_min);
//
// double angle1b = c1.theta(c1.s_max);
//
// double angle2a = c2.theta(c2.s_min);
//
// double angle2b = c2.theta(c2.s_max);
//
// // cerca angoli migliori per partire
// double dmax = abs2pi(angle1a - angle2a);
//
// double dab = abs2pi(angle1a - angle2b);
//
// double dba = abs2pi(angle1b - angle2a);
//
// double dbb = abs2pi(angle1b - angle2b);s1=c1.s_min;s2=c2.s_min;if(dmax<dab)
// {
// dmax = dab;
// s2 = c2.s_max;
// }if(dmax<dba)
// {
// dmax = dba;
// s1 = c1.s_min;
// s2 = c2.s_min;
// }if(dmax<dbb)
// {
// s1 = c1.s_min;
// s2 = c2.s_max;
// }for(
//
// int i = 0;i<max_iter;++i)
// {
// double t1[2], t2[2], p1[2], p2[2] ;
// c1.eval( s1, c1_offs, p1[0], p1[1] ) ;
// c1.eval_D( s1, c1_offs, t1[0], t1[1] ) ;
// c2.eval( s2, c2_offs, p2[0], p2[1] ) ;
// c2.eval_D( s2, c2_offs, t2[0], t2[1] ) ;
/// *
//// risolvo il sistema
//// p1 + alpha * t1 = p2 + beta * t2
//// alpha * t1 - beta * t2 = p2 - p1
////
//// / t1[0] -t2[0] \ / alpha \ = / p2[0] - p1[0] \
//// \ t1[1] -t2[1] / \ beta / \ p2[1] - p1[1] /
// */
// double det = t2[0]*t1[1]-t1[0]*t2[1] ;
// double px = p2[0]-p1[0] ;
// double py = p2[1]-p1[1] ;
// s1 += (py*t2[0] - px*t2[1])/det ;
// s2 += (t1[0]*py - t1[1]*px)/det ;
// if ( s1 <= c1.s_min || s1 >= c1.s_max ||
// s2 <= c2.s_min || s2 >= c2.s_max ) break ;
// if ( Math.abs(px) <= tolerance ||
// Math.abs(py) <= tolerance ) return true ;
// }return false;}
//
// void ClothoidCurve::intersect(
//
// double offs, ClothoidCurve const&clot,
//
// double clot_offs, vector<double>&s1,vector<double>&s2,
//
// int max_iter, double tolerance)const
// {
// vector<ClothoidCurve> c0, c1;
// vector<Triangle2D> t0, t1;
// bbSplit(m_pi / 50, (s_max - s_min) / 3, offs, c0, t0);
// clot.bbSplit(m_pi / 50, (clot.s_max - clot.s_min) / 3, clot_offs, c1, t1);
// s1.clear();
// s2.clear();
// for (int i = 0; i < int(c0.size()); ++i)
// {
// for (int j = 0; j < int(c1.size()); ++j)
// {
// if (t0[i].overlap(t1[j]))
// {
// // uso newton per cercare intersezione
// double tmp_s1, tmp_s2;
// bool ok = intersect_internal(c0[i], offs, tmp_s1, c1[j], clot_offs, tmp_s2, max_iter, tolerance);
// if (ok)
// {
// s1.push_back(tmp_s1);
// s2.push_back(tmp_s2);
// }
// }
// }
// }
// }
//
// // collision detection
// bool
// ClothoidCurve::approsimate_collision( double offs,
// ClothoidCurve const & clot,
// double clot_offs,
// double max_angle,
// double max_size ) const {
// vector<ClothoidCurve> c0, c1 ;
// vector<Triangle2D> t0, t1 ;
// bbSplit( max_angle, max_size, offs, c0, t0 ) ;
// clot.bbSplit( max_angle, max_size, clot_offs, c1, t1 ) ;
// for ( int i = 0 ; i < int(c0.size()) ; ++i ) {
// for ( int j = 0 ; j < int(c1.size()) ; ++j ) {
// if ( t0[i].overlap(t1[j]) ) return true ;
// }
// }
// return false ;
// }
//
// void
// ClothoidCurve::rotate( double angle, double cx, double cy ) {
// double dx = x0 - cx ;
// double dy = y0 - cy ;
// double C = cos(angle) ;
// double S = sin(angle) ;
// double ndx = C*dx - S*dy ;
// double ndy = C*dy + S*dx ;
// x0 = cx + ndx ;
// y0 = cy + ndy ;
// theta0 += angle ;
// }
//
// void
// ClothoidCurve::scale( double s ) {
// k /= s ;
// dk /= s*s ;
// s_min *= s ;
// s_max *= s ;
// }
//
// void
// ClothoidCurve::reverse() {
// theta0 = theta0 + m_pi ;
// if ( theta0 > m_pi ) theta0 -= 2*m_pi ;
// k = -k ;
// double tmp = s_max ;
// s_max = -s_min ;
// s_min = -tmp ;
// }
//
// std::ostream&operator<<(std::ostream&stream,ClothoidCurve const&c)
//
// {stream<<"x0 = "<<c.x0<<"\ny0 = "<<c.y0<<"\ntheta0 = "<<c.theta0<<"\nk = "<<c.k<<"\ndk = "<<c.dk<<"\nL =
// "<<c.s_max-c.s_min<<"\ns_min = "<<c.s_min<<"\ns_max = "<<c.s_max<<"\n";return stream;}
//
// static inline bool
//
// power2( double a )
// { return a*a ; }
//
// // **************************************************************************
//
// static
// inline
// bool
//
// solve2x2( double const b[2],
// double A[2][2],
// double x[2] ) {
//// full pivoting
// int ij = 0 ;
// double Amax = Math.abs(A[0][0]) ;
// double tmp = Math.abs(A[0][1]) ;
// if ( tmp > Amax ) { ij = 1 ; Amax = tmp ; }
// tmp = Math.abs(A[1][0]) ;
// if ( tmp > Amax ) { ij = 2 ; Amax = tmp ; }
// tmp = Math.abs(A[1][1]) ;
// if ( tmp > Amax ) { ij = 3 ; Amax = tmp ; }
// if ( Amax == 0 ) return false ;
// int i[] = { 0, 1 } ;
// int j[] = { 0, 1 } ;
// if ( (ij&0x01) == 0x01 ) { j[0] = 1 ; j[1] = 0 ; }
// if ( (ij&0x02) == 0x02 ) { i[0] = 1 ; i[1] = 0 ; }
//// apply factorization
// A[i[1]][j[0]] /= A[i[0]][j[0]] ;
// A[i[1]][j[1]] -= A[i[1]][j[0]]*A[i[0]][j[1]] ;
//// check for singularity
// double epsi = 1e-10 ;
// if ( Math.abs( A[i[1]][j[1]] ) < epsi ) {
//// L^+ Pb
// double tmp = (b[i[0]] + A[i[1]][j[0]]*b[i[1]]) /
// ( (1+power2(A[i[1]][j[0]]) ) *
// ( power2(A[i[0]][j[0]])+power2(A[i[0]][j[1]]) ) ) ;
// x[j[0]] = tmp*A[i[0]][j[0]] ;
// x[j[1]] = tmp*A[i[0]][j[1]] ;
// } else { // non singular
//// L^(-1) Pb
// x[j[0]] = b[i[0]] ;
// x[j[1]] = b[i[1]]-A[i[1]][j[0]]*x[j[0]] ;
//// U^(-1) x
// x[j[1]] /= A[i[1]][j[1]] ;
// x[j[0]] = (x[j[0]]-A[i[0]][j[1]]*x[j[1]])/A[i[0]][j[0]] ;
// }
// return true ;
// }
//
//// **************************************************************************
//
// void
// G2data::setup( double _x0,
// double _y0,
// double _theta0,
// double _kappa0,
// double _x1,
// double _y1,
// double _theta1,
// double _kappa1 ) {
//
// x0 = _x0 ;
// y0 = _y0 ;
// theta0 = _theta0;
// kappa0 = _kappa0 ;
// x1 = _x1 ;
// y1 = _y1 ;
// theta1 = _theta1 ;
// kappa1 = _kappa1 ;
//
//// scale problem
// double dx = x1 - x0 ;
// double dy = y1 - y0 ;
// phi = atan2( dy, dx ) ;
// Lscale = 2/hypot( dx, dy ) ;
//
// th0 = theta0 - phi ;
// th1 = theta1 - phi ;
//
// k0 = kappa0/Lscale ;
// k1 = kappa1/Lscale ;
//
// DeltaK = k1 - k0 ;
// DeltaTheta = th1 - th0 ;
// }
//
// void
// G2data::setTolerance( double tol ) {
// CLOTHOID_ASSERT( tol > 0 && tol <= 0.1,
// "setTolerance, tolerance = " << tol << " must be in (0,0.1]" ) ;
// tolerance = tol ;
// }
//
// void
// G2data::setMaxIter( int miter ) {
// CLOTHOID_ASSERT( miter > 0 && miter <= 1000,
// "setMaxIter, maxIter = " << miter << " must be in [1,1000]" ) ;
// maxIter = miter ;
// }
//
// // **************************************************************************
//
// void
// G2solve2arc::evalA( double alpha,
// double L,
// double & A,
// double & A_1,
// double & A_2 ) const {
// double K = k0+k1 ;
// double aK = alpha*DeltaK ;
// A = alpha*(L*(aK-K)+2*DeltaTheta) ;
// A_1 = (2*aK-K)*L+2*DeltaTheta;
// A_2 = alpha*(aK-K) ;
// }
//
// void
// G2solve2arc::evalG( double alpha,
// double L,
// double th,
// double k,
// double G[2],
// double G_1[2],
// double G_2[2] ) const {
//
// double A, A_1, A_2, X[3], Y[3] ;
// evalA( alpha, L, A, A_1, A_2 ) ;
// double ak = alpha*k ;
// double Lk = L*k ;
// GeneralizedFresnelCS( 3, A, ak*L, th, X, Y );
//
// G[0] = alpha*X[0] ;
// G_1[0] = X[0]-alpha*(Y[2]*A_1/2+Y[1]*Lk) ;
// G_2[0] = -alpha*(Y[2]*A_2/2+Y[1]*ak) ;
//
// G[1] = alpha*Y[0] ;
// G_1[1] = Y[0]+alpha*(X[2]*A_1/2+X[1]*Lk) ;
// G_2[1] = alpha*(X[2]*A_2/2+X[1]*ak) ;
//
// }
//
// void
// G2solve2arc::evalFJ( double const vars[2],
// double F[2],
// double J[2][2] ) const {
//
// double alpha = vars[0] ;
// double L = vars[1] ;
// double G[2], G_1[2], G_2[2] ;
//
// evalG( alpha, L, th0, k0, G, G_1, G_2 ) ;
//
// F[0] = G[0] - 2/L ; F[1] = G[1] ;
// J[0][0] = G_1[0] ; J[1][0] = G_1[1] ;
// J[0][1] = G_2[0] + 2/(L*L) ; J[1][1] = G_2[1] ;
//
// evalG( alpha-1, L, th1, k1, G, G_1, G_2 ) ;
// F[0] -= G[0] ; F[1] -= G[1] ;
// J[0][0] -= G_1[0] ; J[1][0] -= G_1[1] ;
// J[0][1] -= G_2[0] ; J[1][1] -= G_2[1] ;
// }
//
//// ---------------------------------------------------------------------------
//
// bool
// G2solve2arc::solve() {
// double X[2] = { 0.5, 2 } ;
// bool converged = false ;
// for ( int i = 0 ; i < maxIter && !converged ; ++i ) {
// double F[2], J[2][2], d[2] ;
// evalFJ( X, F, J ) ;
// if ( !solve2x2( F, J, d ) ) break ;
// double lenF = hypot(F[0],F[1]) ;
// X[0] -= d[0] ;
// X[1] -= d[1] ;
// converged = lenF < tolerance ;
// }
// if ( converged ) converged = X[1] > 0 && X[0] > 0 && X[0] < 1 ;
// if ( converged ) buildSolution( X[0], X[1] ) ;
// return converged ;
// }
//
// // **************************************************************************
//
// void
// G2solve2arc::buildSolution( double alpha, double L ) {
// double beta = 1-alpha ;
// double LL = L/Lscale ;
// double s0 = LL*alpha ;
// double s1 = LL*beta ;
//
// double tmp = k0*alpha+k1*beta-2*DeltaTheta/L ;
//
// double dk0 = -Lscale*(k0+tmp)/s0 ;
// double dk1 = Lscale*(k1+tmp)/s1 ;
//
// S0.setup( x0, y0, theta0, kappa0, dk0, 0, s0 ) ;
// S1.setup( x1, y1, theta1, kappa1, dk1, -s1, 0 ) ;
// S1.change_origin( -s1 ) ;
// }
//
// // **************************************************************************
//
// void
// G2solve3arc::setup( double _x0,
// double _y0,
// double _theta0,
// double _kappa0,
// double _frac0,
// double _x1,
// double _y1,
// double _theta1,
// double _kappa1,
// double _frac1 ) {
// G2data::setup( _x0, _y0, _theta0, _kappa0, _x1, _y1, _theta1, _kappa1 ) ;
//
// double tmp = 1/(2-(_frac0+_frac1)) ;
// alpha = _frac0*tmp ;
// beta = _frac1*tmp ;
//
// gamma = (1-alpha-beta)/4 ;
// gamma2 = gamma*gamma ;
//
// a0 = alpha*k0 ;
// b1 = beta*k1 ;
//
// double ab = alpha-beta ;
//
// dK0_0 = 2*alpha*DeltaTheta ;
// dK0_1 = -alpha*(k0+a0+b1) ;
// dK0_2 = -alpha*gamma*(beta-alpha+1) ;
//
// dK1_0 = -2*beta*DeltaTheta ;
// dK1_1 = beta*(k1+a0+b1) ;
// dK1_2 = beta*gamma*(beta-alpha-1) ;
//
// KM_0 = 2*gamma*DeltaTheta ;
// KM_1 = -gamma*(a0+b1) ;
// KM_2 = gamma2*(alpha-beta) ;
//
// thM_0 = (ab*DeltaTheta+(th0+th1))/2 ;
// thM_1 = (a0-b1-ab*(a0+b1))/4 ;
// thM_2 = (gamma*(2*ab*ab-alpha-beta-1))/8 ;
// }
//
// void
// G2solve3arc::evalFJ( double const vars[2],
// double F[2],
// double J[2][2] ) const {
//
// double eta = vars[0] ;
// double zeta = vars[1] ;
//
// double dK0 = dK0_0 + eta*dK0_1 + zeta*dK0_2 ;
// double dK1 = dK1_0 + eta*dK1_1 + zeta*dK1_2 ;
// double KM = KM_0 + eta*KM_1 + zeta*KM_2 ;
// double thM = thM_0 + eta*thM_1 + zeta*thM_2 ;
//
// double xa[3], ya[3], xb[3], yb[3], xM[3], yM[3], xP[3], yP[3] ;
//
// GeneralizedFresnelCS( 3, dK0, a0*eta, th0, xa, ya );
// GeneralizedFresnelCS( 3, dK1, -b1*eta, th1, xb, yb );
// GeneralizedFresnelCS( 3, gamma2*zeta, -KM, thM, xM, yM );
// GeneralizedFresnelCS( 3, gamma2*zeta, KM, thM, xP, yP );
//
// F[0] = alpha*xa[0] + beta*xb[0] + gamma*(xM[0]+xP[0]) - 2/eta ;
// F[1] = alpha*ya[0] + beta*yb[0] + gamma*(yM[0]+yP[0]) ;
//
//// D F[0] / D eta
// J[0][0] = - alpha*(ya[2]*dK0_1/2+ya[1]*a0)
// - beta*(yb[2]*dK1_1/2-yb[1]*b1)
// + gamma * ((yM[1]-yP[1])*KM_1-(yM[0]+yP[0])*thM_1)
// + 2/(eta*eta) ;
//
//// D F[0] / D zeta
// J[0][1] = - alpha*(ya[2]*dK0_2/2) - beta*(yb[2]*dK1_2/2)
// - gamma*( (yM[2]+yP[2])*gamma2/2+(yP[1]-yM[1])*KM_2+(yP[0]+yM[0])*thM_2 ) ;
//
//// D F[1] / D eta
// J[1][0] = alpha*(xa[2]*dK0_1/2+xa[1]*a0) +
// beta*(xb[2]*dK1_1/2-xb[1]*b1) +
// gamma * ((xP[1]-xM[1])*KM_1+(xM[0]+xP[0])*thM_1) ;
//
//// D F[1] / D zeta
// J[1][1] = alpha*(xa[2]*dK0_2/2) + beta*(xb[2]*dK1_2/2)
// + gamma * ( (xM[2]+xP[2])*gamma2/2+(xP[1]-xM[1])*KM_2+(xP[0]+xM[0])*thM_2 ) ;
//
// }
//
//// ---------------------------------------------------------------------------
//
// bool
// G2solve3arc::solve() {
// double X[2] = { 2, 0 } ; // eta, zeta
// bool converged = false ;
// for ( int i = 0 ; i < maxIter && !converged ; ++i ) {
// double F[2], J[2][2], d[2] ;
// evalFJ( X, F, J ) ;
// if ( !solve2x2( F, J, d ) ) break ;
// double lenF = hypot(F[0],F[1]) ;
// X[0] -= d[0] ;
// X[1] -= d[1] ;
// converged = lenF < tolerance ;
// }
// if ( converged ) converged = X[0] > 0 ; // eta > 0 !
// if ( converged ) buildSolution( X[0], X[1] ) ;
// return converged ;
// }
//
// void
// G2solve3arc::buildSolution( double eta, double zeta ) {
//
// double L0 = eta*alpha ;
// double L1 = eta*beta ;
// double LM = eta*gamma ;
//
// double dkappaM = zeta*gamma2 ; // /(eta*eta)*LM*LM ;
// double dkappa0A = dK0_0 + eta*dK0_1 + zeta*dK0_2 ;
// double dkappa1B = dK1_0 + eta*dK1_1 + zeta*dK1_2 ;
// double kappaM = KM_0 + eta*KM_1 + zeta*KM_2 ;
// double thetaM = thM_0 + eta*thM_1 + zeta*thM_2 ;
//
// double xa, ya, xmL, ymL ;
// GeneralizedFresnelCS( dkappa0A, k0*L0, th0, xa, ya );
// GeneralizedFresnelCS( dkappaM, -kappaM, thetaM, xmL, ymL );
//
// double xM = L0 * xa + LM * xmL - 1 ;
// double yM = L0 * ya + LM * ymL ;
//
//// rovescia scalatura
// L0 /= Lscale ;
// L1 /= Lscale ;
// LM /= Lscale ;
//
// dkappa0A /= L0*L0 ;
// dkappa1B /= L1*L1 ;
// dkappaM /= LM*LM ;
// kappaM /= LM ;
//
// S0.setup( x0, y0, theta0, kappa0, dkappa0A, 0, L0 ) ;
// S1.setup( x1, y1, theta1, kappa1, dkappa1B, -L1, 0 ) ;
//
//// la trasformazione inversa da [-1,1] a (x0,y0)-(x1,y1)
//// g(x,y) = RotInv(phi)*(1/lambda*[X;Y] - [xbar;ybar]) = [x;y]
//
// double C = cos(phi) ;
// double S = sin(phi) ;
// double dx = (xM+1)/Lscale ;
// double dy = yM/Lscale ;
// SM.setup( x0 + C * dx - S * dy, y0 + C * dy + S * dx,
// thetaM+phi, kappaM, dkappaM, -LM, LM ) ;
//
//// Sguess.setup_G1( x0_orig, y0_orig, theta0_orig,
//// x1_orig, y1_orig, theta1_orig ) ;
//
// S1.change_origin( -L1 ) ;
// SM.change_origin( -LM ) ;
// }
//
// }
}