Program Listing for File Triangle2D.cc¶
↰ Return to documentation for file (Triangle2D.cc)
/*
* Triangle-Triangle Overlap Test Routines
* July, 2002
* Updated December 2003
*
* This file contains C implementation of algorithms for
* performing two and three-dimensional triangle-triangle intersection test
* The algorithms and underlying theory are described in
*
* "Fast and Robust Triangle-Triangle Overlap Test Using Orientation Predicates"
* P. Guigue - O. Devillers
*
* Journal of Graphics Tools, 8(1), 2003
*
* Several geometric predicates are defined. Their parameters are all
* points. Each point is an array of two or three double precision
* floating point numbers. The geometric predicates implemented in
* this file are:
*
* int tri_tri_overlap_test_3d(p1,q1,r1,p2,q2,r2)
* int tri_tri_overlap_test_2d(p1,q1,r1,p2,q2,r2)
*
* int tri_tri_intersection_test_3d(p1,q1,r1,p2,q2,r2,
* coplanar,source,target)
*
* is a version that computes the segment of intersection when
* the triangles overlap (and are not coplanar)
*
* each function returns 1 if the triangles (including their
* boundary) intersect, otherwise 0
*
*
* Other information are available from the Web page
* http://www.acm.org/jgt/papers/GuigueDevillers03/
*
*/
/*
*
* Two dimensional Triangle-Triangle Overlap Test
*
*/
#include "Clothoids.hh"
// workaround for windows that defines max and min as macros!
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#include <functional>
#include <algorithm>
namespace G2lib {
using std::min;
using std::max;
using std::swap;
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
inline
real_type
orient_2d(
real_type const a[2],
real_type const b[2],
real_type const c[2]
) {
return (a[0]-c[0]) * (b[1]-c[1]) - (a[1]-c[1]) * (b[0]-c[0]);
}
static
inline
bool
intersection_test_vertex(
real_type const P1[2],
real_type const Q1[2],
real_type const R1[2],
// - - - - - - - - - -
real_type const P2[2],
real_type const Q2[2],
real_type const R2[2]
) {
if ( orient_2d(R2,P2,Q1) >= 0 ) {
if ( orient_2d(R2,Q2,Q1) <= 0 ) {
if ( orient_2d(P1,P2,Q1) > 0 ) {
return orient_2d(P1,Q2,Q1) <= 0;
} else {
return orient_2d(P1,P2,R1) >= 0 && orient_2d(Q1,R1,P2) >= 0;
}
} else {
return orient_2d(P1,Q2,Q1) <= 0 &&
orient_2d(R2,Q2,R1) <= 0 &&
orient_2d(Q1,R1,Q2) >= 0;
}
} else {
if ( orient_2d(R2,P2,R1) >= 0 ) {
if ( orient_2d(Q1,R1,R2) >= 0 ) {
return orient_2d(P1,P2,R1) >= 0;
} else {
return orient_2d(Q1,R1,Q2) >= 0 && orient_2d(R2,R1,Q2) >= 0;
}
} else {
return false;
}
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
static
inline
bool
intersection_test_edge(
real_type const P1[2],
real_type const Q1[2],
real_type const R1[2],
real_type const P2[2],
real_type const R2[2]
) {
if ( orient_2d(R2,P2,Q1) >= 0 ) {
if ( orient_2d(P1,P2,Q1) >= 0 ) {
return orient_2d(P1,Q1,R2) >= 0;
} else {
return orient_2d(Q1,R1,P2) >= 0 && orient_2d(R1,P1,P2) >= 0;
}
} else if ( orient_2d(R2,P2,R1) >= 0 ) {
return orient_2d(P1,P2,R1) >= 0 &&
( orient_2d(P1,R1,R2) >= 0 || orient_2d(Q1,R1,R2) >= 0 );
}
return false;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
static
inline
bool
tri_tri_intersection_2d(
real_type const p1[2],
real_type const q1[2],
real_type const r1[2],
real_type const p2[2],
real_type const q2[2],
real_type const r2[2]
) {
if ( orient_2d(p2,q2,p1) >= 0 ) {
if ( orient_2d(q2,r2,p1) >= 0 ) {
return orient_2d(r2,p2,p1) >= 0 || intersection_test_edge(p1,q1,r1,p2,r2);
} else {
if ( orient_2d(r2,p2,p1) >= 0 ) return intersection_test_edge(p1,q1,r1,r2,q2);
else return intersection_test_vertex(p1,q1,r1,p2,q2,r2);
}
} else {
if ( orient_2d(q2,r2,p1) >= 0 ) {
if ( orient_2d(r2,p2,p1) >= 0 ) return intersection_test_edge(p1,q1,r1,q2,p2);
else return intersection_test_vertex(p1,q1,r1,q2,r2,p2);
} else {
return intersection_test_vertex(p1,q1,r1,r2,p2,q2);
}
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
static
inline
bool
tri_tri_overlap_test_2d(
real_type const p1[2],
real_type const q1[2],
real_type const r1[2],
real_type const p2[2],
real_type const q2[2],
real_type const r2[2]
) {
if ( orient_2d(p1,q1,r1) < 0 ) {
if ( orient_2d(p2,q2,r2) < 0 )
return tri_tri_intersection_2d(p1,r1,q1,p2,r2,q2);
else
return tri_tri_intersection_2d(p1,r1,q1,p2,q2,r2);
} else {
if ( orient_2d(p2,q2,r2) < 0 )
return tri_tri_intersection_2d(p1,q1,r1,p2,r2,q2);
else
return tri_tri_intersection_2d(p1,q1,r1,p2,q2,r2);
}
}
#endif
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
Triangle2D::rotate( real_type angle, real_type cx, real_type cy ) {
real_type C = cos(angle);
real_type S = sin(angle);
real_type dx = m_p1[0] - cx;
real_type dy = m_p1[1] - cy;
real_type ndx = C*dx - S*dy;
real_type ndy = C*dy + S*dx;
m_p1[0] = cx + ndx;
m_p1[1] = cy + ndy;
dx = m_p2[0] - cx;
dy = m_p2[1] - cy;
ndx = C*dx - S*dy;
ndy = C*dy + S*dx;
m_p2[0] = cx + ndx;
m_p2[1] = cy + ndy;
dx = m_p3[0] - cx;
dy = m_p3[1] - cy;
ndx = C*dx - S*dy;
ndy = C*dy + S*dx;
m_p3[0] = cx + ndx;
m_p3[1] = cy + ndy;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool
Triangle2D::overlap( Triangle2D const & t2 ) const {
return tri_tri_overlap_test_2d( m_p1, m_p2, m_p3, t2.m_p1, t2.m_p2, t2.m_p3 );
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
real_type
Triangle2D::distMax( real_type x, real_type y ) const {
real_type d1 = hypot( x-m_p1[0], y-m_p1[1] );
real_type d2 = hypot( x-m_p2[0], y-m_p2[1] );
real_type d3 = hypot( x-m_p3[0], y-m_p3[1] );
return max(d1,max(d2,d3));
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifndef DOXYGEN_SHOULD_SKIP_THIS
static
real_type
distSeg(
real_type x,
real_type y,
real_type const * A,
real_type const * B
) {
real_type dx = x - A[0];
real_type dy = y - A[1];
real_type dx1 = B[0] - A[0];
real_type dy1 = B[1] - A[1];
// < P-A - s*(B-A), B-A> = 0
// <P-A, B-A> = s <B-A,B-A>
real_type tmp = dx * dx1 + dy * dy1;
if ( tmp < 0 ) return hypot(dx,dy);
real_type tmp2 = dx1*dx1+dy1*dy1;
if ( tmp > tmp2 ) return hypot(x-B[0],y-B[1]);
real_type S = tmp/tmp2;
real_type X = A[0] + S*dx1;
real_type Y = A[1] + S*dy1;
return hypot( x-X, y-Y );
}
#endif
real_type
Triangle2D::distMin( real_type x, real_type y ) const {
int_type in = isInside( x, y );
if ( in >= 0 ) return 0;
#if 0
LineSegment L1, L2, L3;
L1.build_2P( p1, p2 );
L2.build_2P( p2, p3 );
L3.build_2P( p3, p1 );
real_type d1 = L1.distance( x, y );
real_type d2 = L2.distance( x, y );
real_type d3 = L3.distance( x, y );
#else
real_type d1 = distSeg( x, y, m_p1, m_p2 );
real_type d2 = distSeg( x, y, m_p2, m_p3 );
real_type d3 = distSeg( x, y, m_p3, m_p1 );
#endif
if ( d1 > d2 ) swap( d1, d2 );
if ( d1 > d3 ) swap( d1, d3 );
return d1;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ostream_type &
operator << ( ostream_type & stream, Triangle2D const & t ) {
fmt::print( stream,
"Triangle2D\n"
"P0 = [{},{}]\n"
"P1 = [{},{}]\n"
"P2 = [{},{}]\n",
t.m_p1[0], t.m_p1[1],
t.m_p2[0], t.m_p2[1],
t.m_p3[0], t.m_p3[1]
);
return stream;
}
}
