Program Listing for File Clothoid.cc¶
↰ Return to documentation for file (Clothoid.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 windows that defines max and min as macros!
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#include <cmath>
#include <cfloat>
#include <algorithm>
namespace G2lib {
using std::vector;
using std::abs;
using std::min;
using std::max;
using std::swap;
using std::ceil;
using std::floor;
using std::isfinite;
using std::numeric_limits;
int_type ClothoidCurve::m_max_iter = 10;
real_type ClothoidCurve::m_tolerance = 1e-9;
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
ClothoidCurve::ClothoidCurve( BaseCurve const & C )
: BaseCurve(G2LIB_CLOTHOID)
, m_aabb_done(false)
{
switch ( C.type() ) {
case G2LIB_LINE:
build( *static_cast<LineSegment const *>(&C) );
break;
case G2LIB_CIRCLE:
build( *static_cast<CircleArc const *>(&C) );
break;
case G2LIB_CLOTHOID:
copy( *static_cast<ClothoidCurve const *>(&C) );
break;
case G2LIB_BIARC:
case G2LIB_BIARC_LIST:
case G2LIB_CLOTHOID_LIST:
case G2LIB_POLYLINE:
UTILS_ERROR(
"ClothoidList constructor cannot convert from: {}\n",
CurveType_name[C.type()]
);
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
ClothoidCurve::build(
real_type _x0,
real_type _y0,
real_type _theta0,
real_type _k,
real_type _dk,
real_type _L
) {
UTILS_ASSERT(
_L > 0,
"ClothoidCurve::build( x0={}, y0={}, theta0={}, k={}, dk={}, L={} )\n"
"L must be positive!\n",
_x0, _y0, _theta0, _k, _dk, _L
);
m_CD.x0 = _x0;
m_CD.y0 = _y0;
m_CD.theta0 = _theta0;
m_CD.kappa0 = _k;
m_CD.dk = _dk;
m_L = _L;
m_aabb_done = false;
m_aabb_tree.clear();
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
ClothoidCurve::optimized_sample_internal_ISO(
real_type s_begin,
real_type s_end,
real_type offs,
real_type ds,
real_type max_angle,
vector<real_type> & s
) const {
real_type ss = s_begin;
real_type thh = theta(s_begin);
for ( int_type npts = 0; ss < s_end; ++npts ) {
UTILS_ASSERT0(
npts < 100000000,
"ClothoidCurve::optimized_sample_internal "
"is generating too much points (>100000000)\n"
"something is going wrong or parameters are not well set\n"
);
// estimate angle variation and compute step accodingly
real_type k = m_CD.kappa( ss );
real_type dss = ds/(1+k*offs); // scale length with offset
real_type sss = ss + dss;
if ( sss > s_end ) {
sss = s_end;
dss = s_end-ss;
}
if ( abs(k*dss) > max_angle ) {
dss = abs(max_angle/k);
sss = ss + dss;
}
// check and recompute if necessary
real_type thhh = theta(sss);
if ( abs(thh-thhh) > max_angle ) {
k = m_CD.kappa( sss );
dss = abs(max_angle/k);
sss = ss + dss;
thhh = theta(sss);
}
ss = sss;
thh = thhh;
s.push_back(ss);
}
s.back() = s_end;
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
ClothoidCurve::optimized_sample_ISO(
real_type offs,
int_type npts,
real_type max_angle,
vector<real_type> & s
) const {
s.clear();
s.reserve( size_t(npts) );
s.push_back(0);
real_type ds = m_L/npts;
if ( m_CD.kappa0*m_CD.dk >= 0 || m_CD.kappa(m_L)*m_CD.dk <= 0 ) {
optimized_sample_internal_ISO( 0, m_L, offs, ds, max_angle, s );
} else {
// flex inside, split clothoid
real_type sflex = -m_CD.kappa0/m_CD.dk;
optimized_sample_internal_ISO( 0, sflex, offs, ds, max_angle, s );
optimized_sample_internal_ISO( sflex, m_L, offs, ds, max_angle, s );
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
/*\
| _ _ _____ _ _
| | |__| |_|_ _| _(_)__ _ _ _ __ _| |___
| | '_ \ '_ \| || '_| / _` | ' \/ _` | / -_)
| |_.__/_.__/|_||_| |_\__,_|_||_\__, |_\___|
| |___/
\*/
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
ClothoidCurve::bbTriangles_internal_ISO(
real_type offs,
vector<Triangle2D> & tvec,
real_type s_begin,
real_type s_end,
real_type max_angle,
real_type max_size,
int_type icurve
) const {
static real_type const one_degree = Utils::m_pi/180;
real_type ss = s_begin;
real_type thh = m_CD.theta(ss);
real_type MX = min( m_L, max_size );
for ( int_type npts = 0; ss < s_end; ++npts ) {
UTILS_ASSERT0(
npts < 100000000,
"ClothoidCurve::bbTriangles_internal "
"is generating too much triangles (>100000000)\n"
"something is going wrong or parameters are not well set\n"
);
// estimate angle variation and compute step accodingly
real_type k = m_CD.kappa( ss );
real_type dss = MX/(1+k*offs); // scale length with offset
real_type sss = ss + dss;
if ( sss > s_end ) {
sss = s_end;
dss = s_end-ss;
}
if ( abs(k*dss) > max_angle ) {
dss = abs(max_angle/k);
sss = ss + dss;
}
// check and recompute if necessary
real_type thhh = theta(sss);
if ( abs(thh-thhh) > max_angle ) {
k = m_CD.kappa( sss );
dss = abs(max_angle/k);
sss = ss + dss;
thhh = theta(sss);
}
real_type x0, y0, x1, y1;
m_CD.eval_ISO( ss, offs, x0, y0 );
m_CD.eval_ISO( sss, offs, x1, y1 );
real_type tx0 = cos(thh);
real_type ty0 = sin(thh);
real_type alpha = sss-ss; // se angolo troppo piccolo uso approx piu rozza
if ( abs(thh-thhh) > one_degree ) {
real_type tx1 = cos(thhh);
real_type ty1 = sin(thhh);
real_type det = tx1 * ty0 - tx0 * ty1;
real_type dx = x1-x0;
real_type dy = y1-y0;
alpha = (dy*tx1 - dx*ty1)/det;
}
real_type x2 = x0 + alpha*tx0;
real_type y2 = y0 + alpha*ty0;
Triangle2D t( x0, y0, x2, y2, x1, y1, ss, sss, icurve );
tvec.push_back( t );
ss = sss;
thh = thhh;
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
ClothoidCurve::bbTriangles_ISO(
real_type offs,
vector<Triangle2D> & tvec,
real_type max_angle,
real_type max_size,
int_type icurve
) const {
if ( m_CD.kappa0*m_CD.dk >= 0 || m_CD.kappa(m_L)*m_CD.dk <= 0 ) {
bbTriangles_internal_ISO( offs, tvec, 0, m_L, max_angle, max_size, icurve );
} else {
// flex inside, split clothoid
real_type sflex = -m_CD.kappa0/m_CD.dk;
bbTriangles_internal_ISO( offs, tvec, 0, sflex, max_angle, max_size, icurve );
bbTriangles_internal_ISO( offs, tvec, sflex, m_L, max_angle, max_size, icurve );
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
/*\
| ___ ___
| | _ ) _ ) _____ __
| | _ \ _ \/ _ \ \ /
| |___/___/\___/_\_\
\*/
void
ClothoidCurve::bbox_ISO(
real_type offs,
real_type & xmin,
real_type & ymin,
real_type & xmax,
real_type & ymax
) const {
vector<Triangle2D> tvec;
bbTriangles_ISO( offs, tvec, Utils::m_pi/18, 1e100 );
xmin = ymin = numeric_limits<real_type>::infinity();
xmax = ymax = -xmin;
vector<Triangle2D>::const_iterator it;
for ( it = tvec.begin(); it != tvec.end(); ++it ) {
// - - - - - - - - - - - - - - - - - - - -
if ( it->x1() < xmin ) xmin = it->x1();
else if ( it->x1() > xmax ) xmax = it->x1();
if ( it->x2() < xmin ) xmin = it->x2();
else if ( it->x2() > xmax ) xmax = it->x2();
if ( it->x3() < xmin ) xmin = it->x3();
else if ( it->x3() > xmax ) xmax = it->x3();
// - - - - - - - - - - - - - - - - - - - -
if ( it->y1() < ymin ) ymin = it->y1();
else if ( it->y1() > ymax ) ymax = it->y1();
if ( it->y2() < ymin ) ymin = it->y2();
else if ( it->y2() > ymax ) ymax = it->y2();
if ( it->y3() < ymin ) ymin = it->y3();
else if ( it->y3() > ymax ) ymax = it->y3();
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
/*\
| _ _ ____ ____ _
| / \ / \ | __ )| __ )| |_ _ __ ___ ___
| / _ \ / _ \ | _ \| _ \| __| '__/ _ \/ _ \
| / ___ \ / ___ \| |_) | |_) | |_| | | __/ __/
| /_/ \_\/_/ \_\____/|____/ \__|_| \___|\___|
\*/
#ifndef DOXYGEN_SHOULD_SKIP_THIS
void
ClothoidCurve::build_AABBtree_ISO(
real_type offs,
real_type max_angle,
real_type max_size
) const {
if ( m_aabb_done &&
Utils::isZero( offs-m_aabb_offs ) &&
Utils::isZero( max_angle-m_aabb_max_angle ) &&
Utils::isZero( max_size-m_aabb_max_size ) ) return;
#ifdef G2LIB_USE_CXX11
vector<shared_ptr<BBox const> > bboxes;
#else
vector<BBox const *> bboxes;
#endif
bbTriangles_ISO( offs, m_aabb_tri, max_angle, max_size );
bboxes.reserve(m_aabb_tri.size());
vector<Triangle2D>::const_iterator it;
int_type ipos = 0;
for ( it = m_aabb_tri.begin(); it != m_aabb_tri.end(); ++it, ++ipos ) {
real_type xmin, ymin, xmax, ymax;
it->bbox( xmin, ymin, xmax, ymax );
#ifdef G2LIB_USE_CXX11
bboxes.push_back( make_shared<BBox const>(
xmin, ymin, xmax, ymax, G2LIB_CLOTHOID, ipos
) );
#else
bboxes.push_back(
new BBox( xmin, ymin, xmax, ymax, G2LIB_CLOTHOID, ipos )
);
#endif
}
m_aabb_tree.build(bboxes);
m_aabb_done = true;
m_aabb_offs = offs;
m_aabb_max_angle = max_angle;
m_aabb_max_size = max_size;
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/*\
| _ _ _ _
| ___ ___ | | (_)___(_) ___ _ __
| / __/ _ \| | | / __| |/ _ \| '_ \
| | (_| (_) | | | \__ \ | (_) | | | |
| \___\___/|_|_|_|___/_|\___/|_| |_|
\*/
bool
ClothoidCurve::collision( ClothoidCurve const & C ) const {
this->build_AABBtree_ISO( 0 );
C.build_AABBtree_ISO( 0 );
T2D_collision_ISO fun( this, 0, &C, 0 );
return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
ClothoidCurve::collision_ISO(
real_type offs,
ClothoidCurve const & C,
real_type offs_C
) const {
this->build_AABBtree_ISO( offs );
C.build_AABBtree_ISO( offs_C );
T2D_collision_ISO fun( this, offs, &C, offs_C );
return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
ClothoidCurve::approximate_collision_ISO(
real_type offs,
ClothoidCurve const & C,
real_type offs_C,
real_type max_angle,
real_type max_size
) const {
this->build_AABBtree_ISO( offs, max_angle, max_size );
C.build_AABBtree_ISO( offs_C, max_angle, max_size );
T2D_approximate_collision fun( this, &C );
return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/*\
| _ _ _
| (_)_ __ | |_ ___ _ __ ___ ___ ___| |_
| | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
| | | | | | || __/ | \__ \ __/ (__| |_
| |_|_| |_|\__\___|_| |___/\___|\___|\__|
\*/
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
ClothoidCurve::aabb_intersect_ISO(
Triangle2D const & T1,
real_type offs,
ClothoidCurve const * pC,
Triangle2D const & T2,
real_type offs_C,
real_type & ss1,
real_type & ss2
) const {
real_type eps1 = machepsi1000*m_L;
real_type eps2 = machepsi1000*pC->m_L;
real_type s1_min = T1.S0()-eps1;
real_type s1_max = T1.S1()+eps1;
real_type s2_min = T2.S0()-eps2;
real_type s2_max = T2.S1()+eps2;
int_type nout = 0;
bool converged = false;
ss1 = (s1_min+s1_max)/2;
ss2 = (s2_min+s2_max)/2;
for ( int_type i = 0; i < m_max_iter && !converged; ++i ) {
real_type t1[2], t2[2], p1[2], p2[2];
m_CD.eval_ISO ( ss1, offs, p1[0], p1[1] );
m_CD.eval_ISO_D( ss1, offs, t1[0], t1[1] );
pC->m_CD.eval_ISO ( ss2, offs_C, p2[0], p2[1] );
pC->m_CD.eval_ISO_D( ss2, offs_C, 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] /
*/
real_type det = t2[0]*t1[1]-t1[0]*t2[1];
real_type px = p2[0]-p1[0];
real_type py = p2[1]-p1[1];
ss1 += (py*t2[0] - px*t2[1])/det;
ss2 += (t1[0]*py - t1[1]*px)/det;
if ( ! ( isfinite(ss1) && isfinite(ss1) ) ) break;
bool out = false;
if ( ss1 < s1_min ) { out = true; ss1 = s1_min; }
else if ( ss1 > s1_max ) { out = true; ss1 = s1_max; }
if ( ss2 < s2_min ) { out = true; ss2 = s2_min; }
else if ( ss2 > s2_max ) { out = true; ss2 = s2_max; }
if ( out ) {
if ( ++nout > 3 ) break;
} else {
converged = abs(px) <= m_tolerance && abs(py) <= m_tolerance;
}
}
if ( converged ) {
if ( ss1 < T1.S0() ) ss1 = T1.S0();
else if ( ss1 > T1.S1() ) ss1 = T1.S1();
if ( ss2 < T2.S0() ) ss2 = T2.S0();
else if ( ss2 > T2.S1() ) ss2 = T2.S1();
}
return converged;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
ClothoidCurve::intersect_ISO(
real_type offs,
ClothoidCurve const & C,
real_type offs_C,
IntersectList & ilist,
bool swap_s_vals
) const {
if ( intersect_with_AABBtree ) {
this->build_AABBtree_ISO( offs );
C.build_AABBtree_ISO( offs_C );
AABBtree::VecPairPtrBBox iList;
m_aabb_tree.intersect( C.m_aabb_tree, iList );
AABBtree::VecPairPtrBBox::const_iterator ip;
for ( ip = iList.begin(); ip != iList.end(); ++ip ) {
size_t ipos1 = size_t(ip->first->Ipos());
size_t ipos2 = size_t(ip->second->Ipos());
Triangle2D const & T1 = m_aabb_tri[ipos1];
Triangle2D const & T2 = C.m_aabb_tri[ipos2];
real_type ss1, ss2;
bool converged = aabb_intersect_ISO( T1, offs, &C, T2, offs_C, ss1, ss2 );
if ( converged ) {
if ( swap_s_vals ) swap( ss1, ss2 );
ilist.push_back( Ipair( ss1, ss2 ) );
}
}
} else {
bbTriangles_ISO( offs, m_aabb_tri, Utils::m_pi/18, 1e100 );
C.bbTriangles_ISO( offs_C, C.m_aabb_tri, Utils::m_pi/18, 1e100 );
vector<Triangle2D>::const_iterator i1, i2;
for ( i1 = m_aabb_tri.begin(); i1 != m_aabb_tri.end(); ++i1 ) {
for ( i2 = C.m_aabb_tri.begin(); i2 != C.m_aabb_tri.end(); ++i2 ) {
Triangle2D const & T1 = *i1;
Triangle2D const & T2 = *i2;
real_type ss1, ss2;
bool converged = aabb_intersect_ISO( T1, offs, &C, T2, offs_C, ss1, ss2 );
if ( converged ) {
if ( swap_s_vals ) swap( ss1, ss2 );
ilist.push_back( Ipair( ss1, ss2 ) );
}
}
}
}
}
/*\
| _ _ ____ _ _
| ___| | ___ ___ ___ ___| |_| _ \ ___ (_)_ __ | |_
| / __| |/ _ \/ __|/ _ \/ __| __| |_) / _ \| | '_ \| __|
| | (__| | (_) \__ \ __/\__ \ |_| __/ (_) | | | | | |_
| \___|_|\___/|___/\___||___/\__|_| \___/|_|_| |_|\__|
\*/
void
ClothoidCurve::closestPoint_internal(
real_type s_begin,
real_type s_end,
real_type qx,
real_type qy,
real_type offs,
real_type & x,
real_type & y,
real_type & s,
real_type & dst
) const {
#if 1
// minimize using circle approximation
s = (s_begin + s_end)/2;
int_type nout = 0;
int_type n_ok = 0;
for ( int_type iter = 0; iter < m_max_iter; ++iter ) {
// osculating circle
m_CD.eval_ISO( s, offs, x, y );
real_type th = m_CD.theta( s );
real_type kk = m_CD.kappa( s );
real_type sc = 1+kk*offs;
real_type ds = projectPointOnCircle( x, y, th, kk/sc, qx, qy )/sc;
s += ds;
bool out = false;
if ( s <= s_begin ) { out = true; s = s_begin; }
else if ( s >= s_end ) { out = true; s = s_end; }
if ( out ) {
if ( ++nout > 3 ) break;
} else {
// force one more itaration to improve accuracy
if ( abs(ds) <= m_tolerance && ++n_ok >= 2 ) break;
}
}
dst = hypot( qx-x, qy-y );
#else
real_type ds = (s_end-s_begin)/10;
for ( int_type iter = 0; iter <= 10; ++iter ) {
real_type ss = s_begin + iter * ds;
real_type xx, yy;
CD.eval( ss, offs, xx, yy );
real_type dx = xx-qx;
real_type dy = yy-qy;
real_type dst1 = hypot( dx, dy );
if ( dst1 < dst ) {
s = ss;
x = xx;
y = yy;
dst = dst1;
}
}
#endif
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
ClothoidCurve::closestPoint_internal(
real_type qx,
real_type qy,
real_type offs,
real_type & x,
real_type & y,
real_type & s,
real_type & DST
) const {
DST = numeric_limits<real_type>::infinity();
this->build_AABBtree_ISO( offs );
AABBtree::VecPtrBBox candidateList;
m_aabb_tree.min_distance( qx, qy, candidateList );
AABBtree::VecPtrBBox::const_iterator ic;
UTILS_ASSERT0(
candidateList.size() > 0,
"ClothoidCurve::closestPoint no candidate\n"
);
for ( ic = candidateList.begin(); ic != candidateList.end(); ++ic ) {
size_t ipos = size_t((*ic)->Ipos());
Triangle2D const & T = m_aabb_tri[ipos];
real_type dst = T.distMin( qx, qy );
if ( dst < DST ) {
// refine distance
real_type xx, yy, ss;
closestPoint_internal(
T.S0(), T.S1(), qx, qy, offs, xx, yy, ss, dst
);
if ( dst < DST ) {
DST = dst;
s = ss;
x = xx;
y = yy;
}
}
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int_type
ClothoidCurve::closestPoint_ISO(
real_type qx,
real_type qy,
real_type offs,
real_type & x,
real_type & y,
real_type & s,
real_type & t,
real_type & DST
) const {
this->closestPoint_internal( qx, qy, offs, x, y, s, DST );
// 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 - offs; // signed distance
real_type pt = abs(qxx * ny - qyy * nx);
G2LIB_DEBUG_MESSAGE(
"Clothoid::closestPoint_ISO\n"
"||P-P0|| = {} and {}, |(P-P0).T| = {}\n",
DST, hypot(qxx,qyy), pt
);
return pt > GLIB2_TOL_ANGLE*hypot(qxx,qyy) ? -1 : 1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::thetaTotalVariation() const {
// cerco punto minimo parabola
// root = -k/dk;
real_type kL = m_CD.kappa0;
real_type kR = m_CD.kappa(m_L);
real_type thL = 0;
real_type thR = m_CD.deltaTheta(m_L);
if ( kL*kR < 0 ) {
real_type root = -m_CD.kappa0/m_CD.dk;
if ( root > 0 && root < m_L ) {
real_type thM = m_CD.deltaTheta(root);
return abs( thR - thM ) + abs( thM - thL );
}
}
return abs( thR - thL );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::thetaMinMax( real_type & thMin, real_type & thMax ) const {
// cerco punto minimo parabola
// root = -k/dk;
real_type kL = m_CD.kappa0;
real_type kR = m_CD.kappa(m_L);
real_type thL = 0;
real_type thR = m_CD.deltaTheta(m_L);
if ( thL < thR ) { thMin = thL; thMax = thR; }
else { thMin = thR; thMax = thL; }
if ( kL*kR < 0 ) {
real_type root = -m_CD.kappa0/m_CD.dk;
if ( root > 0 && root < m_L ) {
real_type thM = m_CD.deltaTheta(root);
if ( thM < thMin ) thMin = thM;
else if ( thM > thMax ) thMax = thM;
}
}
return thMax - thMin;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::curvatureMinMax( real_type & kMin, real_type & kMax ) const {
// cerco punto minimo parabola
// root = -k/dk;
kMin = m_CD.kappa0;
kMax = m_CD.kappa(m_L);
if ( kMax < kMin ) swap( kMax, kMin );
return kMax - kMin;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::curvatureTotalVariation() const {
// cerco punto minimo parabola
// root = -k/dk;
real_type km = m_CD.kappa0;
real_type kp = m_CD.kappa(m_L);
return abs(kp-km);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::integralCurvature2() const {
return m_L*( m_CD.kappa0*(m_CD.kappa0+m_L*m_CD.dk) + (m_L*m_L)*m_CD.dk*m_CD.dk/3 );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::integralJerk2() const {
real_type k2 = m_CD.kappa0*m_CD.kappa0;
real_type k3 = m_CD.kappa0*k2;
real_type k4 = k2*k2;
real_type t1 = m_L;
real_type t2 = m_L*t1;
real_type t3 = m_L*t2;
real_type t4 = m_L*t3;
return ((((t4/5*m_CD.dk+t3*m_CD.kappa0)*m_CD.dk+(1+2*t2)*k2)*m_CD.dk+2*t1*k3)*m_CD.dk+k4)*m_L;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
ClothoidCurve::integralSnap2() const {
real_type k2 = m_CD.kappa0*m_CD.kappa0;
real_type k3 = m_CD.kappa0*k2;
real_type k4 = k2*k2;
real_type k5 = k4*m_CD.kappa0;
real_type k6 = k4*k2;
real_type dk2 = m_CD.dk*m_CD.dk;
real_type dk3 = m_CD.dk*dk2;
real_type dk4 = dk2*dk2;
real_type dk5 = dk4*m_CD.dk;
real_type dk6 = dk4*dk2;
real_type t2 = m_L;
real_type t3 = m_L*t2;
real_type t4 = m_L*t3;
real_type t5 = m_L*t4;
real_type t6 = m_L*t5;
real_type t7 = m_L*t6;
return ( (t7/7)*dk6 + dk5*m_CD.kappa0*t6 + 3*dk4*k2*t5 + 5*dk3*k3*t4 +
5*dk2*k4*t3 + 3*dk3*t3 + 3*m_CD.dk*k5*t2 + 9*dk2*m_CD.kappa0*t2 +
k6+9*k2*m_CD.dk ) * m_L;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ostream_type &
operator << ( ostream_type & stream, ClothoidCurve const & c ) {
fmt::print( stream,
"x0 = {:<12} x1 = {:<12}\n"
"y0 = {:<12} y1 = {:<12}\n"
"theta0 = {:<12} theta1 = {:<12}\n"
"kappa0 = {:<12} kappa1 = {:<12}\n"
"dk = {:<12} L = {:<12}\n",
fmt::format("{:.6}",c.xBegin()),
fmt::format("{:.6}",c.xEnd()),
fmt::format("{:.6}",c.yBegin()),
fmt::format("{:.6}",c.yEnd()),
fmt::format("{:.6}",c.thetaBegin()),
fmt::format("{:.6}",c.thetaEnd()),
fmt::format("{:.6}",c.kappaBegin()),
fmt::format("{:.6}",c.kappaEnd()),
fmt::format("{:.6}",c.m_CD.dk),
fmt::format("{:.6}",c.m_L)
);
return stream;
}
}
// EOF: Clothoid.cc
