opennurbs_line.cpp
Go to the documentation of this file.
00001 /* $NoKeywords: $ */
00002 /*
00003 //
00004 // Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
00005 // OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
00006 // McNeel & Associates.
00007 //
00008 // THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
00009 // ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
00010 // MERCHANTABILITY ARE HEREBY DISCLAIMED.
00011 //                              
00012 // For complete openNURBS copyright information see <http://www.opennurbs.org>.
00013 //
00015 */
00016 
00017 #include "pcl/surface/3rdparty/opennurbs/opennurbs.h"
00018 
00019 ON_Line::ON_Line()
00020 {
00021   from.Zero();
00022   to.Zero();
00023 }
00024 
00025 ON_Line::ON_Line( const ON_3dPoint& from_pt, const ON_3dPoint& to_pt )
00026 : from(from_pt), to(to_pt)
00027 {}
00028 
00029 ON_Line::~ON_Line()
00030 {
00031 }
00032 
00033 ON_3dPoint& ON_Line::operator[](int i)
00034 {
00035   return (i<=0) ? from : to;
00036 }
00037 
00038 const ON_3dPoint& ON_Line::operator[](int i) const
00039 {
00040   return (i<=0) ? from : to;
00041 }
00042 
00043 bool ON_Line::Create( const ON_3dPoint& from_pt, const ON_3dPoint& to_pt )
00044 {
00045   from = from_pt;
00046   to = to_pt;
00047   return IsValid();
00048 }
00049 
00050 
00051 bool ON_Line::IsValid() const
00052 {
00053   return (from != to && from.IsValid() && to.IsValid());
00054 }
00055 
00056 double ON_Line::Length() const
00057 {
00058   return from.DistanceTo(to);
00059 }
00060 
00061 ON_3dVector ON_Line::Direction() const
00062 {
00063   return (to-from);
00064 }
00065 
00066 ON_3dVector ON_Line::Tangent() const
00067 {
00068   ON_3dVector V = Direction();
00069   V.Unitize();
00070   return V;
00071 }
00072 
00073 ON_3dPoint ON_Line::PointAt( double t ) const
00074 {
00075   // 26 Feb 2003 Dale Lear
00076   //     Changed 
00077   //          return (1-t)*from + t*to;
00078   //     to the following so that axis aligned lines will
00079   //     return exact answers for large values of t.  
00080   //     See RR 9683.
00081   const double s = 1.0-t;
00082   return  ON_3dPoint( (from.x == to.x) ? from.x : s*from.x + t*to.x,
00083                       (from.y == to.y) ? from.y : s*from.y + t*to.y,
00084                       (from.z == to.z) ? from.z : s*from.z + t*to.z );
00085 }
00086 
00087 void ON_Line::Reverse()
00088 {
00089   ON_3dPoint tmp = from;
00090   from = to;
00091   to = tmp;
00092 }
00093 
00094 bool ON_Line::ClosestPointTo( const ON_3dPoint& point, double *t ) const
00095 {
00096   bool rc = false;
00097   if ( t ) {
00098     const ON_3dVector D = Direction();
00099     const double DoD = D.LengthSquared();
00100     if ( DoD > 0.0 ) {
00101       if ( point.DistanceTo(from) <= point.DistanceTo(to) ) {
00102         *t = ((point - from)*D)/DoD;
00103       }
00104       else {
00105         *t = 1.0 + ((point - to)*D)/DoD;
00106       }
00107       rc = true;
00108     }
00109     else {
00110       *t = 0.0;
00111     }
00112   }
00113   return rc;
00114 }
00115 
00116 ON_3dPoint ON_Line::ClosestPointTo( const ON_3dPoint& point ) const
00117 {
00118   double t;
00119   ClosestPointTo( point, &t );
00120   return PointAt(t);
00121 }
00122 
00123 double ON_Line::DistanceTo( ON_3dPoint test_point ) const
00124 {
00125   return test_point.DistanceTo(ClosestPointTo(test_point));
00126 }
00127 
00128 
00129 bool ON_Line::Transform( const ON_Xform& tr )
00130 {
00131   from = tr*from;
00132   to = tr*to;
00133   // 5 June 2003 Dale Lear RR 10493
00134   //    Always return true.
00135   //return (from != to) ? true : false;
00136   return true;
00137 }
00138 
00139 // rotate line about a point and axis
00140 bool ON_Line::Rotate(
00141       double sin_angle,                  // sin(angle)
00142       double cos_angle,                  // cos(angle)
00143       const ON_3dVector& axis,  // axis of rotation
00144       const ON_3dPoint& center  // center of rotation
00145       )
00146 {
00147   ON_Xform rot;
00148   rot.Rotation( sin_angle, cos_angle, axis, center );
00149   const bool bFixP0 = (from==center);
00150   const bool bFixP1 = (to==center);
00151   const bool rc = Transform( rot );
00152   if ( bFixP0 )
00153     from = center;
00154   if ( bFixP1 )
00155     to = center;
00156   return rc;
00157 }
00158 
00159 bool ON_Line::Rotate(
00160       double a,                 // angle in radians
00161       const ON_3dVector& axis, // axis of rotation
00162       const ON_3dPoint& center // center of rotation
00163       )
00164 {
00165   return Rotate( sin(a), cos(a), axis, center );
00166 }
00167 
00168 bool ON_Line::Translate(
00169       const ON_3dVector& delta
00170       )
00171 {
00172   ON_Xform tr;
00173   tr.Translation( delta );
00174   return Transform( tr );
00175 }
00176 
00177 int ON_ArePointsOnLine( // returns 0=no, 1 = yes, 2 = pointset is (to tolerance) a single point on the line
00178         int dim,     // 2 or 3
00179         int is_rat,
00180         int count, 
00181         int stride, const double* point,
00182         const ON_BoundingBox& bbox, // if needed, use ON_GetBoundingBox(dim,is_rat,count,stride,point)
00183         const ON_Line& line,  // line to test
00184         double tolerance
00185         )
00186 {
00187   double w;
00188   int i, j, k;
00189 
00190   if ( count < 1 )
00191     return 0;
00192 
00193   if ( !line.IsValid() )
00194   {
00195     ON_ERROR("line parameter not valid");
00196     return 0;
00197   }
00198   if ( !bbox.IsValid() )
00199   {
00200     ON_ERROR("bbox parameter not valid");
00201     return 0;
00202   }
00203   if ( !ON_IsValid(tolerance) || tolerance < 0.0 )
00204   {
00205     ON_ERROR("tolerance parameter not valid");
00206     return 0;
00207   }
00208   if ( dim < 2 || dim > 3 )
00209   {
00210     ON_ERROR("dim parameter not valid");
00211     return 0;
00212   }
00213   if ( 0 == point )
00214   {
00215     ON_ERROR("point parameter not valid");
00216     return 0;
00217   }
00218   if ( stride < (is_rat?(dim+1):dim) )
00219   {
00220     ON_ERROR("stride parameter not valid");
00221     return 0;
00222   }
00223 
00224   int rc = 0;
00225 
00226   if ( tolerance == 0.0 ) {
00227     tolerance = bbox.Tolerance();
00228   }
00229 
00230   ON_3dPoint Q;
00231 
00232   // test bounding box to quickly detect the common coordinate axis cases
00233   rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
00234   for ( i = 0; rc && i < 2; i++ ) {
00235     Q.x = bbox[i].x;
00236     for ( j = 0; rc && j < 2; j++) {
00237       Q.y = bbox[j].y;
00238       for ( k = 0; rc && k < 2; k++) {
00239         Q.z = bbox[k].z;
00240         if ( Q.DistanceTo( line.ClosestPointTo( Q ) ) > tolerance )
00241           rc = 0;
00242       }
00243     }
00244   }
00245 
00246   if ( !rc ) {
00247     // test points one by one
00248     Q.Zero();
00249     rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
00250     if ( is_rat ) {
00251       for ( i = 0; i < count; i++ ) {
00252         w = point[dim];
00253         if ( w == 0.0 ) {
00254           ON_ERROR("rational point has zero weight");
00255           return 0;
00256         }
00257         ON_ArrayScale( dim, 1.0/w, point, &Q.x );
00258         if ( Q.DistanceTo( line.ClosestPointTo( Q ) ) > tolerance ) {
00259           rc = 0;
00260           break;
00261         }
00262         point += stride;
00263       }
00264     }
00265     else {
00266       for ( i = 0; i < count; i++ ) {
00267         memcpy( &Q.x, point, dim*sizeof(Q.x) );
00268         if ( Q.DistanceTo( line.ClosestPointTo( Q ) ) > tolerance ) {
00269           rc = 0;
00270           break;
00271         }
00272         point += stride;
00273       }
00274     }
00275   }
00276 
00277   return rc;
00278 }
00279 
00280 
00281 ON_BoundingBox ON_Line::BoundingBox() const
00282 {
00283   ON_BoundingBox bbox;
00284   bbox.Set( 3, false, 2, 3, &from.x, false );
00285   return bbox;
00286 }
00287 
00288 bool ON_Line::GetBoundingBox(
00289        ON_BoundingBox& bbox,
00290        int bGrowBox
00291        ) const
00292 {
00293   bbox.Set( 3, false, 2, 3, &from.x, bGrowBox );
00294   return true;
00295 }
00296 
00297 bool ON_Line::InPlane( ON_Plane& plane, double tolerance ) const
00298 {
00299   const ON_3dVector v = to-from;
00300   const bool bTinyX = fabs(v.x) <= tolerance;
00301   const bool bTinyY = fabs(v.y) <= tolerance;
00302   const bool bTinyZ = fabs(v.z) <= tolerance;
00303   bool rc = true;
00304   ON_3dVector X;
00305   ON_3dVector Y;
00306   if ( bTinyZ && ( !bTinyX || !bTinyY ) )
00307   {
00308     X = ON_xaxis;
00309     Y = ON_yaxis;
00310   }
00311   else if ( bTinyX && ( !bTinyY || !bTinyZ ) )
00312   {
00313     X = ON_yaxis;
00314     Y = ON_zaxis;
00315   }
00316   else if ( bTinyY && ( !bTinyZ || !bTinyX ) )
00317   {
00318     X = ON_zaxis;
00319     Y = ON_xaxis;
00320   }
00321   else
00322   {
00323     X = v;
00324     X.Unitize();
00325     Y.PerpendicularTo(X);
00326     if ( bTinyX && bTinyY && bTinyZ )
00327     {
00328       rc = false;
00329       if ( X.IsZero() )
00330       {
00331         X = ON_xaxis;
00332         Y = ON_yaxis;
00333       }
00334     }
00335   }
00336   plane.CreateFromFrame( from, X, Y );
00337   return rc;
00338 }
00339 
00340 double ON_Line::MinimumDistanceTo( const ON_3dPoint& P ) const
00341 {
00342   double d, t;
00343   if (ClosestPointTo(P,&t))
00344   {
00345     if ( t < 0.0 ) t = 0.0; else if (t > 1.0) t = 1.0;
00346     d = PointAt(t).DistanceTo(P);
00347   }
00348   else
00349   {
00350     // degenerate line
00351     d = from.DistanceTo(P);
00352     t = to.DistanceTo(P);
00353     if ( t < d )
00354       d = t;
00355   }
00356   return d;
00357 }
00358 
00359 double ON_Line::MinimumDistanceTo( const ON_Line& L ) const
00360 {
00361   ON_3dPoint A, B;
00362   double a, b, t, x, d;
00363   bool bCheckA, bCheckB;
00364 
00365   bool bGoodX = ON_Intersect(*this,L,&a,&b);
00366 
00367   bCheckA = true;
00368   if ( a < 0.0) a = 0.0; else if (a > 1.0) a = 1.0; else bCheckA=!bGoodX;
00369   bCheckB = true;
00370   if ( b < 0.0) b = 0.0; else if (b > 1.0) b = 1.0; else bCheckB=!bGoodX;
00371 
00372   A = PointAt(a);
00373   B = L.PointAt(b);
00374   d = A.DistanceTo(B);
00375 
00376   if ( bCheckA )
00377   {
00378     L.ClosestPointTo(A,&t);
00379     if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
00380     x = L.PointAt(t).DistanceTo(A);
00381     if ( x < d )
00382       d = x;
00383   }
00384 
00385   if ( bCheckB )
00386   {
00387     ClosestPointTo(B,&t);
00388     if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
00389     x = PointAt(t).DistanceTo(B);
00390     if (x < d )
00391       d = x;
00392   }
00393  
00394   return d;
00395 }
00396 
00397 double ON_Line::MaximumDistanceTo( const ON_3dPoint& P ) const
00398 {
00399   double a, b;
00400   a = from.DistanceTo(P);
00401   b = to.DistanceTo(P);
00402   return ((a<b)?b:a);
00403 }
00404 
00405 double ON_Line::MaximumDistanceTo( const ON_Line& L ) const
00406 {
00407   double a, b;
00408   a = MaximumDistanceTo(L.from);
00409   b = MaximumDistanceTo(L.to);
00410   return ((a<b)?b:a);
00411 }
00412 
00413 bool ON_Line::IsFartherThan( double d, const ON_3dPoint& P ) const
00414 {
00415   if ( P.x > to.x+d && P.x > from.x+d )
00416   {
00417     return true;
00418   }
00419   if ( P.x < to.x-d && P.x < from.x-d )
00420   {
00421     return true;
00422   }
00423   if ( P.y > to.y+d && P.y > from.y+d )
00424   {
00425     return true;
00426   }
00427   if ( P.y < to.y-d && P.y < from.y-d )
00428   {
00429     return true;
00430   }
00431   if ( P.z > to.z+d && P.z > from.z+d )
00432   {
00433     return true;
00434   }
00435   if ( P.z < to.z-d && P.z < from.z-d )
00436   {
00437     return true;
00438   }
00439   return (MinimumDistanceTo(P) > d);
00440 }
00441 
00442 
00443 bool ON_Line::IsFartherThan( double d, const ON_Line& L ) const
00444 {
00445   ON_3dPoint A, B;
00446   double a, b, t, x;
00447   bool bCheckA, bCheckB;
00448 
00449   a = from.x; if (to.x < a) {b=a; a = to.x;} else b = to.x;
00450   if ( b+d < L.from.x && b+d < L.to.x )
00451     return true;
00452   if ( a-d > L.from.x && a-d > L.to.x )
00453     return true;
00454 
00455   a = from.y; if (to.y < a) {b=a; a = to.y;} else b = to.y;
00456   if ( b+d < L.from.y && b+d < L.to.y )
00457     return true;
00458   if ( a-d > L.from.y && a-d > L.to.y )
00459     return true;
00460 
00461   a = from.z; if (to.z < a) {b=a; a = to.z;} else b = to.z;
00462   if ( b+d < L.from.z && b+d < L.to.z )
00463     return true;
00464   if ( a-d > L.from.z && a-d > L.to.z )
00465     return true;
00466 
00467   if ( !ON_Intersect(*this,L,&a,&b) )
00468   {
00469     // lines are parallel or anti parallel
00470     if ( Direction()*L.Direction() >= 0.0 )
00471     {
00472       // lines are parallel
00473       a = 0.0;
00474       L.ClosestPointTo(from,&b);
00475       // If ( b >= 0.0), then this->from and L(b) are a pair of closest points.
00476       if ( b < 0.0 )
00477       {
00478         // Othersise L.from and this(a) are a pair of closest points.
00479         b = 0.0;
00480         ClosestPointTo(L.from,&a);
00481       }
00482     }
00483     else
00484     {
00485       // lines are anti parallel
00486       a = 1.0;
00487       L.ClosestPointTo(to,&b);
00488       // If ( b >= 0.0), then this->to and L(b) are a pair of closest points.
00489       if ( b < 0.0 )
00490       {
00491         // Othersise L.to and this(a) are a pair of closest points.
00492         b = 0.0;
00493         ClosestPointTo(L.from,&a);
00494       }
00495     }
00496   }
00497 
00498   A = PointAt(a);
00499   B = L.PointAt(b);
00500   x = A.DistanceTo(B);
00501   if (x > d)
00502     return true;
00503 
00504   bCheckA = true;
00505   if ( a < 0.0) a = 0.0; else if (a > 1.0) a = 1.0; else bCheckA=false;
00506   if (bCheckA )
00507   {
00508     A = PointAt(a);
00509     L.ClosestPointTo(A,&t);
00510     if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
00511     x = L.PointAt(t).DistanceTo(A);
00512   }
00513 
00514   bCheckB = true;
00515   if ( b < 0.0) b = 0.0; else if (b > 1.0) b = 1.0; else bCheckB=false;
00516   if ( bCheckB )
00517   {
00518     B = L.PointAt(b);
00519     ClosestPointTo(B,&t);
00520     if (t<0.0) t = 0.0; else if (t > 1.0) t = 1.0;
00521     t = PointAt(t).DistanceTo(B);
00522     if ( bCheckA )
00523     {
00524       if ( t<x ) x = t;
00525     }
00526     else
00527     {
00528       x = t;
00529     }
00530   }
00531  
00532   return (x > d);
00533 }
00534 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:01