text: /* $NoKeywords: $ */
  /*
  //
  // Copyright (c) 1993-2011 Robert McNeel & Associates. All rights reserved.
  // OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
  // McNeel & Associates.
  //
  // THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
  // ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
  // MERCHANTABILITY ARE HEREBY DISCLAIMED.
  //
  // For complete openNURBS copyright information see <http://www.opennurbs.org>.
  //
  ////////////////////////////////////////////////////////////////
  */
  
  #include <pcl/surface/openNURBS/opennurbs.h>
  
  ON_Line::ON_Line() {
      from.Zero();
      to.Zero();
  }
  
  ON_Line::ON_Line(const ON_3dPoint &from_pt, const ON_3dPoint &to_pt)
      : from(from_pt), to(to_pt) {}
  
  ON_Line::~ON_Line() {}
  
  ON_3dPoint &ON_Line::operator[](int i) { return (i <= 0) ? from : to; }
  
  const ON_3dPoint &ON_Line::operator[](int i) const {
      return (i <= 0) ? from : to;
  }
  
  bool ON_Line::Create(const ON_3dPoint &from_pt, const ON_3dPoint &to_pt) {
      from = from_pt;
      to = to_pt;
      return IsValid();
  }
  
  bool ON_Line::IsValid() const {
      return (from != to && from.IsValid() && to.IsValid());
  }
  
  double ON_Line::Length() const { return from.DistanceTo(to); }
  
  ON_3dVector ON_Line::Direction() const { return (to - from); }
  
  ON_3dVector ON_Line::Tangent() const {
      ON_3dVector V = Direction();
      V.Unitize();
      return V;
  }
  
  ON_3dPoint ON_Line::PointAt(double t) const {
      // 26 Feb 2003 Dale Lear
      //     Changed
      //          return (1-t)*from + t*to;
      //     to the following so that axis aligned lines will
      //     return exact answers for large values of t.
      //     See RR 9683.
      const double s = 1.0 - t;
      return ON_3dPoint((from.x == to.x) ? from.x : s * from.x + t * to.x,
                        (from.y == to.y) ? from.y : s * from.y + t * to.y,
                        (from.z == to.z) ? from.z : s * from.z + t * to.z);
  }
  
  void ON_Line::Reverse() {
      ON_3dPoint tmp = from;
      from = to;
      to = tmp;
  }
  
  bool ON_Line::ClosestPointTo(const ON_3dPoint &point, double *t) const {
      bool rc = false;
      if (t) {
          const ON_3dVector D = Direction();
          const double DoD = D.LengthSquared();
          if (DoD > 0.0) {
              if (point.DistanceTo(from) <= point.DistanceTo(to)) {
                  *t = ((point - from) * D) / DoD;
              } else {
                  *t = 1.0 + ((point - to) * D) / DoD;
              }
              rc = true;
          } else {
              *t = 0.0;
          }
      }
      return rc;
  }
  
  ON_3dPoint ON_Line::ClosestPointTo(const ON_3dPoint &point) const {
      double t;
      ClosestPointTo(point, &t);
      return PointAt(t);
  }
  
  double ON_Line::DistanceTo(ON_3dPoint test_point) const {
      return test_point.DistanceTo(ClosestPointTo(test_point));
  }
  
  bool ON_Line::Transform(const ON_Xform &tr) {
      from = tr * from;
      to = tr * to;
      // 5 June 2003 Dale Lear RR 10493
      //    Always return true.
      // return (from != to) ? true : false;
      return true;
  }
  
  // rotate line about a point and axis
  bool ON_Line::Rotate(double sin_angle,        // sin(angle)
                       double cos_angle,        // cos(angle)
                       const ON_3dVector &axis, // axis of rotation
                       const ON_3dPoint &center // center of rotation
  ) {
      ON_Xform rot;
      rot.Rotation(sin_angle, cos_angle, axis, center);
      const bool bFixP0 = (from == center);
      const bool bFixP1 = (to == center);
      const bool rc = Transform(rot);
      if (bFixP0)
          from = center;
      if (bFixP1)
          to = center;
      return rc;
  }
  
  bool ON_Line::Rotate(double a,                // angle in radians
                       const ON_3dVector &axis, // axis of rotation
                       const ON_3dPoint &center // center of rotation
  ) {
      return Rotate(sin(a), cos(a), axis, center);
  }
  
  bool ON_Line::Translate(const ON_3dVector &delta) {
      ON_Xform tr;
      tr.Translation(delta);
      return Transform(tr);
  }
  
  int ON_ArePointsOnLine( // returns 0=no, 1 = yes, 2 = pointset is (to tolerance)
                          // a single point on the line
      int dim,            // 2 or 3
      int is_rat, int count, int stride, const double *point,
      const ON_BoundingBox &
          bbox, // if needed, use ON_GetBoundingBox(dim,is_rat,count,stride,point)
      const ON_Line &line, // line to test
      double tolerance) {
      double w;
      int i, j, k;
  
      if (count < 1)
          return 0;
  
      if (!line.IsValid()) {
          ON_ERROR("line parameter not valid");
          return 0;
      }
      if (!bbox.IsValid()) {
          ON_ERROR("bbox parameter not valid");
          return 0;
      }
      if (!ON_IsValid(tolerance) || tolerance < 0.0) {
          ON_ERROR("tolerance parameter not valid");
          return 0;
      }
      if (dim < 2 || dim > 3) {
          ON_ERROR("dim parameter not valid");
          return 0;
      }
      if (0 == point) {
          ON_ERROR("point parameter not valid");
          return 0;
      }
      if (stride < (is_rat ? (dim + 1) : dim)) {
          ON_ERROR("stride parameter not valid");
          return 0;
      }
  
      int rc = 0;
  
      if (tolerance == 0.0) {
          tolerance = bbox.Tolerance();
      }
  
      ON_3dPoint Q;
  
      // test bounding box to quickly detect the common coordinate axis cases
      rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
      for (i = 0; rc && i < 2; i++) {
          Q.x = bbox[i].x;
          for (j = 0; rc && j < 2; j++) {
              Q.y = bbox[j].y;
              for (k = 0; rc && k < 2; k++) {
                  Q.z = bbox[k].z;
                  if (Q.DistanceTo(line.ClosestPointTo(Q)) > tolerance)
                      rc = 0;
              }
          }
      }
  
      if (!rc) {
          // test points one by one
          Q.Zero();
          rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
          if (is_rat) {
              for (i = 0; i < count; i++) {
                  w = point[dim];
                  if (w == 0.0) {
                      ON_ERROR("rational point has zero weight");
                      return 0;
                  }
                  ON_ArrayScale(dim, 1.0 / w, point, &Q.x);
                  if (Q.DistanceTo(line.ClosestPointTo(Q)) > tolerance) {
                      rc = 0;
                      break;
                  }
                  point += stride;
              }
          } else {
              for (i = 0; i < count; i++) {
                  memcpy(&Q.x, point, dim * sizeof(Q.x));
                  if (Q.DistanceTo(line.ClosestPointTo(Q)) > tolerance) {
                      rc = 0;
                      break;
                  }
                  point += stride;
              }
          }
      }
  
      return rc;
  }
  
  ON_BoundingBox ON_Line::BoundingBox() const {
      ON_BoundingBox bbox;
      bbox.Set(3, false, 2, 3, &from.x, false);
      return bbox;
  }
  
  bool ON_Line::GetBoundingBox(ON_BoundingBox &bbox, int bGrowBox) const {
      bbox.Set(3, false, 2, 3, &from.x, bGrowBox);
      return true;
  }
  
  bool ON_Line::InPlane(ON_Plane &plane, double tolerance) const {
      const ON_3dVector v = to - from;
      const bool bTinyX = fabs(v.x) <= tolerance;
      const bool bTinyY = fabs(v.y) <= tolerance;
      const bool bTinyZ = fabs(v.z) <= tolerance;
      bool rc = true;
      ON_3dVector X;
      ON_3dVector Y;
      if (bTinyZ && (!bTinyX || !bTinyY)) {
          X = ON_xaxis;
          Y = ON_yaxis;
      } else if (bTinyX && (!bTinyY || !bTinyZ)) {
          X = ON_yaxis;
          Y = ON_zaxis;
      } else if (bTinyY && (!bTinyZ || !bTinyX)) {
          X = ON_zaxis;
          Y = ON_xaxis;
      } else {
          X = v;
          X.Unitize();
          Y.PerpendicularTo(X);
          if (bTinyX && bTinyY && bTinyZ) {
              rc = false;
              if (X.IsZero()) {
                  X = ON_xaxis;
                  Y = ON_yaxis;
              }
          }
      }
      plane.CreateFromFrame(from, X, Y);
      return rc;
  }
  
  double ON_Line::MinimumDistanceTo(const ON_3dPoint &P) const {
      double d, t;
      if (ClosestPointTo(P, &t)) {
          if (t < 0.0)
              t = 0.0;
          else if (t > 1.0)
              t = 1.0;
          d = PointAt(t).DistanceTo(P);
      } else {
          // degenerate line
          d = from.DistanceTo(P);
          t = to.DistanceTo(P);
          if (t < d)
              d = t;
      }
      return d;
  }
  
  double ON_Line::MinimumDistanceTo(const ON_Line &L) const {
      ON_3dPoint A, B;
      double a, b, t, x, d;
      bool bCheckA, bCheckB;
  
      bool bGoodX = ON_Intersect(*this, L, &a, &b);
  
      bCheckA = true;
      if (a < 0.0)
          a = 0.0;
      else if (a > 1.0)
          a = 1.0;
      else
          bCheckA = !bGoodX;
      bCheckB = true;
      if (b < 0.0)
          b = 0.0;
      else if (b > 1.0)
          b = 1.0;
      else
          bCheckB = !bGoodX;
  
      A = PointAt(a);
      B = L.PointAt(b);
      d = A.DistanceTo(B);
  
      if (bCheckA) {
          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);
          if (x < d)
              d = x;
      }
  
      if (bCheckB) {
          ClosestPointTo(B, &t);
          if (t < 0.0)
              t = 0.0;
          else if (t > 1.0)
              t = 1.0;
          x = PointAt(t).DistanceTo(B);
          if (x < d)
              d = x;
      }
  
      return d;
  }
  
  double ON_Line::MaximumDistanceTo(const ON_3dPoint &P) const {
      double a, b;
      a = from.DistanceTo(P);
      b = to.DistanceTo(P);
      return ((a < b) ? b : a);
  }
  
  double ON_Line::MaximumDistanceTo(const ON_Line &L) const {
      double a, b;
      a = MaximumDistanceTo(L.from);
      b = MaximumDistanceTo(L.to);
      return ((a < b) ? b : a);
  }
  
  bool ON_Line::IsFartherThan(double d, const ON_3dPoint &P) const {
      if (P.x > to.x + d && P.x > from.x + d) {
          return true;
      }
      if (P.x < to.x - d && P.x < from.x - d) {
          return true;
      }
      if (P.y > to.y + d && P.y > from.y + d) {
          return true;
      }
      if (P.y < to.y - d && P.y < from.y - d) {
          return true;
      }
      if (P.z > to.z + d && P.z > from.z + d) {
          return true;
      }
      if (P.z < to.z - d && P.z < from.z - d) {
          return true;
      }
      return (MinimumDistanceTo(P) > d);
  }
  
  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);
  }
  