Program Listing for File Biarc.cc¶
↰ Return to documentation for file (Biarc.cc)
/*--------------------------------------------------------------------------*\
| |
| Copyright (C) 2017 |
| |
| , __ , __ |
| /|/ \ /|/ \ |
| | __/ _ ,_ | __/ _ ,_ |
| | \|/ / | | | | \|/ / | | | |
| |(__/|__/ |_/ \_/|/|(__/|__/ |_/ \_/|/ |
| /| /| |
| \| \| |
| |
| Enrico Bertolazzi |
| Dipartimento di Ingegneria Industriale |
| Universita` degli Studi di Trento |
| email: enrico.bertolazzi@unitn.it |
| |
\*--------------------------------------------------------------------------*/
#include "Clothoids.hh"
namespace G2lib {
using std::numeric_limits;
using std::abs;
/*\
| ____ _
| | __ )(_) __ _ _ __ ___
| | _ \| |/ _` | '__/ __|
| | |_) | | (_| | | | (__
| |____/|_|\__,_|_| \___|
\*/
Biarc::Biarc( BaseCurve const & C )
: BaseCurve(G2LIB_BIARC)
{
switch ( C.type() ) {
case G2LIB_LINE:
{
LineSegment const & LS = *static_cast<LineSegment const *>(&C);
bool ok = this->build(
LS.xBegin(), LS.yBegin(), LS.thetaBegin(),
LS.xEnd(), LS.yEnd(), LS.thetaEnd()
);
UTILS_ASSERT(
ok,
"Biarc constructor failed convert from: {}\n",
CurveType_name[C.type()]
);
}
break;
case G2LIB_CIRCLE:
{
CircleArc const & LS = *static_cast<CircleArc const *>(&C);
bool ok = this->build(
LS.xBegin(), LS.yBegin(), LS.thetaBegin(),
LS.xEnd(), LS.yEnd(), LS.thetaEnd()
);
UTILS_ASSERT(
ok,
"Biarc constructor failed convert from: {}\n",
CurveType_name[C.type()]
);
}
break;
case G2LIB_BIARC:
*this = *static_cast<Biarc const *>(&C);
break;
case G2LIB_CLOTHOID:
case G2LIB_BIARC_LIST:
case G2LIB_CLOTHOID_LIST:
case G2LIB_POLYLINE:
UTILS_ERROR(
"Biarc constructor cannot convert from: {}\n",
CurveType_name[C.type()]
);
}
}
bool
Biarc::build(
real_type x0,
real_type y0,
real_type theta0,
real_type x1,
real_type y1,
real_type theta1
) {
real_type dx = x1-x0;
real_type dy = y1-y0;
real_type d = hypot(dy,dx);
real_type omega = atan2(dy,dx);
// put in range
real_type th0 = theta0 - omega;
real_type th1 = theta1 - omega;
rangeSymm(th0);
rangeSymm(th1);
real_type thstar = - (th0+th1)/2;
real_type dth = (th1 - th0)/4;
real_type dth0 = (thstar - th0)/2;
real_type dth1 = (thstar - th1)/2;
real_type t = 2 * cos( dth ) / d;
real_type l0 = 1/(t*Sinc( dth0 ));
real_type l1 = 1/(t*Sinc( dth1 ));
real_type k0 = 2*t*sin( dth0 );
real_type k1 = -2*t*sin( dth1 );
real_type epsi = 100*d*numeric_limits<real_type>::epsilon();
if ( l0 > epsi && l1 > epsi ) {
m_C0.build( x0, y0, theta0, k0, l0 );
real_type an = omega+(thstar+th0)/2;
real_type xs = x0 + cos(an)/t;
real_type ys = y0 + sin(an)/t;
m_C1.build( xs, ys, omega+thstar, k1, l1 );
return true;
}
return false;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
Biarc::build_3P(
real_type x0,
real_type y0,
real_type x1,
real_type y1,
real_type x2,
real_type y2
) {
real_type dxa = x1-x0;
real_type dya = y1-y0;
real_type dxb = x2-x1;
real_type dyb = y2-y1;
real_type La = hypot(dya,dxa);
real_type Lb = hypot(dyb,dxb);
real_type arg = (dxa*dxb + dya * dyb)/(La*Lb);
if ( arg > 1 ) arg = 1;
else if ( arg < -1 ) arg = -1;
real_type om = acos(arg);
real_type at = (La/(La+Lb))*om;
real_type bt = (Lb/(La+Lb))*om;
// find solution using Halley
real_type Delta = 0;
bool found = false;
for ( int_type iter = 0; iter < 10 && !found; ++iter ) {
real_type ga[3], gb[3];
gfun( at+Delta, ga );
gfun( bt-Delta, gb );
real_type f = ga[0]/La - gb[0]/Lb;
real_type df = ga[1]/La + gb[1]/Lb;
real_type ddf = ga[2]/La - gb[2]/Lb;
real_type h = (df*f)/(df*df-0.5*f*ddf);
Delta -= h;
found = abs(h) < 1e-10 && abs(f) < 1e-10;
}
if ( found ) {
at += Delta; bt -= Delta;
real_type tha = atan2(dya,dxa);
real_type thb = atan2(dyb,dxb);
if ( dxa*dyb < dya*dxb ) {
tha += at;
thb += bt;
} else {
tha -= at;
thb -= bt;
}
m_C0.build_G1( x0, y0, tha, x1, y1 );
m_C1.build_G1( x1, y1, thb, x2, y2 );
}
return found;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::bbox(
real_type & xmin,
real_type & ymin,
real_type & xmax,
real_type & ymax
) const {
m_C0.bbox( xmin, ymin, xmax, ymax );
real_type xmi1, ymi1, xma1, yma1;
m_C1.bbox( xmi1, ymi1, xma1, yma1 );
if ( xmi1 < xmin ) xmin = xmi1;
if ( xma1 > xmax ) xmax = xma1;
if ( ymi1 < ymin ) ymin = ymi1;
if ( yma1 > ymax ) ymax = yma1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::bbox_ISO(
real_type offs,
real_type & xmin,
real_type & ymin,
real_type & xmax,
real_type & ymax
) const {
m_C0.bbox_ISO( offs, xmin, ymin, xmax, ymax );
real_type xmi1, ymi1, xma1, yma1;
m_C1.bbox_ISO( offs, xmi1, ymi1, xma1, yma1 );
if ( xmi1 < xmin ) xmin = xmi1;
if ( xma1 > xmax ) xmax = xma1;
if ( ymi1 < ymin ) ymin = ymi1;
if ( yma1 > ymax ) ymax = yma1;
}
/*\
| _ __
| | |_ _ __ __ _ _ __ ___ / _| ___ _ __ _ __ ___
| | __| '__/ _` | '_ \/ __| |_ / _ \| '__| '_ ` _ \
| | |_| | | (_| | | | \__ \ _| (_) | | | | | | | |
| \__|_| \__,_|_| |_|___/_| \___/|_| |_| |_| |_|
\*/
void
Biarc::reverse() {
CircleArc TMP(m_C0);
m_C0 = m_C1; m_C0.reverse();
m_C1 = TMP; m_C1.reverse();
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::scale( real_type scl ) {
real_type newx0 = m_C0.xBegin() + scl*(m_C1.xBegin()-m_C0.xBegin());
real_type newy0 = m_C0.yBegin() + scl*(m_C1.yBegin()-m_C0.yBegin());
m_C1.changeOrigin( newx0, newy0 );
m_C1.scale( scl );
m_C0.scale( scl );
}
void
Biarc::changeOrigin( real_type newx0, real_type newy0 ) {
m_C0.changeOrigin(newx0,newy0);
m_C1.changeOrigin(m_C0.xEnd(),m_C0.yEnd());
}
void
Biarc::trim( real_type s_begin, real_type s_end ) {
UTILS_ASSERT(
s_end > s_begin,
"Biarc::trim( begin={}, s_end={} ) s_end must be > s_begin\n",
s_begin, s_end
);
real_type L0 = m_C0.length();
if ( s_end <= L0 ) {
m_C0.trim( s_begin, s_end );
m_C1 = m_C0;
real_type ss = m_C0.length();
m_C0.trim( 0, ss/2 );
m_C1.trim( ss/2, ss );
} else if ( s_begin >= L0 ) {
m_C1.trim( s_begin-L0, s_end-L0 );
m_C0 = m_C1;
real_type ss = m_C0.length();
m_C0.trim( 0, ss/2 );
m_C1.trim( ss/2, ss );
} else {
m_C0.trim( s_begin, L0 );
m_C1.trim( 0, s_end-L0 );
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::theta( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.theta(s);
else return m_C1.theta(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::theta_D( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.m_k;
else return m_C1.m_k;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::tx( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tx(s);
else return m_C1.tx(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::tx_D( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tx_D(s);
else return m_C1.tx_D(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::tx_DD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tx_DD(s);
else return m_C1.tx_DD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::tx_DDD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tx_DDD(s);
else return m_C1.tx_DDD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::ty( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.ty(s);
else return m_C1.ty(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::ty_D( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.ty_D(s);
else return m_C1.ty_D(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::ty_DD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.ty_DD(s);
else return m_C1.ty_DD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::ty_DDD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.ty_DDD(s);
else return m_C1.ty_DDD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X(s);
else return m_C1.X(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_D( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_D(s);
else return m_C1.X_D(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_DD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_DD(s);
else return m_C1.X_DD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_DDD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_DDD(s);
else return m_C1.X_DDD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y(s);
else return m_C1.Y(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_D( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_D(s);
else return m_C1.Y_D(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_DD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_DD(s);
else return m_C1.Y_DD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_DDD( real_type s ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_DDD(s);
else return m_C1.Y_DDD(s-L0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_ISO( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_ISO(s,offs);
else return m_C1.X_ISO(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_ISO_D( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_ISO_D(s,offs);
else return m_C1.X_ISO_D(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_ISO_DD( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_ISO_DD(s,offs);
else return m_C1.X_ISO_DD(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::X_ISO_DDD( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.X_ISO_DDD(s,offs);
else return m_C1.X_ISO_DDD(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_ISO( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_ISO(s,offs);
else return m_C1.Y_ISO(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_ISO_D( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_ISO_D(s,offs);
else return m_C1.Y_ISO_D(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_ISO_DD( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_ISO_DD(s,offs);
else return m_C1.Y_ISO_DD(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Biarc::Y_ISO_DDD( real_type s, real_type offs ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.Y_ISO_DDD(s,offs);
else return m_C1.Y_ISO_DDD(s-L0,offs);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::tg( real_type s, real_type & tx, real_type & ty ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tg(s,tx,ty);
else return m_C1.tg(s-L0,tx,ty);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::tg_D( real_type s, real_type & tx_D, real_type & ty_D ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tg_D(s,tx_D,ty_D);
else return m_C1.tg_D(s-L0,tx_D,ty_D);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::tg_DD( real_type s, real_type & tx_DD, real_type & ty_DD ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tg_DD(s,tx_DD,ty_DD);
else return m_C1.tg_DD(s-L0,tx_DD,ty_DD);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::tg_DDD( real_type s, real_type & tx_DDD, real_type & ty_DDD ) const {
real_type L0 = m_C0.length();
if ( s < L0 ) return m_C0.tg_DDD(s,tx_DDD,ty_DDD);
else return m_C1.tg_DDD(s-L0,tx_DDD,ty_DDD);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::evaluate(
real_type s,
real_type & th,
real_type & k,
real_type & x,
real_type & y
) const {
if ( s < m_C0.length() ) {
th = m_C0.theta(s);
k = m_C0.curvature();
m_C0.eval(s,x,y);
} else {
s -= m_C0.length();
th = m_C1.theta(s);
k = m_C1.curvature();
m_C1.eval(s,x,y);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval(
real_type s,
real_type & x,
real_type & y
) const {
if ( s < m_C0.length() ) {
m_C0.eval(s,x,y);
} else {
s -= m_C0.length();
m_C1.eval(s,x,y);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_D(
real_type s,
real_type & x_D,
real_type & y_D
) const {
if ( s < m_C0.length() ) {
m_C0.eval_D(s,x_D,y_D);
} else {
s -= m_C0.length();
m_C1.eval_D(s,x_D,y_D);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_DD(
real_type s,
real_type & x_DD,
real_type & y_DD
) const {
if ( s < m_C0.length() ) {
m_C0.eval_DD(s,x_DD,y_DD);
} else {
s -= m_C0.length();
m_C1.eval_DD(s,x_DD,y_DD);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_DDD(
real_type s,
real_type & x_DDD,
real_type & y_DDD
) const {
if ( s < m_C0.length() ) {
m_C0.eval_DDD(s,x_DDD,y_DDD);
} else {
s -= m_C0.length();
m_C1.eval_DDD(s,x_DDD,y_DDD);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_ISO(
real_type s,
real_type offs,
real_type & x,
real_type & y
) const {
if ( s < m_C0.length() ) {
m_C0.eval_ISO(s,offs,x,y);
} else {
s -= m_C0.length();
m_C1.eval_ISO(s,offs,x,y);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_ISO_D(
real_type s,
real_type offs,
real_type & x_D,
real_type & y_D
) const {
if ( s < m_C0.length() ) {
m_C0.eval_ISO_D(s,offs,x_D,y_D);
} else {
s -= m_C0.length();
m_C1.eval_ISO_D(s,offs,x_D,y_D);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_ISO_DD(
real_type s,
real_type offs,
real_type & x_DD,
real_type & y_DD
) const {
if ( s < m_C0.length() ) {
m_C0.eval_ISO_DD(s,offs,x_DD,y_DD);
} else {
s -= m_C0.length();
m_C1.eval_ISO_DD(s,offs,x_DD,y_DD);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::eval_ISO_DDD(
real_type s,
real_type offs,
real_type & x_DDD,
real_type & y_DDD
) const {
if ( s < m_C0.length() ) {
m_C0.eval_ISO_DDD(s,offs,x_DDD,y_DDD);
} else {
s -= m_C0.length();
m_C1.eval_ISO_DDD(s,offs,x_DDD,y_DDD);
}
}
/*\
| _ _ _
| (_)_ __ | |_ ___ _ __ ___ ___ ___| |_
| | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
| | | | | | || __/ | \__ \ __/ (__| |_
| |_|_| |_|\__\___|_| |___/\___|\___|\__|
\*/
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::intersect(
Biarc const & B,
IntersectList & ilist,
bool swap_s_vals
) const {
IntersectList ilist00, ilist01, ilist10, ilist11;
m_C0.intersect( B.m_C0, ilist00, false );
m_C0.intersect( B.m_C1, ilist01, false );
m_C1.intersect( B.m_C0, ilist10, false );
m_C1.intersect( B.m_C1, ilist11, false );
real_type L = m_C0.length();
real_type LB = B.m_C0.length();
IntersectList::iterator it;
ilist.reserve( ilist.size() +
ilist00.size() +
ilist01.size() +
ilist10.size() +
ilist11.size() );
for ( it = ilist01.begin(); it != ilist01.end(); ++it ) it->second += LB;
for ( it = ilist10.begin(); it != ilist10.end(); ++it ) it->first += L;
for ( it = ilist11.begin(); it != ilist11.end(); ++it )
{ it->first += L; it->second += LB; }
if ( swap_s_vals ) {
for ( it = ilist00.begin(); it != ilist00.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist01.begin(); it != ilist01.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist10.begin(); it != ilist10.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist11.begin(); it != ilist11.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
} else {
for ( it = ilist00.begin(); it != ilist00.end(); ++it ) ilist.push_back( *it );
for ( it = ilist01.begin(); it != ilist01.end(); ++it ) ilist.push_back( *it );
for ( it = ilist10.begin(); it != ilist10.end(); ++it ) ilist.push_back( *it );
for ( it = ilist11.begin(); it != ilist11.end(); ++it ) ilist.push_back( *it );
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Biarc::intersect_ISO(
real_type offs,
Biarc const & B,
real_type offs_B,
IntersectList & ilist,
bool swap_s_vals
) const {
IntersectList ilist00, ilist01, ilist10, ilist11;
m_C0.intersect_ISO( offs, B.m_C0, offs_B, ilist00, false );
m_C0.intersect_ISO( offs, B.m_C1, offs_B, ilist01, false );
m_C1.intersect_ISO( offs, B.m_C0, offs_B, ilist10, false );
m_C1.intersect_ISO( offs, B.m_C1, offs_B, ilist11, false );
real_type L = m_C0.length();
real_type LB = B.m_C0.length();
IntersectList::iterator it;
ilist.reserve( ilist.size() +
ilist00.size() +
ilist01.size() +
ilist10.size() +
ilist11.size() );
for ( it = ilist01.begin(); it != ilist01.end(); ++it ) it->second += LB;
for ( it = ilist10.begin(); it != ilist10.end(); ++it ) it->first += L;
for ( it = ilist11.begin(); it != ilist11.end(); ++it )
{ it->first += L; it->second += LB; }
if ( swap_s_vals ) {
for ( it = ilist00.begin(); it != ilist00.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist01.begin(); it != ilist01.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist10.begin(); it != ilist10.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
for ( it = ilist11.begin(); it != ilist11.end(); ++it )
ilist.push_back( Ipair(it->second,it->first) );
} else {
for ( it = ilist00.begin(); it != ilist00.end(); ++it ) ilist.push_back( *it );
for ( it = ilist01.begin(); it != ilist01.end(); ++it ) ilist.push_back( *it );
for ( it = ilist10.begin(); it != ilist10.end(); ++it ) ilist.push_back( *it );
for ( it = ilist11.begin(); it != ilist11.end(); ++it ) ilist.push_back( *it );
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int_type
Biarc::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 x1, y1, s1, t1, dst1;
int_type res = m_C0.closestPoint_ISO( qx, qy, x, y, s, t, dst );
int_type res1 = m_C1.closestPoint_ISO( qx, qy, x1, y1, s1, t1, dst1 );
if ( dst1 < dst ) {
x = x1;
y = y1;
s = s1;
t = t1;
dst = dst1;
res = res1;
}
return res;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int_type
Biarc::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 {
real_type x1, y1, s1, t1, dst1;
int_type res = m_C0.closestPoint_ISO( qx, qy, offs, x, y, s, t, dst );
int_type res1 = m_C1.closestPoint_ISO( qx, qy, offs, x1, y1, s1, t1, dst1 );
if ( dst1 < dst ) {
x = x1;
y = y1;
s = s1;
t = t1;
dst = dst1;
res = res1;
}
return res;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
build_guess_theta(
int_type n,
real_type const * x,
real_type const * y,
real_type * theta
) {
UTILS_ASSERT0(
n > 1, "build_guess_theta, at least 2 points are necessary\n"
);
Biarc b;
if ( n == 2 ) {
theta[0] = theta[1] = atan2( y[1] - y[0], x[1] - x[0] );
} else {
bool ok, ciclic = hypot( x[0]-x[n-1], y[0]-y[n-1] ) < 1e-10;
if ( ciclic ) {
ok = b.build_3P( x[n-2], y[n-2], x[0], y[0], x[1], y[1] );
UTILS_ASSERT0( ok, "build_guess_theta, failed\n" );
theta[0] = theta[n-1] = b.thetaMiddle();
}
for ( int_type k = 1; k < n-1; ++k ) {
ok = b.build_3P( x[k-1], y[k-1], x[k], y[k], x[k+1], y[k+1] );
UTILS_ASSERT0( ok, "build_guess_theta, failed\n" );
theta[k] = b.thetaMiddle();
if ( k == 1 && !ciclic ) theta[0] = b.thetaBegin();
if ( k == n-2 && !ciclic ) theta[n-1] = b.thetaEnd();
}
}
return true;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ostream_type &
operator << ( ostream_type & stream, Biarc const & bi ) {
stream
<< "C0\n" << bi.m_C0
<< "C1\n" << bi.m_C1
<< "\n";
return stream;
}
}
