Пример #1
0
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;
}
Пример #2
0
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;
}
Пример #3
0
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;
}
Пример #4
0
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);
}