int ON_Intersect( // returns 0 = no intersections,
// 1 = one intersection,
// 2 = 2 intersections
// If 0 is returned, first point is point
// on line closest to sphere and 2nd point is the point
// on the sphere closest to the line.
// If 1 is returned, first point is obtained by evaluating
// the line and the second point is obtained by evaluating
// the sphere.
const ON_Line& line, const ON_Sphere& sphere,
ON_3dPoint& A, ON_3dPoint& B // intersection point(s) returned here
)
{
int rc = 0;
const ON_3dPoint sphere_center = sphere.plane.origin;
const double sphere_radius = fabs(sphere.radius);
double tol = sphere_radius*ON_SQRT_EPSILON;
if ( tol < ON_ZERO_TOLERANCE )
tol = ON_ZERO_TOLERANCE;
ON_3dPoint line_center = line.ClosestPointTo(sphere_center);
double d = line_center.DistanceTo(sphere_center);
if ( d >= sphere_radius-tol ) {
rc = ( d <= sphere_radius-tol ) ? 1 : 0;
A = line_center;
B = sphere.ClosestPointTo(line_center);
}
else {
d /= sphere_radius;
double h = sphere_radius*sqrt(1.0 - d*d);
ON_3dVector V = line.Direction();
V.Unitize();
A = sphere.ClosestPointTo(line_center - h*V);
B = sphere.ClosestPointTo(line_center + h*V);
d = A.DistanceTo(B);
if ( d <= ON_ZERO_TOLERANCE ) {
A = line_center;
B = sphere.ClosestPointTo(line_center);
rc = 1;
}
else
rc = 2;
}
return rc;
}
bool ON_Intersect( const ON_BoundingBox& bbox,
const ON_Line& line,
double tol,
ON_Interval* line_parameters)
{
double a,b,d,mn,mx,s0,s1, t0, t1;
const double M = 1.0e308;
// i,j,k are indices of coordinates to trim.
// trim the direction with the biggest line deltas first
ON_3dVector v = line.Direction();
const int i = v.MaximumCoordinateIndex();
// gaurd against ON_UNSET_VALUE as input
if ( !(tol >= 0.0) )
tol = 0.0;
// clip i-th coordinate
a = line.from[i];
b = line.to[i];
mn = bbox.m_min[i];
mx = bbox.m_max[i];
if ( !(mn <= mx) )
return false;
mn -= (tol+a);
mx += (tol-a);
if ( !(mn <= mx) )
return false;
d = b-a;
if ( 0.0 == d )
{
// happens when line.from == line.to
if ( 0.0 < mn || 0.0 > mx )
{
// point is in box
if ( line_parameters )
{
// setting parameters makes no sense - just use 0.0
// so it's clear we have a point
line_parameters->Set(0.0,0.0);
}
return true;
}
return false; // point is outside box
}
if ( fabs(d) < 1.0 && (fabs(mn) >= fabs(d)*M || fabs(mx) >= fabs(d)*M) )
{
// the value of mn/d or mx/d is too large for a realistic answer to be computed
return false;
}
d = 1.0/d;
t0 = mn*d;
t1 = mx*d;
// set "chord" = line segment that begins and ends on the
// i-th coordinate box side planes.
ON_Line chord(line.PointAt(t0),line.PointAt(t1));
// test j-th coordinate direction
const int j = (i + (fabs(v[(i+1)%3])>fabs(v[(i+2)%3])?1:2) ) % 3;
a = chord.from[j];
b = chord.to[j];
mn = bbox.m_min[j];
mx = bbox.m_max[j];
if ( !(mn <= mx) )
return false;
mn -= (tol+a);
mx += (tol-a);
if ( !(mn <= mx) )
return false;
d = b-a;
if ( (0.0 < mn && d < mn) || (0.0 > mx && d > mx) )
{
// chord lies outside the box
return false;
}
while ( fabs(d) >= 1.0 || (fabs(mn) <= fabs(d)*M && fabs(mx) <= fabs(d)*M) )
{
// The chord is not (nearly) parallel to the j-th sides.
// See if the chord needs to be trimmed by the j-th sides.
d = 1.0/d;
s0 = mn*d;
s1 = mx*d;
if ( s0 > 1.0 )
{
if ( s1 > 1.0 )
{
// unstable calculation happens when
// fabs(d) is very tiny and chord is
// on the j-th side.
break;
}
s0 = 1.0;
}
else if ( s0 < 0.0 )
{
if (s1 < 0.0)
{
// unstable calculation happens when
// fabs(d) is very tiny and chord is
// on the j-th side.
break;
}
s0 = 0.0;
}
if ( s1 < 0.0 ) s1 = 0.0; else if ( s1 > 1.0 ) s1 = 1.0;
d = (1.0-s0)*t0 + s0*t1;
t1 = (1.0-s1)*t0 + s1*t1;
t0 = d;
v = chord.PointAt(s0);
chord.to = chord.PointAt(s1);
chord.from = v;
break;
}
// test k-th coordinate direction
const int k = (i&&j) ? 0 : ((i!=1&&j!=1)?1:2);
a = chord.from[k];
b = chord.to[k];
mn = bbox.m_min[k];
mx = bbox.m_max[k];
if ( !(mn <= mx) )
return false;
mn -= (tol+a);
mx += (tol-a);
if ( !(mn <= mx) )
return false;
d = b-a;
if ( (0.0 < mn && d < mn) || (0.0 > mx && d > mx) )
{
// chord does not intersect the rectangle
return false;
}
if ( line_parameters )
{
while ( fabs(d) >= 1.0 || (fabs(mn) <= fabs(d)*M && fabs(mx) <= fabs(d)*M) )
{
// The chord is not (nearly) parallel to the k-th sides.
// See if the chord needs to be trimmed by the k-th sides.
d = 1.0/d;
s0 = mn*d;
s1 = mx*d;
if ( s0 > 1.0 )
{
if ( s1 > 1.0 )
{
// unstable calculation happens when
// fabs(d) is very tiny and chord is
// on the k-th side.
break;
}
s0 = 1.0;
}
else if ( s0 < 0.0 )
{
if (s1 < 0.0)
{
// unstable calculation happens when
// fabs(d) is very tiny and chord is
// on the k-th side.
break;
}
s0 = 0.0;
}
if ( s1 < 0.0 ) s1 = 0.0; else if ( s1 > 1.0 ) s1 = 1.0;
d = (1.0-s0)*t0 + s0*t1;
t1 = (1.0-s1)*t0 + s1*t1;
t0 = d;
break;
}
if (t0 > t1 )
{
line_parameters->Set(t1,t0);
}
else
{
line_parameters->Set(t0,t1);
}
}
return true;
}
bool ON_Intersect( const ON_Line& lineA, const ON_Line& lineB,
double* lineA_parameter,
double* lineB_parameter
)
{
// If you are looking at this code because you don't like an
// answer you are getting, then the first thing to try is
// to read the header file comments and try calling
// ON_IntersectLineLine.
bool rc = false;
double M_zero_tol = 0.0;
int i, rank;
double pr_tolerance, pivot, X[2], Y[2];
ON_3dVector A = lineA.Direction();
ON_3dVector B = lineB.Direction();
ON_3dVector C = lineB[0] - lineA[0];
ON_Matrix M(2,2);
M[0][0] = ON_DotProduct( A, A );
M[1][1] = ON_DotProduct( B, B );
M[0][1] = M[1][0] = -ON_DotProduct( A, B );
// this swap done to get row+col pivot accuracy
if ( M[0][0] < M[1][1] ) {
M.SwapCols(0,1);
i = 1;
}
else {
i = 0;
}
pr_tolerance = fabs(M[1][1])*ON_SQRT_EPSILON;
M_zero_tol = fabs(M[1][1])*ON_EPSILON;
Y[0] = ON_DotProduct( A, C );
Y[1] = -ON_DotProduct( B, C );
rank = M.RowReduce( M_zero_tol, Y, &pivot );
if ( rank == 2 )
{
// 19 November 2003 Dale Lear and Chuck
// Added lineA.from/to == lineB.from/to tests
// so exact answer gets returned when people
// expect it.
rc = true;
if ( lineA.from == lineB.from )
{
if ( lineA_parameter )
*lineA_parameter = 0.0;
if ( lineB_parameter )
*lineB_parameter = 0.0;
}
else if ( lineA.from == lineB.to )
{
if ( lineA_parameter )
*lineA_parameter = 0.0;
if ( lineB_parameter )
*lineB_parameter = 1.0;
}
else if ( lineA.to == lineB.from )
{
if ( lineA_parameter )
*lineA_parameter = 1.0;
if ( lineB_parameter )
*lineB_parameter = 0.0;
}
else if ( lineA.to == lineB.to )
{
if ( lineA_parameter )
*lineA_parameter = 1.0;
if ( lineB_parameter )
*lineB_parameter = 1.0;
}
else
{
rc = M.BackSolve( 0.0, 2, Y, X );
if ( rc )
{
if ( lineA_parameter )
*lineA_parameter = X[i];
if ( lineB_parameter )
*lineB_parameter = X[1-i];
if ( fabs(pivot) <= pr_tolerance )
{
// test answer because matrix was close to singular
// (This test is slow but it is rarely used.)
ON_3dPoint pA = lineA.PointAt(X[i]);
ON_3dPoint pB = lineB.PointAt(X[1-i]);
double d = pA.DistanceTo(pB);
if ( d > pr_tolerance && d > ON_ZERO_TOLERANCE ) {
ON_3dPoint qA = lineA.ClosestPointTo(pB);
ON_3dPoint qB = lineB.ClosestPointTo(pA);
double dA = pA.DistanceTo(qB);
double dB = pB.DistanceTo(qA);
if ( 1.1*dA < d ) {
rc = false;
}
else if ( 1.1*dB < d ) {
rc = false;
}
}
}
}
}
}
return rc;
}
bool ON_Line::IsFartherThan( double d, const ON_Line& L ) const
{
ON_3dPoint A, B;
double a, b, t, x;
bool bCheckA, bCheckB;
a = from.x; if (to.x < a) {b=a; a = to.x;} else b = to.x;
if ( b+d < L.from.x && b+d < L.to.x )
return true;
if ( a-d > L.from.x && a-d > L.to.x )
return true;
a = from.y; if (to.y < a) {b=a; a = to.y;} else b = to.y;
if ( b+d < L.from.y && b+d < L.to.y )
return true;
if ( a-d > L.from.y && a-d > L.to.y )
return true;
a = from.z; if (to.z < a) {b=a; a = to.z;} else b = to.z;
if ( b+d < L.from.z && b+d < L.to.z )
return true;
if ( a-d > L.from.z && a-d > L.to.z )
return true;
if ( !ON_Intersect(*this,L,&a,&b) )
{
// lines are parallel or anti parallel
if ( Direction()*L.Direction() >= 0.0 )
{
// lines are parallel
a = 0.0;
L.ClosestPointTo(from,&b);
// If ( b >= 0.0), then this->from and L(b) are a pair of closest points.
if ( b < 0.0 )
{
// Othersise L.from and this(a) are a pair of closest points.
b = 0.0;
ClosestPointTo(L.from,&a);
}
}
else
{
// lines are anti parallel
a = 1.0;
L.ClosestPointTo(to,&b);
// If ( b >= 0.0), then this->to and L(b) are a pair of closest points.
if ( b < 0.0 )
{
// Othersise L.to and this(a) are a pair of closest points.
b = 0.0;
ClosestPointTo(L.from,&a);
}
}
}
A = PointAt(a);
B = L.PointAt(b);
x = A.DistanceTo(B);
if (x > d)
return true;
bCheckA = true;
if ( a < 0.0) a = 0.0; else if (a > 1.0) a = 1.0; else bCheckA=false;
if (bCheckA )
{
A = PointAt(a);
L.ClosestPointTo(A,&t);
if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
x = L.PointAt(t).DistanceTo(A);
}
bCheckB = true;
if ( b < 0.0) b = 0.0; else if (b > 1.0) b = 1.0; else bCheckB=false;
if ( bCheckB )
{
B = L.PointAt(b);
ClosestPointTo(B,&t);
if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
t = PointAt(t).DistanceTo(B);
if ( bCheckA )
{
if ( t<x ) x = t;
}
else
{
x = t;
}
}
return (x > d);
}