Program Listing for File Line.cc¶
↰ Return to documentation for file (Line.cc)
/*--------------------------------------------------------------------------*\
| |
| Copyright (C) 2017 |
| |
| , __ , __ |
| /|/ \ /|/ \ |
| | __/ _ ,_ | __/ _ ,_ |
| | \|/ / | | | | \|/ / | | | |
| |(__/|__/ |_/ \_/|/|(__/|__/ |_/ \_/|/ |
| /| /| |
| \| \| |
| |
| Enrico Bertolazzi |
| Dipartimento di Ingegneria Industriale |
| Universita` degli Studi di Trento |
| email: enrico.bertolazzi@unitn.it |
| |
\*--------------------------------------------------------------------------*/
#include "Clothoids.hh"
// Microsoft visual studio Workaround
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#include <algorithm>
namespace G2lib {
using std::max;
using std::min;
using std::swap;
LineSegment::LineSegment( BaseCurve const & C )
: BaseCurve(G2LIB_LINE)
{
switch ( C.type() ) {
case G2LIB_LINE:
*this = *static_cast<LineSegment const *>(&C);
break;
case G2LIB_CIRCLE:
case G2LIB_CLOTHOID:
case G2LIB_BIARC:
case G2LIB_BIARC_LIST:
case G2LIB_CLOTHOID_LIST:
case G2LIB_POLYLINE:
UTILS_ERROR(
"LineSegment constructor cannot convert from: {}\n",
CurveType_name[C.type()]
);
}
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
typedef struct {
real_type p[2];
real_type q[2];
real_type c;
real_type s;
real_type L;
} L_struct;
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
static
bool
onSegment(
real_type const p[2],
real_type const q[2],
real_type const r[2],
real_type const epsi
) {
real_type mi_x, ma_x;
if ( p[0] > r[0] ) { mi_x = r[0]; ma_x = p[0]; }
else { mi_x = p[0]; ma_x = r[0]; }
bool ok = q[0] <= ma_x+epsi && q[0] >= mi_x-epsi;
if ( ok ) {
real_type mi_y, ma_y;
if ( p[1] > r[1] ) { mi_y = r[1]; ma_y = p[1]; }
else { mi_y = p[1]; ma_y = r[1]; }
ok = q[1] <= ma_y+epsi && q[1] >= mi_y-epsi;
}
return ok;
}
// To find orientation of ordered triplet (p, q, r).
// The function returns following values
// 0 --> p, q and r are collinear
// 1 --> Clockwise
// 2 --> Counterclockwise
static
int_type
orientation(
real_type const p[2],
real_type const q[2],
real_type const r[2],
real_type const epsi
) {
// See https://www.geeksforgeeks.org/orientation-3-ordered-points/
// for details of below formula.
real_type qp_x = q[0] - p[0];
real_type qp_y = q[1] - p[1];
real_type rq_x = r[0] - q[0];
real_type rq_y = r[1] - q[1];
real_type det = qp_y * rq_x - qp_x * rq_y;
if ( std::abs(det) < epsi ) return 0; // collinear
return (det > 0)? 1: 2; // clock or counterclock wise
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
static
bool
intersect(
real_type epsi,
L_struct const & L1,
L_struct const & L2,
real_type & s1,
real_type & s2
) {
// Find the four orientations needed for general and special cases
int_type o1 = orientation( L1.p, L1.q, L2.p, epsi );
int_type o2 = orientation( L1.p, L1.q, L2.q, epsi );
int_type o3 = orientation( L2.p, L2.q, L1.p, epsi );
int_type o4 = orientation( L2.p, L2.q, L1.q, epsi );
// General case
if ( o1 != o2 && o3 != o4 ) {
real_type det = L1.c * L2.s - L1.s * L2.c;
real_type px = L2.p[0]-L1.p[0];
real_type py = L2.p[1]-L1.p[1];
s1 = (px * L2.s - py * L2.c)/ det;
s2 = (px * L1.s - py * L1.c)/ det;
return true;
}
// Special Cases
// p1, q1 and p2 are collinear and p2 lies on segment p1q1
if ( o1 == 0 && onSegment( L1.p, L2.p, L1.q, epsi ) ) {
s1 = hypot( L2.p[0]-L1.p[0], L2.p[1]-L1.p[1] );
s2 = 0;
return true;
}
// p1, q1 and q2 are collinear and q2 lies on segment p1q1
if ( o2 == 0 && onSegment( L1.p, L2.q, L1.q, epsi ) ) {
s1 = hypot( L2.q[0]-L1.p[0], L2.q[1]-L1.p[1] );
s2 = L2.L;
return true;
}
// p2, q2 and p1 are collinear and p1 lies on segment p2q2
if ( o3 == 0 && onSegment( L2.p, L1.p, L2.q, epsi ) ) {
s1 = 0;
s2 = hypot( L1.p[0]-L2.p[0], L1.p[1]-L2.p[1] );
return true;
}
// p2, q2 and q1 are collinear and q1 lies on segment p2q2
if ( o4 == 0 && onSegment( L2.p, L1.q, L2.q, epsi ) ) {
s1 = L1.L;
s2 = hypot( L1.q[0]-L2.p[0], L1.q[1]-L2.p[1] );
return true;
}
s1 = s2 = 0;
return false; // Doesn't fall in any of the above cases
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
static
bool
collision(
real_type epsi,
L_struct const & L1,
L_struct const & L2
) {
// Find the four orientations needed for general and special cases
int_type o1 = orientation( L1.p, L1.q, L2.p, epsi );
int_type o2 = orientation( L1.p, L1.q, L2.q, epsi );
int_type o3 = orientation( L2.p, L2.q, L1.p, epsi );
int_type o4 = orientation( L2.p, L2.q, L1.q, epsi );
// General case
if ( o1 != o2 && o3 != o4 ) return true;
// Special Cases
// p1, q1 and p2 are collinear and p2 lies on segment p1q1
if ( o1 == 0 && onSegment( L1.p, L2.p, L1.q, epsi ) ) return true;
// p1, q1 and q2 are collinear and q2 lies on segment p1q1
if ( o2 == 0 && onSegment( L1.p, L2.q, L1.q, epsi ) ) return true;
// p2, q2 and p1 are collinear and p1 lies on segment p2q2
if ( o3 == 0 && onSegment( L2.p, L1.p, L2.q, epsi ) ) return true;
// p2, q2 and q1 are collinear and q1 lies on segment p2q2
if ( o4 == 0 && onSegment( L2.p, L1.q, L2.q, epsi ) ) return true;
return false; // Doesn't fall in any of the above cases
}
#endif
/*\
| _ _
| | | (_)_ __ ___
| | | | | '_ \ / _ \
| | |___| | | | | __/
| |_____|_|_| |_|\___|
\*/
void
LineSegment::build_2P(
real_type x0,
real_type y0,
real_type x1,
real_type y1
) {
real_type dx = x1-x0;
real_type dy = y1-y0;
m_L = hypot( dx, dy );
m_x0 = x0;
m_y0 = y0;
m_theta0 = atan2(dy, dx);
if ( m_L > 0 ) {
m_c0 = dx / m_L;
m_s0 = dy / m_L;
} else {
m_c0 = m_s0 = 0;
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::bbox(
real_type & xmin,
real_type & ymin,
real_type & xmax,
real_type & ymax
) const {
xmin = m_x0; xmax = m_x0+m_L*m_c0;
ymin = m_y0; ymax = m_y0+m_L*m_s0;
if ( xmin > xmax ) swap( xmin, xmax );
if ( ymin > ymax ) swap( ymin, ymax );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::bbox_ISO(
real_type offs,
real_type & xmin,
real_type & ymin,
real_type & xmax,
real_type & ymax
) const {
real_type dx = offs*nx_Begin_ISO();
real_type dy = offs*ny_Begin_ISO();
xmin = m_x0+dx; xmax = xEnd()+dx;
ymin = m_y0+dy; ymax = yEnd()+dy;
if ( xmin > xmax ) swap( xmin, xmax );
if ( ymin > ymax ) swap( ymin, ymax );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::rotate( real_type angle, real_type cx, real_type cy ) {
real_type dx = m_x0 - cx;
real_type dy = m_y0 - cy;
real_type C = cos(angle);
real_type S = sin(angle);
real_type ndx = C*dx - S*dy;
real_type ndy = C*dy + S*dx;
m_x0 = cx + ndx;
m_y0 = cy + ndy;
m_theta0 += angle;
m_c0 = cos(m_theta0);
m_s0 = sin(m_theta0);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::reverse() {
m_x0 += m_c0 * m_L;
m_y0 += m_s0 * m_L;
m_c0 = -m_c0;
m_s0 = -m_s0;
m_theta0 += Utils::m_pi;
if ( m_theta0 > Utils::m_pi ) m_theta0 -= Utils::m_2pi;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::paramNURBS( int_type & n_knots, int_type & n_pnts ) const {
n_pnts = 2;
n_knots = 4;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::toNURBS( real_type * knots, real_type Poly[][3] ) const {
knots[0] = knots[1] = 0;
knots[2] = knots[3] = 1;
Poly[0][0] = m_x0;
Poly[0][1] = m_y0;
Poly[0][2] = 1;
Poly[1][0] = m_x0+m_L*m_c0;
Poly[1][1] = m_y0+m_L*m_s0;
Poly[1][2] = 1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
LineSegment::toBS( real_type * knots, real_type Poly[2][2] ) const {
knots[0] = knots[1] = 0;
knots[2] = knots[3] = 1;
Poly[0][0] = m_x0;
Poly[0][1] = m_y0;
Poly[1][0] = m_x0+m_L*m_c0;
Poly[1][1] = m_y0+m_L*m_s0;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
LineSegment::intersect(
LineSegment const & S,
real_type & s1,
real_type & s2
) const {
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
L_struct L1;
L_struct L2;
L1.p[0] = xBegin();
L1.p[1] = yBegin();
L1.q[0] = xEnd();
L1.q[1] = yEnd();
L1.c = m_c0;
L1.s = m_s0;
L1.L = m_L;
L2.p[0] = S.xBegin();
L2.p[1] = S.yBegin();
L2.q[0] = S.xEnd();
L2.q[1] = S.yEnd();
L2.c = S.m_c0;
L2.s = S.m_s0;
L2.L = S.m_L;
real_type const epsi = max(m_L,S.m_L)*machepsi100;
return G2lib::intersect( epsi, L1, L2, s1, s2 );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
LineSegment::intersect_ISO(
real_type offs,
LineSegment const & S,
real_type S_offs,
real_type & s1,
real_type & s2
) const {
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
L_struct L1;
L_struct L2;
L1.p[0] = xBegin_ISO(offs);
L1.p[1] = yBegin_ISO(offs);
L1.q[0] = xEnd_ISO(offs);
L1.q[1] = yEnd_ISO(offs);
L1.c = m_c0;
L1.s = m_s0;
L1.L = m_L;
L2.p[0] = S.xBegin_ISO(S_offs);
L2.p[1] = S.yBegin_ISO(S_offs);
L2.q[0] = S.xEnd_ISO(S_offs);
L2.q[1] = S.yEnd_ISO(S_offs);
L2.c = S.m_c0;
L2.s = S.m_s0;
L2.L = S.m_L;
real_type const epsi = max(m_L,S.m_L)*machepsi100;
return G2lib::intersect( epsi, L1, L2, s1, s2 );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
LineSegment::collision( LineSegment const & S ) const {
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
L_struct L1;
L_struct L2;
L1.p[0] = xBegin();
L1.p[1] = yBegin();
L1.q[0] = xEnd();
L1.q[1] = yEnd();
L1.c = m_c0;
L1.s = m_s0;
L1.L = m_L;
L2.p[0] = S.xBegin();
L2.p[1] = S.yBegin();
L2.q[0] = S.xEnd();
L2.q[1] = S.yEnd();
L2.c = S.m_c0;
L2.s = S.m_s0;
L2.L = S.m_L;
real_type const epsi = max(m_L,S.m_L)*machepsi100;
return G2lib::collision( epsi, L1, L2 );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
LineSegment::collision_ISO(
real_type offs,
LineSegment const & S,
real_type S_offs
) const {
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
L_struct L1;
L_struct L2;
L1.p[0] = xBegin_ISO(offs);
L1.p[1] = yBegin_ISO(offs);
L1.q[0] = xEnd_ISO(offs);
L1.q[1] = yEnd_ISO(offs);
L1.c = m_c0;
L1.s = m_s0;
L1.L = m_L;
L2.p[0] = S.xBegin_ISO(S_offs);
L2.p[1] = S.yBegin_ISO(S_offs);
L2.q[0] = S.xEnd_ISO(S_offs);
L2.q[1] = S.yEnd_ISO(S_offs);
L2.c = S.m_c0;
L2.s = S.m_s0;
L2.L = S.m_L;
real_type const epsi = max(m_L,S.m_L)*machepsi100;
return G2lib::collision( epsi, L1, L2 );
}
/*\
| _ _ _
| (_)_ __ | |_ ___ _ __ ___ ___ ___| |_
| | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
| | | | | | || __/ | \__ \ __/ (__| |_
| |_|_| |_|\__\___|_| |___/\___|\___|\__|
\*/
/*\
| _ _ ____ _ _
| ___| | ___ ___ ___ ___| |_| _ \ ___ (_)_ __ | |_
| / __| |/ _ \/ __|/ _ \/ __| __| |_) / _ \| | '_ \| __|
| | (__| | (_) \__ \ __/\__ \ |_| __/ (_) | | | | | |_
| \___|_|\___/|___/\___||___/\__|_| \___/|_|_| |_|\__|
\*/
int_type
LineSegment::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 dx = qx - m_x0;
real_type dy = qy - m_y0;
s = dx * tx_Begin() + dy * ty_Begin();
t = dx * nx_Begin_ISO() + dy * ny_Begin_ISO();
if ( s < 0 ) { // distanza sul bordo 0
s = 0;
x = m_x0;
y = m_y0;
} else if ( s > m_L ) {
s = m_L;
eval( s, x, y );
} else {
dst = std::abs(t);
eval( s, x, y );
return 1;
}
dx = qx-x;
dy = qy-y;
t = dx * nx_Begin_ISO() + dy * ny_Begin_ISO();
dst = hypot( dx, dy );
return -1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int_type
LineSegment::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 xx0 = m_x0+offs*nx_Begin_ISO();
real_type yy0 = m_y0+offs*ny_Begin_ISO();
real_type dx = qx - xx0;
real_type dy = qy - yy0;
s = dx * tx_Begin() + dy * ty_Begin();
t = dx * nx_Begin_ISO() + dy * ny_Begin_ISO();
if ( s < 0 ) { // distanza sul bordo 0
s = 0;
x = xx0;
y = yy0;
} else if ( s > m_L ) {
s = m_L;
eval_ISO( s, offs, x, y );
} else {
t += offs;
dst = std::abs(t);
eval_ISO( s, offs, x, y );
}
dx = qx-x;
dy = qy-y;
t = dx * nx_Begin_ISO() + dy * ny_Begin_ISO() + offs;
dst = hypot( dx, dy );
return -1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ostream_type &
operator << ( ostream_type & stream, LineSegment const & c ) {
fmt::print( stream,
"x0 = {}\n"
"y0 = {}\n"
"theta0 = {}\n"
"L = {}\n",
c.m_x0, c.m_y0, c.m_theta0, c.m_L
);
return stream;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
}
// EOF: Line.cc
