Program Listing for File ClothoidDistance.cc¶
↰ Return to documentation for file (ClothoidDistance.cc)
/*--------------------------------------------------------------------------*\
| |
| Copyright (C) 2017 |
| |
| , __ , __ |
| /|/ \ /|/ \ |
| | __/ _ ,_ | __/ _ ,_ |
| | \|/ / | | | | \|/ / | | | |
| |(__/|__/ |_/ \_/|/|(__/|__/ |_/ \_/|/ |
| /| /| |
| \| \| |
| |
| Enrico Bertolazzi |
| Dipartimento di Ingegneria Industriale |
| Universita` degli Studi di Trento |
| email: enrico.bertolazzi@unitn.it |
| |
\*--------------------------------------------------------------------------*/
#include "Clothoids.hh"
// Workaround for Visual Studio
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
namespace G2lib {
using std::abs;
using std::sqrt;
using std::sin;
using std::cos;
using std::max;
using std::min;
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::closestPointBySample(
real_type ds,
real_type qx,
real_type qy,
real_type & X,
real_type & Y,
real_type & S
) const {
S = 0;
X = m_CD.x0;
Y = m_CD.y0;
real_type DST = hypot( X-qx, Y-qy );
real_type SSS = ds;
while ( SSS <= m_L ) {
real_type theta, kappa, XS, YS;
m_CD.evaluate( SSS, theta, kappa, XS, YS );
real_type dst = hypot( XS-qx, YS-qy );
if ( dst < DST ) {
DST = dst;
S = SSS;
X = XS;
Y = YS;
}
SSS += ds;
}
return DST;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
bool
closestPointQC2(
real_type epsi,
ClothoidData const & CD,
real_type L,
real_type qx,
real_type qy,
real_type & S
) {
// S = GUESS
int nb = 0;
real_type theta, kappa, dS, dx, dy;
real_type s = S;
for ( int iter = 0; iter < 20 && nb < 2; ++iter ) {
CD.evaluate( s, theta, kappa, dx, dy ); dx -= qx; dy -= qy;
real_type Cs = cos(theta);
real_type Ss = sin(theta);
real_type a0 = Cs * dy - Ss * dx;
real_type b0 = Ss * dy + Cs * dx;
real_type tmp = a0*kappa;
// approx clothoid with a circle
if ( 1+2*tmp > 0 ) {
tmp = b0/(1+tmp);
dS = -tmp*Atanc(tmp*kappa);
} else {
real_type om = atan2( b0, a0+1/kappa );
if ( kappa < 0 ) {
if ( om < 0 ) om += Utils::m_pi;
else om -= Utils::m_pi;
}
dS = -om/kappa;
}
s += dS;
if ( abs( dS ) < epsi ) {
if ( s < -epsi || s > L+epsi ) return false;
S = s;
return true;
}
// check divergence
if ( s < 0 ) { s = 0; ++nb; }
else if ( s > L ) { s = L; ++nb; }
else { nb = 0; }
}
return false;
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
closestPointQC1(
real_type epsi,
ClothoidData const & CD,
real_type L,
real_type qx,
real_type qy,
real_type & X,
real_type & Y,
real_type & S
) {
real_type phi0 = CD.theta0 - atan2( CD.y0 - qy, CD.x0 - qx );
bool ok0 = cos(phi0) < 0; // distanza decrescente
real_type theta1, kappa1, x1, y1;
CD.evaluate( L, theta1, kappa1, x1, y1 );
real_type phi1 = theta1 - atan2( y1 - qy, x1 - qx );
bool ok1 = cos(phi1) > 0; // distanza crescente
real_type s0 = 0, x0 = CD.x0, y0 = CD.y0;
if ( ok0 ) ok0 = closestPointQC2( epsi, CD, L, qx, qy, s0 );
if ( ok0 ) CD.eval( s0, x0, y0 );
real_type d0 = hypot( x0-qx, y0-qy );
real_type s1 = L;
if ( ok1 ) ok1 = closestPointQC2( epsi, CD, L, qx, qy, s1 );
if ( ok1 ) CD.eval( s1, x1, y1 );
real_type d1 = hypot( x1-qx, y1-qy );
if ( !ok0 && !ok1 ) { // s1 - s0 > 2 * epsi ) { // buoni entrambi estremi
S = (s0+s1)/2;
bool okm = closestPointQC2( epsi, CD, L, qx, qy, S );
if ( okm ) {
CD.eval( S, X, Y );
real_type dm = hypot( X-qx, Y-qy );
if ( dm < d0 && dm < d1 ) return dm;
}
}
if ( d0 < d1 ) { S = s0; X = x0; Y = y0; return d0; }
else { S = s1; X = x1; Y = y1; return d1; }
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
closestPointQC(
real_type epsi,
ClothoidData const & CD,
real_type L,
real_type qx,
real_type qy,
real_type & X,
real_type & Y,
real_type & S
) {
real_type DTheta = abs( CD.theta(L) - CD.theta0 );
if ( DTheta <= Utils::m_2pi )
return closestPointQC1( epsi, CD, L, qx, qy, X, Y, S );
real_type cx = CD.c0x();
real_type cy = CD.c0y();
//if ( hypot( CD.x0 - cx, CD.y0 - cy ) <= hypot( qx - cx, qy - cy ) ) {
if ( 1 <= abs(CD.kappa0) * hypot( qx - cx, qy - cy ) ) {
real_type ell = CD.aplus(Utils::m_2pi);
return closestPointQC1( epsi, CD, ell, qx, qy, X, Y, S );
}
ClothoidData CD1;
CD.reverse( L, CD1 );
cx = CD1.c0x();
cy = CD1.c0y();
//if ( hypot( CD1.x0 - cx, CD1.y0 - cy ) >= hypot( qx - cx, qy - cy ) ) {
if ( 1 >= abs(CD1.kappa0) * hypot( qx - cx, qy - cy ) ) {
real_type ell = CD1.aplus(Utils::m_2pi);
real_type d = closestPointQC1( epsi, CD1, ell, qx, qy, X, Y, S );
S = L - S;
return d;
}
real_type ell = CD.aplus(DTheta/2);
real_type d0 = closestPointQC( epsi, CD, ell, qx, qy, X, Y, S );
CD.eval( ell, CD1 );
real_type X1, Y1, S1;
real_type d1 = closestPointQC( epsi, CD1, L-ell, qx, qy, X1, Y1, S1 );
if ( d1 < d0 ) { S = ell+S1; X = X1; Y = Y1; return d1; }
return d0;
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
bool
closestPointStandard3(
real_type epsi,
real_type a,
real_type b,
real_type qx,
real_type qy,
real_type & S
) {
// S = GUESS
int nb = 0;
real_type s = S, dS, dx, dy;
for ( int iter = 0; iter < 20 && nb < 2; ++iter ) {
// approx clothoid with a circle
real_type kappa = Utils::m_pi * s;
real_type theta = 0.5*(kappa*s);
FresnelCS( s, dx, dy ); dx -= qx; dy -= qy;
real_type Cs = cos(theta);
real_type Ss = sin(theta);
real_type a0 = Cs * dy - Ss * dx;
real_type b0 = Ss * dy + Cs * dx;
real_type tmp = a0*kappa;
if ( 1+2*tmp > 0 ) {
tmp = b0/(1+tmp);
dS = -tmp*Atanc(tmp*kappa);
} else {
real_type om = atan2( b0, a0+1/kappa );
if ( kappa < 0 ) {
if ( om < 0 ) om += Utils::m_pi;
else om -= Utils::m_pi;
}
dS = -om/kappa;
}
s += dS;
if ( abs( dS ) < epsi ) {
if ( s < a-epsi|| s > b+epsi ) break;
S = s;
return true;
}
// check divergence
if ( s < a ) { s = a; ++nb; }
else if ( s > b ) { s = b; ++nb; }
else { nb = 0; }
}
return false;
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
closestPointStandard2(
real_type epsi,
real_type a,
real_type b,
real_type qx,
real_type qy,
real_type & S
) {
real_type dx, dy;
FresnelCS( a, dx, dy ); dx -= qx; dy -= qy;
real_type phia = Utils::m_pi_2 * (a*a) - atan2( dy, dx );
bool ok0 = cos(phia) < 0; // distanza decrescente
FresnelCS( b, dx, dy ); dx -= qx; dy -= qy;
real_type phib = Utils::m_pi_2 * (b*b) - atan2( dy, dx );
bool ok1 = cos(phib) > 0; // distanza crescente
real_type s0 = a;
if ( ok0 ) ok0 = closestPointStandard3( epsi, a, b, qx, qy, s0 );
FresnelCS( s0, dx, dy ); dx -= qx; dy -= qy;
real_type d0 = hypot( dx, dy );
real_type s1 = b;
if ( ok1 ) ok1 = closestPointStandard3( epsi, a, b, qx, qy, s1 );
FresnelCS( s1, dx, dy ); dx -= qx; dy -= qy;
real_type d1 = hypot( dx, dy );
if ( !ok0 && !ok1 ) { // s1 - s0 > 2 * epsi ) { // buoni entrambi estremi
S = (s0+s1)/2;
bool ok = closestPointStandard3( epsi, a, b, qx, qy, S );
if ( ok ) {
FresnelCS( S, dx, dy ); dx -= qx; dy -= qy;
real_type dm = hypot( dx, dy );
if ( dm < d0 && dm < d1 ) return dm;
}
}
if ( d0 < d1 ) { S = s0; return d0; }
else { S = s1; return d1; }
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
closestPointStandard(
real_type epsi,
ClothoidData const & CD,
real_type L,
real_type qx,
real_type qy,
real_type & S
) {
// transform to standard clothoid
real_type sflex = -CD.kappa0/CD.dk;
UTILS_ASSERT( sflex <= 0, " bad sflex = {}\n", sflex );
real_type thflex = CD.theta0 + 0.5*CD.kappa0*sflex;
real_type ssf = sin(thflex);
real_type csf = cos(thflex);
real_type gamma = sqrt(abs(CD.dk)/Utils::m_pi);
real_type a = -sflex*gamma;
real_type b = (L-sflex)*gamma;
real_type xflex, yflex;
CD.eval( sflex, xflex, yflex );
real_type xx = qx - xflex;
real_type yy = qy - yflex;
// gamma * R^(-1)
real_type qqx = gamma * ( csf * xx + ssf * yy );
real_type qqy = gamma * ( -ssf * xx + csf * yy );
// M^(-1)
if ( CD.dk < 0 ) qqy = -qqy;
// now in standard form
if ( b*b-a*a <= 4 ) {
real_type d = closestPointStandard2( epsi, a, b, qqx, qqy, S );
S = sflex + S/gamma;
return d/gamma;
}
FresnelCS( a, xx, yy );
real_type di = hypot(qqx-0.5,qqy-0.5);
real_type da = hypot(xx-0.5,yy-0.5);
if ( di >= da ) {
real_type La = 4/(a+sqrt(a*a+4));
real_type d = closestPointStandard2( epsi, a, a+La, qqx, qqy, S );
S = sflex + S/gamma;
return d/gamma;
}
FresnelCS( b, xx, yy );
real_type db = hypot(xx-0.5,yy-0.5);
if ( di <= db ) {
real_type Lb = 4/(b+sqrt(b*b-4));
real_type d = closestPointStandard2( epsi, b-Lb, b, qqx, qqy, S );
S = sflex + S/gamma;
return d/gamma;
}
real_type ss = a;
bool converged = false;
for ( int iter = 0; iter < 20 && !converged; ++iter ) {
FresnelCS( ss, xx, yy );
real_type kappa = Utils::m_pi * ss;
real_type theta = Utils::m_pi_2 * (ss*ss);
real_type rhox = xx - 0.5;
real_type rhoy = yy - 0.5;
real_type rho = hypot( rhox, rhoy );
real_type f = rho - di;
//if ( abs(f) < epsi ) break;
real_type tphi = theta - atan2( rhoy, rhox );
real_type df = cos( tphi );
real_type t = sin( tphi );
real_type ddf = t*(kappa-t/rho);
real_type ds = (f*df)/((df*df)-f*ddf/2);
ss -= ds;
converged = abs(ds) < epsi;
}
UTILS_ASSERT0( converged, "closestPointStandard not converged\n" );
real_type Lp = min( b-ss, 4/(ss+sqrt(ss*ss+4)) );
real_type Lm = min( ss-a, 4/(ss+sqrt(ss*ss-4)) );
real_type sp, sm;
real_type dp = closestPointStandard2( epsi, ss, ss+Lp, qqx, qqy, sp );
real_type dm = closestPointStandard2( epsi, ss-Lm, ss, qqx, qqy, sm );
if ( dp < dm ) { S = sflex + sp/gamma; return dp/gamma; }
else { S = sflex + sm/gamma; return dm/gamma; }
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
closestPoint1(
real_type epsi,
ClothoidData const & CD,
real_type L,
real_type qx,
real_type qy,
real_type & X,
real_type & Y,
real_type & S
) {
real_type NT = 4; // number of turn of the clothid after wich is considered quasi-circular
real_type DK = sqrt(NT*Utils::m_2pi*abs(CD.dk));
if ( abs(CD.kappa0) >= DK ) {
return closestPointQC( epsi, CD, L, qx, qy, X, Y, S );
}
if ( abs(CD.kappa0)+abs(CD.dk)*L <= DK ) {
real_type d = closestPointStandard( epsi, CD, L, qx, qy, S );
CD.eval( S, X, Y );
return d;
}
real_type ell = (DK-abs(CD.kappa0))/abs(CD.dk);
UTILS_ASSERT( ell > 0 && ell < L, "bad ell = {} L = {}\n", ell, L );
ClothoidData CDS;
CD.eval( ell, CDS );
real_type S0;
real_type d0 = closestPointStandard( epsi, CD, ell, qx, qy, S0 );
real_type d1 = closestPointQC( epsi, CDS, L-ell, qx, qy, X, Y, S );
if ( d0 < d1 ) {
S = S0;
CD.eval( S, X, Y );
return d0;
} else {
S += ell;
return d1;
}
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int_type
ClothoidCurve::closestPoint_ISO(
real_type qx,
real_type qy,
real_type & X,
real_type & Y,
real_type & S,
real_type & T,
real_type & dst
) const {
real_type epsi = 1e-10;
// check if flex is inside curve, if so then split
if ( m_CD.kappa0*m_CD.dk >= 0 ) { // flex on the left
dst = closestPoint1( epsi, m_CD, m_L, qx, qy, X, Y, S );
} else if ( m_CD.dk*m_CD.kappa(m_L) <= 0 ) { // flex on the right, reverse curve
ClothoidData CD1;
m_CD.reverse( m_L, CD1 );
dst = closestPoint1( epsi, CD1, m_L, qx, qy, X, Y, S );
S = m_L-S;
} else {
// flex inside, split clothoid
ClothoidData C0, C1;
real_type sflex = m_CD.split_at_flex( C0, C1 );
real_type d0 = closestPoint1( epsi, C0, m_L-sflex, qx, qy, X, Y, S );
real_type x1, y1, s1;
real_type d1 = closestPoint1( epsi, C1, sflex, qx, qy, x1, y1, s1 );
if ( d1 < d0 ) {
S = sflex - s1;
X = x1;
Y = y1;
dst = d1;
} else {
S += sflex;
dst = d0;
}
}
// check if projection is orthogonal
real_type nx, ny;
nor_ISO( S, nx, ny );
real_type qxx = qx - X;
real_type qyy = qy - Y;
T = qxx * nx + qyy * ny; // signed distance
real_type pt = abs(qxx * ny - qyy * nx);
G2LIB_DEBUG_MESSAGE(
"ClothoidCurve::closestPoint_ISO: ||P-P0|| = {}, |(P-P0).T| = {}\n",
dst, pt
);
return pt > GLIB2_TOL_ANGLE*dst ? -1 : 1;
}
/*
+ // approx clothoid with a circle
real_type theta, kappa;
eval( S, theta, kappa, X, Y );
- real_type dx = X-x;
- real_type dy = Y-y;
- real_type d = hypot( dx, dy );
- real_type tphi = theta - atan2( dy, dx );
- real_type f = d*cos(tphi);
- real_type g = d*sin(tphi);
- real_type df = 1-kappa*g;
- real_type ddf = -kappa*f*(dk+kappa);
-
*/
}
// EOF: ClothoidDistance.cc
