opennurbs_curve.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_VIRTUAL_OBJECT_IMPLEMENT(ON_Curve,ON_Geometry,"4ED7D4D7-E947-11d3-BFE5-0010830122F0");
00020 
00021 ON_Curve::ON_Curve()
00022 : ON_Geometry()
00023 {}
00024 
00025 ON_Curve::ON_Curve(const ON_Curve& src)
00026 : ON_Geometry(src)
00027 {}
00028 
00029 ON_Curve& ON_Curve::operator=(const ON_Curve& src)
00030 {
00031   if ( this != &src ) {
00032     DestroyCurveTree();
00033     ON_Geometry::operator=(src);
00034   }
00035   return *this;
00036 }
00037 
00038 ON_Curve::~ON_Curve()
00039 {
00040   // Do not call the (virtual) DestroyRuntimeCache or 
00041   // DestroyCurveTree (which calls DestroyRuntimeCache()
00042   // because it opens the potential for crashes in a
00043   // "dirty" destructors of classes derived from ON_Curve
00044   // that to not use DestroyRuntimeCache() in their
00045   // destructors and to not set deleted pointers to zero.
00046 }
00047 
00048 unsigned int ON_Curve::SizeOf() const
00049 {
00050   unsigned int sz = ON_Geometry::SizeOf();
00051   sz += (sizeof(*this) - sizeof(ON_Geometry));
00052   // Currently, the size of m_ctree is not included
00053   // because this is cached runtime information.
00054   // Applications that care about object size are 
00055   // typically storing "inactive" objects for potential
00056   // future use and should call DestroyRuntimeCache(true)
00057   // to remove any runtime cache information.
00058   return sz;
00059 }
00060 
00061 
00062 ON_Curve* ON_Curve::DuplicateCurve() const
00063 {
00064   // ON_CurveProxy overrides this virtual function.
00065   return Duplicate();
00066 }
00067 
00068 
00069 ON::object_type ON_Curve::ObjectType() const
00070 {
00071   return ON::curve_object;
00072 }
00073 
00074 
00075 ON_BOOL32 ON_Curve::GetDomain(double* s0,double* s1) const
00076 {
00077   ON_BOOL32 rc = false;
00078   ON_Interval d = Domain();
00079   if ( d.IsIncreasing() ) {
00080     if(s0) *s0 = d.Min();
00081     if (s1) *s1 = d.Max();
00082     rc = true;
00083   }
00084   return rc;
00085 }
00086 
00087 void ON_Curve::DestroyCurveTree()
00088 {
00089   DestroyRuntimeCache(true);
00090 }
00091 
00092 bool ON_Curve::GetTightBoundingBox( 
00093                 ON_BoundingBox& tight_bbox, 
00094     int bGrowBox,
00095                 const ON_Xform* xform
00096     ) const
00097 {
00098   if ( bGrowBox && !tight_bbox.IsValid() )
00099   {
00100     bGrowBox = false;
00101   }
00102 
00103   if ( !bGrowBox )
00104   {
00105     tight_bbox.Destroy();
00106   }
00107 
00108   // In general, putting start and end point in the box lets me avoid
00109   // testing lots of nodes.
00110   ON_3dPoint P = PointAtStart();
00111   if ( xform )
00112     P = (*xform)*P;
00113   tight_bbox.Set( P, bGrowBox );
00114   bGrowBox = true;
00115 
00116   P = PointAtEnd();
00117   if ( xform )
00118     P = (*xform)*P;
00119   tight_bbox.Set( P, bGrowBox );
00120 
00121   ON_BoundingBox curve_bbox = BoundingBox();
00122   if ( ON_WorldBBoxIsInTightBBox( tight_bbox, curve_bbox, xform ) )
00123   {
00124     // Curve is inside tight_bbox
00125     return true;
00126   }
00127 
00128   ON_NurbsCurve N;
00129   if ( 0 == GetNurbForm(N) )
00130     return false;
00131   if ( N.m_order < 2 || N.m_cv_count < N.m_order )
00132     return false;
00133 
00134   ON_BezierCurve B;
00135   for ( int span_index = 0; span_index <= N.m_cv_count - N.m_order; span_index++ )
00136   {
00137     if ( !(N.m_knot[span_index + N.m_order-2] < N.m_knot[span_index + N.m_order-1]) )
00138       continue;
00139     if ( !N.ConvertSpanToBezier( span_index, B ) )
00140       continue;
00141     if ( !B.GetTightBoundingBox(tight_bbox,bGrowBox,xform) )
00142       continue;
00143     bGrowBox = true;
00144   }
00145 
00146   return (0!=bGrowBox);
00147 }
00148 
00149 bool ON_Curve::SetDomain( ON_Interval domain )
00150 {
00151   return ( domain.IsIncreasing() && SetDomain( domain[0], domain[1] )) ? true : false;
00152 }
00153 
00154 
00155 ON_BOOL32 ON_Curve::SetDomain( double, double )
00156 {
00157   // this virtual function is overridden by curves that can change their domain.
00158   return false;
00159 }
00160 
00161 ON_BOOL32 ON_Curve::ChangeClosedCurveSeam( double t )
00162 {
00163   // this virtual function is overridden by curves that can be closed
00164   return false;
00165 }
00166 
00167 //virtual
00168 bool ON_Curve::ChangeDimension( int desired_dimension )
00169 {
00170   return (desired_dimension > 0 && desired_dimension == Dimension() );
00171 }
00172 
00173 
00174 //virtual
00175 ON_BOOL32 ON_Curve::GetSpanVectorIndex(
00176        double t,                // [IN] t = evaluation parameter
00177        int side,                // [IN] side 0 = default, -1 = from below, +1 = from above
00178        int* span_vector_i,      // [OUT] span vector index
00179        ON_Interval* span_domain // [OUT] domain of the span containing "t"
00180        ) const
00181 {
00182   ON_BOOL32 rc = false;
00183   int i;
00184   int span_count = SpanCount();
00185   if ( span_count > 0 ) {
00186     double* span_vector = (double*)onmalloc((span_count+1)*sizeof(span_vector[0]));
00187     rc = GetSpanVector( span_vector );
00188     if (rc) {
00189       i = ON_NurbsSpanIndex( 2, span_count+1, span_vector, t, side, 0 );
00190       if ( i >= 0 && i <= span_count ) {
00191         if ( span_vector_i )
00192           *span_vector_i = i;
00193         if ( span_domain )
00194           span_domain->Set( span_vector[i], span_vector[i+1] );
00195       }
00196       else
00197         rc = false;
00198     }
00199     onfree(span_vector);
00200   }
00201   return rc;
00202 }
00203 
00204 
00205 ON_BOOL32 ON_Curve::GetParameterTolerance( // returns tminus < tplus: parameters tminus <= s <= tplus
00206        double t,       // t = parameter in domain
00207        double* tminus, // tminus
00208        double* tplus   // tplus
00209        ) const
00210 {
00211   ON_BOOL32 rc = false;
00212   ON_Interval d = Domain();
00213   if ( d.IsIncreasing() )
00214     rc = ON_GetParameterTolerance( d[0], d[1], t, tminus, tplus );
00215   return rc;
00216 }
00217 
00218 int ON_Curve::IsPolyline(
00219       ON_SimpleArray<ON_3dPoint>* pline_points, // default = NULL
00220       ON_SimpleArray<double>* pline_t // default = NULL
00221       ) const
00222 {
00223   // virtual function that is overridden
00224   return 0;
00225 }
00226 
00227 
00228 ON_BOOL32 ON_Curve::IsLinear( double tolerance ) const
00229 {
00230   ON_BOOL32 rc = false;
00231   if ( Dimension() == 2 || Dimension() == 3 ) {
00232     const int span_count = SpanCount();
00233     const int span_degree = Degree();
00234     if ( span_count > 0 ) {
00235       ON_SimpleArray<double> s(span_count+1);
00236       s.SetCount(span_count+1);
00237       if ( GetSpanVector( s.Array() ) ) {
00238         if ( tolerance == 0.0 )
00239           tolerance = ON_ZERO_TOLERANCE;
00240         ON_Line line( PointAtStart(), PointAtEnd() );
00241         if ( line.Length() > tolerance ) {
00242           double t, t0, d, delta;
00243           ON_Interval sp;
00244           int n, i, span_index;
00245           rc = true;
00246           t0 = 0;              //  Domain()[0];
00247           ON_3dPoint P;
00248 
00249           for ( span_index = 0; span_index < span_count; span_index++ ) {
00250             sp.Set( s[span_index], s[span_index+1] );
00251             n = 2*span_degree+1;
00252             delta = 1.0/n;
00253             for ( i = (span_index)?0:1; i < n; i++ ) {
00254               P = PointAt( sp.ParameterAt(i*delta) );
00255               if ( !line.ClosestPointTo( P, &t ) )
00256                 rc = false;
00257               else if ( t < t0 )
00258                 rc = false;
00259                                                         else if (t > 1.0 + ON_SQRT_EPSILON)
00260                                                                 rc = false;
00261               d = P.DistanceTo( line.PointAt(t) );
00262               if ( d > tolerance )
00263                 rc = false;
00264               t0 = t;
00265             }
00266           }
00267         }
00268       }
00269     }
00270   }
00271   return rc;
00272 }
00273 
00274 bool ON_Curve::IsEllipse(
00275     const ON_Plane* plane,
00276     ON_Ellipse* ellipse,
00277     double tolerance
00278     ) const
00279 {
00280   // virtual function
00281   ON_Arc arc;
00282   bool rc = IsArc(plane,&arc,tolerance)?true:false;
00283   if (rc && ellipse)
00284   {
00285     ellipse->plane = arc.plane;
00286     ellipse->radius[0] = arc.radius;
00287     ellipse->radius[1] = arc.radius;
00288   }
00289   return rc;
00290 }
00291 
00292 bool ON_Curve::IsArcAt( 
00293   double t, 
00294   const ON_Plane* plane,
00295   ON_Arc* arc,
00296   double tolerance,
00297   double* t0, 
00298   double* t1
00299   ) const
00300 {
00301   double k, k0, k1;
00302   int hint;
00303   if ( !GetDomain(&k0,&k1) )
00304     return false;
00305   if ( 0 != t0 )
00306     *t0 = k0;
00307   if ( 0 != t1 )
00308     *t1 = k1;
00309   if ( !ON_IsValid(t) )
00310     return false;
00311   if ( ! (t <= k1) )
00312     return false;
00313 
00314   if ( IsArc(plane,arc,tolerance) )
00315     return true; // entire curve is an arc
00316 
00317   // check sub-segments
00318   hint = 0;
00319   for ( k = k0; k0 <= t && GetNextDiscontinuity(ON::G2_locus_continuous, k0, k1, &k, &hint); k0 = k )
00320   {
00321     if ( !(k > k0) )
00322       break; // sanity check to prevent infinite loops
00323     if( t <= k )
00324     {
00325       if ( 0 != t0 )
00326         *t0 = k0;
00327       if ( 0 != t1 )
00328         *t1 = k1;
00329       ON_CurveProxy subcrv(this,ON_Interval(k0,k));
00330       if ( subcrv.IsArc(plane,arc,tolerance) )
00331         return true;
00332 
00333       // NOTE WELL: 
00334       //   When t == k, we need to check the next segment as well
00335       //   (happens when t is the parameter between a line and arc segment.)
00336       //   The "k0 <= t" test is in the for() condition will
00337       //   terminate the loop when t < k
00338     }
00339   }
00340 
00341   return false;
00342 }
00343 
00344 
00345 ON_BOOL32 ON_Curve::IsArc( const ON_Plane* plane, ON_Arc* arc, double tolerance ) const
00346 {
00347   ON_BOOL32 rc = false;
00348   double c0, c, t, delta;
00349   int n, i, span_index;
00350   ON_Plane pln;
00351   ON_Arc a;
00352   ON_3dPoint P, C;
00353 
00354   if ( !plane ) {
00355     if ( !IsPlanar(&pln,tolerance) )
00356       return false;
00357     plane = &pln;
00358   }
00359   if ( !arc )
00360     arc = &a;
00361   const int span_count = SpanCount();
00362   const int span_degree = Degree();
00363   if ( span_count < 1 )
00364     return false;
00365   ON_SimpleArray<double> d(span_count+1);
00366   d.SetCount(span_count+1);
00367   if ( !GetSpanVector(d.Array()) )
00368     return false;
00369 
00370   const ON_BOOL32 bIsClosed = IsClosed();
00371 
00372   ON_3dPoint P0 = PointAt( d[0] );
00373   t = bIsClosed ? 0.5*d[0] + 0.5*d[span_count] : d[span_count];
00374   ON_3dPoint P1 = PointAt( 0.5*d[0] + 0.5*t );
00375   ON_3dPoint P2 = PointAt( t );
00376 
00377   if ( !arc->Create(P0,P1,P2) )
00378     return false;
00379 
00380   if ( bIsClosed )
00381     arc->SetAngleRadians(2.0*ON_PI);
00382 
00383   ON_Interval arc_domain = arc->Domain();
00384   ON_3dPoint A0 = arc->PointAt(arc_domain[0]);
00385   ON_3dPoint A1 = arc->PointAt(arc_domain[1]);
00386   ON_3dPoint C0 = PointAtStart();
00387   ON_3dPoint C1 = PointAtEnd();
00388   if (    false == ON_PointsAreCoincident(3,0,&A0.x,&C0.x) 
00389        || false == ON_PointsAreCoincident(3,0,&A1.x,&C1.x) 
00390      )
00391   {
00392     return false;
00393   }
00394 
00395 
00396   if ( tolerance == 0.0 )
00397     tolerance = ON_ZERO_TOLERANCE;
00398   rc = true;
00399   c0 = 0.0;
00400   for ( span_index = 0; rc && span_index < span_count; span_index++ ) {
00401     n = 2*span_degree+1;
00402     if ( n < 4 )
00403       n = 4;
00404     delta = 1.0/n;
00405     for ( i = 0; i < n; i++ ) {
00406       t = i*delta;
00407       P = PointAt( (1.0-t)*d[span_index] + t*d[span_index+1] );
00408       if ( !arc->ClosestPointTo(P,&c) ) {
00409         rc = false;
00410         break;        
00411       }
00412       if ( c < c0 ) {
00413         rc = false;
00414         break;
00415       }
00416       C = arc->PointAt(c);        
00417       if ( C.DistanceTo(P) > tolerance ) {
00418         rc = 0;
00419         break;
00420       }
00421       c0 = c;
00422     }
00423   }
00424 
00425   return rc;
00426 }
00427 
00428 ON_BOOL32 ON_Curve::IsPlanar( ON_Plane* plane, double tolerance ) const
00429 {
00430   ON_BOOL32 rc = false;
00431   const int dim = Dimension();
00432   if ( dim == 2 ) 
00433   {
00434     // all 2d curves use this code to set the plane
00435     // so that there is consistent behavior.
00436     rc = true;
00437     if ( plane )
00438     {
00439       *plane = ON_xy_plane;
00440       //plane->CreateFromFrame( PointAtStart(), ON_xaxis, ON_yaxis );
00441     }
00442   }
00443   else if ( IsLinear(tolerance) )
00444   {
00445     rc = true;
00446     if ( plane )
00447     {
00448       ON_Line line( PointAtStart(), PointAtEnd() );
00449       if ( !line.InPlane( *plane, tolerance ) )
00450         line.InPlane( *plane, 0.0 );
00451     }
00452   }
00453   else if ( dim == 3 ) 
00454   {
00455     const int span_count = SpanCount();
00456     if ( span_count < 1 )
00457       return false;
00458     const int span_degree = Degree();
00459     if ( span_degree < 1 )
00460       return false;
00461     ON_SimpleArray<double> s(span_count+1);
00462     s.SetCount(span_count+1);
00463     if ( !GetSpanVector(s.Array()) )
00464       return false;
00465     ON_Interval d = Domain();
00466 
00467     // use initial point, tangent, and evaluated spans to guess a plane
00468     ON_3dPoint pt = PointAt(d.ParameterAt(0.0));
00469     ON_3dVector x = TangentAt(d.ParameterAt(0.0));
00470     if ( x.Length() < 0.95 ) {
00471       return false;
00472     }
00473     int n = (span_degree > 1) ? span_degree+1 : span_degree;
00474     double delta = 1.0/n;
00475     int i, span_index, hint = 0;
00476     ON_3dPoint q;
00477     ON_3dVector y;
00478     ON_BOOL32 bNeedY = true;
00479     for ( span_index = 0; span_index < span_count && bNeedY; span_index++ ) {
00480       d.Set(s[span_index],s[span_index+1]);
00481       for ( i = span_index ? 0 : 1; i < n && bNeedY; i++ ) {
00482         if ( !EvPoint( d.ParameterAt(i*delta), q, 0, &hint ) )
00483           return false;
00484         y = q-pt;
00485         y = y - (y*x)*x;
00486         bNeedY = ( y.Length()  <= 1.0e-6 );
00487       }
00488     }
00489     if ( bNeedY )
00490       y.PerpendicularTo(x);
00491     ON_Plane pln( pt, x, y );
00492     if ( plane )
00493       *plane = pln;
00494 
00495     // test
00496     rc = true;
00497     n = 2*span_degree + 1;
00498     delta = 1.0/n;
00499     double h = pln.plane_equation.ValueAt(PointAtEnd());
00500     if ( fabs(h) > tolerance )
00501       rc = false;
00502     hint = 0;
00503     for ( span_index = 0; rc && span_index < span_count; span_index++ ) {
00504       d.Set(s[span_index],s[span_index+1]);
00505       for ( i = 0; rc && i < n; i++ ) {
00506         if ( !EvPoint( d.ParameterAt(i*delta), q, 0, &hint ) )
00507           rc = false;
00508         else {
00509           h = pln.plane_equation.ValueAt(q);
00510           if ( fabs(h) > tolerance )
00511             rc = false;
00512         }
00513       }
00514     }
00515   }
00516   return rc;
00517 }
00518 
00519 ON_BOOL32 ON_Curve::IsClosed() const
00520 {
00521   bool rc = false;
00522   double *a, *b, *c, *p, w[12];
00523   const int dim = Dimension();
00524   a = 0;
00525   if ( dim > 1 ) 
00526   {
00527     ON_Interval d = Domain();
00528     a = (dim>3) ? (double*)onmalloc(dim*4*sizeof(*a)) : w;
00529     b = a+dim;
00530     c = b+dim;
00531     p = c+dim;
00532     if (    Evaluate( d.ParameterAt(0.0), 0, dim, a, 1 )
00533          && Evaluate( d.ParameterAt(1.0), 0, dim, p,-1 ) 
00534        )
00535     {
00536       // Note:  The point compare test should be the same
00537       //        as the one used in ON_PolyCurve::HasGap().
00538       //
00539       if ( ON_PointsAreCoincident( dim, false, a, p ) ) 
00540       {
00541         if (    Evaluate( d.ParameterAt(1.0/3.0), 0, dim, b, 0 ) 
00542              && Evaluate( d.ParameterAt(2.0/3.0), 0, dim, c, 0 )
00543            )
00544         {
00545           if (    false == ON_PointsAreCoincident( dim, false, a, b ) 
00546                && false == ON_PointsAreCoincident( dim, false, p, c ) 
00547                && false == ON_PointsAreCoincident( dim, false, p, b ) 
00548                && false == ON_PointsAreCoincident( dim, false, p, c ) 
00549              )
00550           {
00551             rc = true;
00552           }
00553         }
00554       }
00555     }
00556     if ( dim > 3 && 0 != a )
00557       onfree(a);
00558   }
00559 
00560   return rc;
00561 }
00562 
00563 
00564 ON_BOOL32 ON_Curve::IsPeriodic() const
00565 {
00566   // curve types that may be periodic override this virtual function
00567   return false;
00568 }
00569 
00570 bool ON_Curve::GetNextDiscontinuity( 
00571                 ON::continuity c,
00572                 double t0,
00573                 double t1,
00574                 double* t,
00575                 int* hint,
00576                 int* dtype,
00577                 double cos_angle_tolerance,
00578                 double curvature_tolerance
00579                 ) const
00580 {
00581   // this function must be overridden by curve objects that
00582   // can have parametric discontinuities on the interior of the curve.
00583 
00584   bool rc = false;
00585 
00586   if ( dtype )
00587     *dtype = 0;
00588   
00589   if ( t0 != t1 )
00590   {
00591     bool bTestC0 = false;
00592     bool bTestD1 = false;
00593     bool bTestD2 = false;
00594     bool bTestT = false;
00595     bool bTestK = false;
00596     switch(c)
00597     {
00598     case ON::C0_locus_continuous:
00599       bTestC0 = true;
00600       break;
00601     case ON::C1_locus_continuous:
00602       bTestC0 = true;
00603       bTestD1 = true;
00604       break;
00605     case ON::C2_locus_continuous:
00606       bTestC0 = true;
00607       bTestD1 = true;
00608       bTestD2 = true;
00609       break;
00610     case ON::G1_locus_continuous:
00611       bTestC0 = true;
00612       bTestT  = true;
00613       break;
00614     case ON::G2_locus_continuous:
00615       bTestC0 = true;
00616       bTestT  = true;
00617       bTestK  = true;
00618       break;
00619     default:
00620       // other values ignored on purpose.
00621       break;
00622     }
00623 
00624     if ( bTestC0 )
00625     {
00626       // 20 March 2003 Dale Lear:
00627       //   Have to look for locus discontinuities at ends.
00628       //   Must test both ends becuase t0 > t1 is valid input.
00629       //   In particular, for ON_CurveProxy::GetNextDiscontinuity() 
00630       //   to work correctly on reversed "real" curves, the 
00631       //   t0 > t1 must work right.
00632       ON_Interval domain = Domain();
00633 
00634       if ( t0 < domain[1] && t1 >= domain[1] )
00635         t1 = domain[1];
00636       else if ( t0 > domain[0] && t1 <= domain[0] )
00637         t1 = domain[0];
00638 
00639       if ( (t0 < domain[1] && t1 >= domain[1]) || (t0 > domain[0] && t1 <= domain[0]) )
00640       {
00641         if ( IsClosed() )
00642         {
00643           if ( bTestD1 || bTestT )
00644           {
00645             // need to check locus continuity at start/end of closed curve.
00646             ON_3dPoint Pa, Pb;
00647             ON_3dVector D1a, D1b, D2a, D2b;
00648             if (    Ev2Der(domain[0],Pa,D1a,D2a,1,NULL) 
00649                  && Ev2Der(domain[1],Pb,D1b,D2b,-1,NULL) )
00650             {
00651               Pb = Pa; // IsClosed() = true means assume Pa=Pb;
00652               if ( bTestD1 )
00653               {
00654                 if ( !(D1a-D1b).IsTiny(D1b.MaximumCoordinate()*ON_SQRT_EPSILON ) )
00655                 {
00656                   if ( dtype )
00657                     *dtype = 1;
00658                   *t = t1;
00659                   rc = true;
00660                 }
00661                 else if ( bTestD2 && !(D2a-D2b).IsTiny(D2b.MaximumCoordinate()*ON_SQRT_EPSILON) )
00662                 {
00663                   if ( dtype )
00664                     *dtype = 2;
00665                   *t = t1;
00666                   rc = true;
00667                 }
00668 
00669               }
00670               else if ( bTestT )
00671               {
00672                 ON_3dVector Ta, Tb, Ka, Kb;
00673                 ON_EvCurvature( D1a, D2a, Ta, Ka );
00674                 ON_EvCurvature( D1b, D2b, Tb, Kb );
00675                 if ( Ta*Tb < cos_angle_tolerance )
00676                 {
00677                   if ( dtype )
00678                     *dtype = 1;
00679                   *t = t1;
00680                   rc = true;
00681                 }
00682                 else if ( bTestK )
00683                 {
00684                   // NOTE: 
00685                   //  This test must exactly match the one
00686                   //  used in ON_NurbsCurve::GetNextDiscontinuity()
00687                   if ( !ON_IsG2CurvatureContinuous( Ka, Kb,
00688                                                     cos_angle_tolerance,
00689                                                     curvature_tolerance
00690                                                     )
00691                      )
00692                   {
00693                     if ( dtype )
00694                       *dtype = 2;
00695                     *t = t1;
00696                     rc = true;
00697                   }
00698                 }
00699               }
00700             }
00701           }
00702         }
00703         else
00704         {
00705           // open curves are not locus continuous at ends.
00706           if (dtype )
00707             *dtype = 0; // locus C0 discontinuity
00708           *t = t1;
00709           rc = true;
00710         }
00711       }
00712     }
00713   }
00714 
00715   return rc;
00716 }
00717 
00718 
00719 
00720 bool ON_Curve::IsContinuous(
00721     ON::continuity desired_continuity,
00722     double t, 
00723     int* hint, // default = NULL,
00724     double point_tolerance, // default=ON_ZERO_TOLERANCE
00725     double d1_tolerance, // default==ON_ZERO_TOLERANCE
00726     double d2_tolerance, // default==ON_ZERO_TOLERANCE
00727     double cos_angle_tolerance, // default==ON_DEFAULT_ANGLE_TOLERANCE_COSINE
00728     double curvature_tolerance  // default==ON_SQRT_EPSILON
00729     ) const
00730 {
00731   ON_Interval domain = Domain();
00732   if ( !domain.IsIncreasing() )
00733   {
00734     return true;
00735   }
00736 
00737   ON_3dPoint Pm, Pp;
00738   ON_3dVector D1m, D1p, D2m, D2p, Tm, Tp, Km, Kp;
00739 
00740   bool bIsClosed = false;
00741 
00742   // 20 March 2003 Dale Lear
00743   //     I added this preable to handle the new
00744   //     locus continuity values.
00745   double t0 = t;
00746   double t1 = t;
00747   switch(desired_continuity)
00748   {
00749   case ON::C0_locus_continuous:
00750   case ON::C1_locus_continuous:
00751   case ON::C2_locus_continuous:
00752   case ON::G1_locus_continuous:
00753   case ON::G2_locus_continuous:
00754     if ( t <= domain[0] )
00755     {
00756       // By convention - see comments by ON::continuity enum.
00757       return true;
00758     }
00759     if ( t == domain[1] )
00760     {
00761       if ( !IsClosed() )
00762       {
00763         // open curves are not locus continuous at the end parameter
00764         // see comments by ON::continuity enum
00765         return false;
00766       }
00767       else 
00768       {
00769         if ( ON::C0_locus_continuous == desired_continuity )
00770         {
00771           return true;
00772         }
00773         bIsClosed = true;
00774       }
00775 
00776       t0 = domain[0];
00777       t1 = domain[1];
00778     }
00779     break;
00780 
00781   case ON::unknown_continuity:
00782   case ON::C0_continuous:
00783   case ON::C1_continuous:
00784   case ON::C2_continuous:
00785   case ON::G1_continuous:
00786   case ON::G2_continuous:
00787   case ON::Cinfinity_continuous:
00788   case ON::Gsmooth_continuous:
00789   default:
00790     // does not change pre 20 March behavior - just skips the out
00791     // of domain evaluation on parametric queries.
00792     if ( t <= domain[0] || t >= domain[1] )
00793       return true;
00794     break;
00795   }
00796 
00797   // at this point, no difference between parametric and locus tests.
00798   desired_continuity = ON::ParametricContinuity(desired_continuity);
00799 
00800 
00801   // this is slow and uses evaluation
00802   // virtual overrides on curve classes that can have multiple spans
00803   // are much faster because the avoid evaluation
00804   switch ( desired_continuity )
00805   {
00806   case ON::unknown_continuity:
00807     break;
00808 
00809   case ON::C0_continuous:  
00810     if ( !EvPoint( t1, Pm, -1, hint ) )
00811       return false;
00812     if ( !EvPoint( t0, Pp,  1, hint ) )
00813       return false;
00814     if ( bIsClosed )
00815       Pm = Pp;
00816     if ( !(Pm-Pp).IsTiny(point_tolerance) )
00817       return false;
00818     break;
00819 
00820   case ON::C1_continuous:
00821     if ( !Ev1Der( t1, Pm, D1m, -1, hint ) )
00822       return false;
00823     if ( !Ev1Der( t0, Pp, D1p,  1, hint ) )
00824       return false;
00825     if ( bIsClosed )
00826       Pm = Pp;
00827     if ( !(Pm-Pp).IsTiny(point_tolerance) || !(D1m-D1p).IsTiny(d1_tolerance) )
00828       return false;
00829     break;
00830 
00831   case ON::G1_continuous:
00832     if ( !EvTangent( t1, Pm, Tm, -1, hint ) )
00833       return false;
00834     if ( !EvTangent( t0, Pp, Tp,  1, hint ) )
00835       return false;
00836     if ( bIsClosed )
00837       Pm = Pp;
00838     if ( !(Pm-Pp).IsTiny(point_tolerance) || Tm*Tp < cos_angle_tolerance )
00839       return false;
00840     break;
00841 
00842   case ON::C2_continuous:
00843     if ( !Ev2Der( t1, Pm, D1m, D2m, -1, hint ) )
00844       return false;
00845     if ( !Ev2Der( t0, Pp, D1p, D2p,  1, hint ) )
00846       return false;
00847     if ( bIsClosed )
00848       Pm = Pp;
00849     if ( !(Pm-Pp).IsTiny(point_tolerance) || !(D1m-D1p).IsTiny(d1_tolerance) || !(D2m-D2p).IsTiny(d2_tolerance) )
00850       return false;
00851     break;
00852 
00853   case ON::G2_continuous:
00854   case ON::Gsmooth_continuous:
00855     if ( !EvCurvature( t1, Pm, Tm, Km, -1, hint ) )
00856       return false;
00857     if ( !EvCurvature( t0, Pp, Tp, Kp,  1, hint ) )
00858       return false;
00859     if ( !bIsClosed && !(Pm-Pp).IsTiny(point_tolerance) )
00860       return false;
00861     if ( Tm*Tp < cos_angle_tolerance )
00862       return false; // tangent discontinuity
00863 
00864     if ( desired_continuity == ON::Gsmooth_continuous )
00865     {
00866       if ( !ON_IsGsmoothCurvatureContinuous(Km,Kp,cos_angle_tolerance,curvature_tolerance) )
00867         return false;
00868     }
00869     else
00870     {
00871       if ( !ON_IsG2CurvatureContinuous(Km,Kp,cos_angle_tolerance,curvature_tolerance) )
00872         return false;
00873     }
00874     break;
00875 
00876   case ON::C0_locus_continuous:
00877   case ON::C1_locus_continuous:
00878   case ON::C2_locus_continuous:
00879   case ON::G1_locus_continuous:
00880   case ON::G2_locus_continuous:
00881   case ON::Cinfinity_continuous:
00882     break;
00883   }
00884 
00885   return true;
00886 }
00887 
00888 
00889 ON_3dPoint ON_Curve::PointAt( double t ) const
00890 {
00891   ON_3dPoint p(0.0,0.0,0.0);
00892   if ( !EvPoint(t,p) )
00893     p = ON_UNSET_POINT;
00894   return p;
00895 }
00896 
00897 ON_3dPoint ON_Curve::PointAtStart() const
00898 {
00899   return PointAt(Domain().Min());
00900 }
00901 
00902 ON_3dPoint ON_Curve::PointAtEnd() const
00903 {
00904   return PointAt(Domain().Max());
00905 }
00906 
00907 
00908 ON_BOOL32 ON_Curve::SetStartPoint(ON_3dPoint start_point)
00909 {
00910   return false;
00911 }
00912 
00913 ON_BOOL32 ON_Curve::SetEndPoint(ON_3dPoint end_point)
00914 {
00915   return false;
00916 }
00917 
00918 ON_3dVector ON_Curve::DerivativeAt( double t ) const
00919 {
00920   ON_3dPoint  p(0.0,0.0,0.0);
00921   ON_3dVector d(0.0,0.0,0.0);
00922   Ev1Der(t,p,d);
00923   return d;
00924 }
00925 
00926 ON_3dVector ON_Curve::TangentAt( double t ) const
00927 {
00928   ON_3dPoint point;
00929   ON_3dVector tangent;
00930   EvTangent( t, point, tangent );
00931   return tangent;
00932 }
00933 
00934 ON_3dVector ON_Curve::CurvatureAt( double t ) const
00935 {
00936   ON_3dPoint point;
00937   ON_3dVector tangent, kappa;
00938   EvCurvature( t, point, tangent, kappa );
00939   return kappa;
00940 }
00941 
00942 ON_BOOL32 ON_Curve::EvTangent(
00943        double t,
00944        ON_3dPoint& point,
00945        ON_3dVector& tangent,
00946        int side,
00947        int* hint
00948        ) const
00949 {
00950   ON_3dVector D1, D2;//, K;
00951   tangent.Zero();
00952   int rc = Ev1Der( t, point, tangent, side, hint );
00953   if ( rc && !tangent.Unitize() ) 
00954   {
00955     if ( Ev2Der( t, point, D1, D2, side, hint ) )
00956     {
00957       // Use l'Hopital's rule to show that if the unit tanget
00958       // exists, the 1rst derivative is zero, and the 2nd
00959       // derivative is nonzero, then the unit tangent is equal
00960       // to +/-the unitized 2nd derivative.  The sign is equal
00961       // to the sign of D1(s) o D2(s) as s approaches the 
00962       // evaluation parameter.
00963       tangent = D2;
00964       rc = tangent.Unitize();
00965       if ( rc )
00966       {
00967         ON_Interval domain = Domain();
00968         double tminus = 0.0;
00969         double tplus = 0.0;
00970         if ( domain.IsIncreasing() && GetParameterTolerance( t, &tminus, &tplus ) )
00971         {
00972           ON_3dPoint p;
00973           ON_3dVector d1, d2;
00974           double eps = 0.0;
00975           double d1od2tol = 0.0; //1.0e-10; // 1e-5 is too big
00976           double d1od2;
00977           double tt = t;
00978           //double dt = 0.0;
00979 
00980           if ( (t < domain[1] && side >= 0) || (t == domain[0]) )
00981           {
00982             eps = tplus-t;
00983             if ( eps <= 0.0 || t+eps > domain.ParameterAt(0.1) )
00984               return rc;
00985           }
00986           else if ( (t > domain[0] && side < 0) || (t == domain[1]) )
00987           {
00988             eps = tminus - t;
00989             if ( eps >= 0.0 || t+eps < domain.ParameterAt(0.9) )
00990               return rc;
00991           }
00992 
00993           int i, negative_count=0, zero_count=0;
00994           int test_count = 3;
00995           for ( i = 0; i < test_count; i++, eps *= 0.5 )
00996           {
00997             tt = t + eps;
00998             if ( tt == t )
00999               break;
01000             if (!Ev2Der( tt, p, d1, d2, side, 0 ))
01001               break;
01002             d1od2 = d1*d2;
01003             if ( d1od2 > d1od2tol )
01004               break;
01005             if ( d1od2 < d1od2tol )
01006               negative_count++;
01007             else
01008               zero_count++;
01009           }
01010           if ( negative_count > 0 && test_count == negative_count+zero_count )
01011           {
01012             // all sampled d1od2 values were <= 0 
01013             // and at least one was strictly < 0.
01014             tangent.Reverse();
01015           }
01016         }
01017       }
01018     }
01019   }
01020   return rc;
01021 }
01022 
01023 ON_BOOL32 ON_Curve::EvCurvature(
01024        double t,
01025        ON_3dPoint& point,
01026        ON_3dVector& tangent,
01027        ON_3dVector& kappa,
01028        int side,
01029        int* hint
01030        ) const
01031 {
01032   ON_3dVector d1, d2;
01033   int rc = Ev2Der( t, point, d1, d2, side, hint );
01034   if ( rc )
01035     rc = ON_EvCurvature( d1, d2, tangent, kappa );
01036   return rc;
01037 }
01038 
01039 
01040 
01041 ON_BOOL32 ON_Curve::FrameAt( double t, ON_Plane& plane) const
01042 {
01043   ON_BOOL32 rc = false;
01044   ON_Interval domain = Domain();
01045   if( t < domain[0] - ON_EPSILON || t > domain[1] + ON_EPSILON)
01046     return false;
01047 
01048   ON_3dPoint pt;
01049   ON_3dVector d1, d2, T, K;
01050   rc = Ev2Der( t, pt, d1, d2 );
01051   if (rc)
01052     rc = ON_EvCurvature( d1, d2, T, K );
01053   if (rc)
01054   {
01055     if ( !K.Unitize() )
01056       K.PerpendicularTo(T);
01057     K.Unitize();
01058     plane.origin = pt;
01059     plane.xaxis = T;
01060     plane.yaxis = K;
01061     plane.zaxis = ON_CrossProduct(plane.xaxis,plane.yaxis);
01062     plane.UpdateEquation();
01063   }
01064   return rc;
01065 }
01066 
01067 ON_BOOL32 ON_Curve::EvPoint( // returns false if unable to evaluate
01068        double t,         // evaluation parameter
01069        ON_3dPoint& point,   // returns value of curve
01070        int side,        // optional - determines which side to evaluate from
01071                        //         0 = default
01072                        //      <  0 to evaluate from below, 
01073                        //      >  0 to evaluate from above
01074        int* hint       // optional - evaluation hint used to speed repeated
01075                        //            evaluations
01076        ) const
01077 {
01078   ON_BOOL32 rc = false;
01079   double ws[128];
01080   double* v;
01081   if ( Dimension() <= 3 ) {
01082     v = &point.x;
01083     point.x = 0.0;
01084     point.y = 0.0;
01085     point.z = 0.0;
01086   }
01087   else if ( Dimension() <= 128 ) {
01088     v = ws;
01089   }
01090   else {
01091     v = (double*)onmalloc(Dimension()*sizeof(*v));
01092   }
01093   rc = Evaluate( t, 0, Dimension(), v, side, hint );
01094   if ( Dimension() > 3 ) {
01095     point.x = v[0];
01096     point.y = v[1];
01097     point.z = v[2];
01098     if ( Dimension() > 128 )
01099       onfree(v);
01100   }
01101   return rc;
01102 }
01103 
01104 bool ON_Brep::EvaluatePoint( const class ON_ObjRef& objref, ON_3dPoint& P ) const
01105 {
01106   // TODO
01107   return false;
01108 }
01109 
01110 bool ON_Surface::EvaluatePoint( const class ON_ObjRef& objref, ON_3dPoint& P ) const
01111 {
01112   // TODO
01113   return false;
01114 }
01115 
01116 bool ON_PolyCurve::EvaluatePoint( const class ON_ObjRef& objref, ON_3dPoint& P ) const
01117 {
01118   // TODO
01119   return false;
01120 }
01121 
01122 bool ON_Curve::EvaluatePoint( const class ON_ObjRef& objref, ON_3dPoint& P ) const
01123 {
01124   // virtual function default
01125   bool rc = false;
01126 
01127   ON_3dPoint Q = ON_UNSET_POINT;
01128   if ( 1 == objref.m_evp.m_t_type )
01129   {
01130     if ( !EvPoint(objref.m_evp.m_t[0],Q) )
01131       Q = ON_UNSET_POINT;
01132   }
01133 
01134   switch( objref.m_osnap_mode )
01135   {
01136   case ON::os_center:
01137     {
01138       ON_Ellipse ellipse;
01139       if ( IsEllipse(0,&ellipse) )
01140       {
01141         P = ellipse.plane.origin;
01142         rc = true;
01143       }
01144       else
01145       {
01146         ON_SimpleArray<ON_3dPoint> pline;
01147         if ( IsClosed() && IsPolyline(&pline) && pline.Count() >= 4 )
01148         {
01149           P = pline[0];
01150           int i;
01151           for ( i = pline.Count()-2; i > 0; i-- )
01152           {
01153             Q = pline[i];
01154             P.x += Q.x; P.y += Q.y; P.z += Q.z;
01155           }
01156           double s = 1.0/(pline.Count()-1.0);
01157           P.x *= s; P.y *= s; P.z *= s;
01158           rc = true;
01159         }
01160         else if ( Q.IsValid() )
01161         {
01162           ON_3dVector T, K;
01163           if ( EvCurvature(objref.m_evp.m_t[0],Q,T,K) )
01164           {
01165             double k = K.Length();
01166             if ( k > 0.0 )
01167             {
01168               P = Q + (1.0/(k*k))*K;
01169               rc = true;
01170             }
01171           }
01172         }
01173       }
01174     }
01175     break;
01176 
01177   case ON::os_focus:
01178     {
01179       ON_Ellipse ellipse;
01180       if ( IsEllipse(0,&ellipse) )
01181       {
01182         ON_3dPoint F1, F2;
01183         if ( ellipse.GetFoci(F1,F2) )
01184         {
01185           P = ( F1.DistanceTo(Q) <= F1.DistanceTo(Q)) ? F1 : F2;
01186           rc = true;
01187         }
01188       }
01189     }
01190     break;
01191 
01192   case ON::os_midpoint:
01193     {
01194     }
01195     break;
01196 
01197   case ON::os_end:
01198     {
01199       ON_SimpleArray<ON_3dPoint> pline;
01200       if ( IsPolyline(&pline)  )
01201       {
01202         P = pline[0];
01203         double d = P.DistanceTo(Q);
01204         int i;
01205         for ( i = 1; i < pline.Count(); i++ )
01206         {
01207           double d1 = pline[i].DistanceTo(Q);
01208           if ( d1 < d )
01209           {
01210             d = d1;
01211             P = pline[i];
01212             rc = true;
01213           }
01214         }
01215       }
01216       else
01217       {
01218         P = PointAtStart();
01219         rc = true;
01220         if ( !IsClosed() )
01221         {
01222           ON_3dPoint P1 = PointAtEnd();
01223           if ( P.DistanceTo(Q) > P1.DistanceTo(Q) )
01224           {
01225             P = P1;
01226           }
01227         }
01228       }      
01229     }
01230     break;
01231 
01232   default:
01233     if ( Q.IsValid() )
01234     {
01235       P = Q;
01236       rc = true;
01237     }
01238     break;
01239   }
01240 
01241   return rc;
01242 }
01243 
01244 ON_BOOL32 ON_Curve::Ev1Der( // returns false if unable to evaluate
01245        double t,         // evaluation parameter
01246        ON_3dPoint& point,
01247        ON_3dVector& derivative,
01248        int side,        // optional - determines which side to evaluate from
01249                        //      <= 0 to evaluate from below, 
01250                        //      >  0 to evaluate from above
01251        int* hint       // optional - evaluation hint used to speed repeated
01252                        //            evaluations
01253        ) const
01254 {
01255   ON_BOOL32 rc = false;
01256   const int dim = Dimension();
01257   double ws[2*64];
01258   double* v;
01259   point.x = 0.0;
01260   point.y = 0.0;
01261   point.z = 0.0;
01262   derivative.x = 0.0;
01263   derivative.y = 0.0;
01264   derivative.z = 0.0;
01265   if ( dim <= 64 ) {
01266     v = ws;
01267   }
01268   else {
01269     v = (double*)onmalloc(2*dim*sizeof(*v));
01270   }
01271   rc = Evaluate( t, 1, dim, v, side, hint );
01272   point.x = v[0];
01273   derivative.x = v[dim];
01274   if ( dim > 1 ) {
01275     point.y = v[1];
01276     derivative.y = v[dim+1];
01277     if ( dim > 2 ) {
01278       point.z = v[2];
01279       derivative.z = v[dim+2];
01280       if ( dim > 64 )
01281         onfree(v);
01282     }
01283   }
01284 
01285   return rc;
01286 }
01287 
01288 ON_BOOL32 ON_Curve::Ev2Der( // returns false if unable to evaluate
01289        double t,         // evaluation parameter
01290        ON_3dPoint& point,
01291        ON_3dVector& firstDervative,
01292        ON_3dVector& secondDervative,
01293        int side,        // optional - determines which side to evaluate from
01294                        //      <= 0 to evaluate from below, 
01295                        //      >  0 to evaluate from above
01296        int* hint       // optional - evaluation hint used to speed repeated
01297                        //            evaluations
01298        ) const
01299 {
01300   ON_BOOL32 rc = false;
01301   const int dim = Dimension();
01302   double ws[3*64];
01303   double* v;
01304   point.x = 0.0;
01305   point.y = 0.0;
01306   point.z = 0.0;
01307   firstDervative.x = 0.0;
01308   firstDervative.y = 0.0;
01309   firstDervative.z = 0.0;
01310   secondDervative.x = 0.0;
01311   secondDervative.y = 0.0;
01312   secondDervative.z = 0.0;
01313   if ( dim <= 64 ) {
01314     v = ws;
01315   }
01316   else {
01317     v = (double*)onmalloc(3*dim*sizeof(*v));
01318   }
01319   rc = Evaluate( t, 2, dim, v, side, hint );
01320   point.x = v[0];
01321   firstDervative.x = v[dim];
01322   secondDervative.x = v[2*dim];
01323   if ( dim > 1 ) {
01324     point.y = v[1];
01325     firstDervative.y = v[dim+1];
01326     secondDervative.y = v[2*dim+1];
01327     if ( dim > 2 ) {
01328       point.z = v[2];
01329       firstDervative.z = v[dim+2];
01330       secondDervative.z = v[2*dim+2];
01331       if ( dim > 64 )
01332         onfree(v);
01333     }
01334   }
01335 
01336   return rc;
01337 }
01338 
01339 static
01340 ON::eCurveType ON_CurveType( const ON_Curve* curve )
01341 {
01342   const ON_ClassId* curve_id = &ON_Curve::m_ON_Curve_class_id;
01343   const ON_ClassId* id = curve->ClassId();
01344 
01345   // "fake virtual" handling of fast/easy special cases
01346   while (0 != id && curve_id != id )
01347   {
01348     if ( &ON_ArcCurve::m_ON_ArcCurve_class_id == id )
01349       return ON::ctArc;
01350     if ( &ON_LineCurve::m_ON_LineCurve_class_id == id )
01351       return ON::ctLine;
01352     if ( &ON_PolylineCurve::m_ON_PolylineCurve_class_id == id )
01353       return ON::ctPolyline;
01354     if ( &ON_CurveProxy::m_ON_CurveProxy_class_id == id )
01355       return ON::ctProxy;
01356     if ( &ON_PolyCurve::m_ON_PolyCurve_class_id == id )
01357       return ON::ctPolycurve;
01358     if ( &ON_NurbsCurve::m_ON_NurbsCurve_class_id == id )
01359       return ON::ctNurbs;
01360     if ( &ON_CurveOnSurface::m_ON_CurveOnSurface_class_id == id )
01361       return ON::ctOnsurface;
01362     id = id->BaseClass();
01363   }
01364   
01365   return ON::ctCurve;
01366 }
01367 
01368 /*
01369 Description:
01370   Carefully match curve ends.
01371 Parameters:
01372   curve0 - [in]
01373   end0 - [in] 0=match start of curve0, 1 = match end of curve0
01374   curve1 - [in]
01375   end1 - [in] 0=match start of curve1, 1 = match end of curve1
01376   gap_tolerance - [in]
01377     The match is not performed if the initial gap is <= gap_tolerance.
01378     If gap_tolerance < 0, then ON_ComparePoint() is used to
01379     compare the points.
01380 Returns:
01381   True if ends of curves are matched to requested gap_tolerance.
01382 */
01383 bool ON_MatchCurveEnds( ON_Curve* curve0,
01384                         int end0,
01385                         ON_Curve* curve1,
01386                         int end1,
01387                         double gap_tolerance = 0.0
01388                         );
01389 
01390 bool ON_MatchCurveEnds( ON_Curve* curve0,
01391                         int end0,
01392                         ON_Curve* curve1,
01393                         int end1,
01394                         double gap_tolerance )
01395 {
01396   ON_BOOL32 rc = false;
01397   if ( 0 != curve0 && 0 != curve1 
01398        && end0 >= 0 && end0 <= 1 
01399        && end1 >= 0 && end1 <= 1 )
01400   {
01401     ON_3dPoint P0 = end0 ? curve0->PointAtEnd() : curve0->PointAtStart();
01402     ON_3dPoint P1 = end1 ? curve1->PointAtEnd() : curve1->PointAtStart();
01403     rc = ( gap_tolerance < 0.0 )
01404        ? (0==ON_ComparePoint( 3, false, &P0.x, &P1.x ) )
01405        : (P0.DistanceTo(P1) <= gap_tolerance);
01406     if ( !rc )
01407     {
01408       // try to close the gap
01409       ON_Curve* seg[2] = {0,0};
01410       int fix[2] = {0,0};
01411       ON_3dPoint fixPoint[2];
01412       fixPoint[0] = ON_UNSET_POINT;
01413       fixPoint[1] = ON_UNSET_POINT;
01414 
01415       // hurestic for deciding which point gets moved
01416       int i;
01417       for ( i = 0; i <= 1; i++ )
01418       {
01419         ON_3dPoint Q0 = ON_UNSET_POINT;
01420         ON_3dPoint Q1 = ON_UNSET_POINT;
01421         ON_Curve* c = i ? curve1 : curve0;
01422         ON::eCurveType ct = ON_CurveType(c);
01423         int e = i ? end1 : end0;
01424         while ( ON::ctPolycurve == ct )
01425         {
01426           c->DestroyRuntimeCache();
01427           ON_PolyCurve* polycurve = ON_PolyCurve::Cast(c);
01428           if ( 0 == polycurve )
01429             break;
01430           c = polycurve->SegmentCurve(e?(polycurve->Count()-1):0);
01431           if( 0 == c )
01432             return false;
01433           ct = ON_CurveType(c);
01434         }
01435         seg[i] = c;
01436         switch(ct)
01437         {
01438         case ON::ctArc: // arc
01439           if ( c->IsClosed() )
01440             fix[i] = 200;
01441           else
01442           {
01443             fix[i] = 100;
01444           }
01445           Q0 = c->PointAtStart();
01446           Q1 = c->PointAtEnd();
01447           break;
01448         case ON::ctLine: // line
01449           fix[i] = 20;
01450           Q0 = c->PointAtStart();
01451           Q1 = c->PointAtEnd();
01452           break;
01453         case ON::ctPolyline: // polyline
01454           fix[i] = 10;
01455           if ( c->SpanCount() == 1 )
01456           {
01457             // same as a line
01458             fix[i] = 20;
01459             Q0 = c->PointAtStart();
01460             Q1 = c->PointAtEnd();
01461           }
01462           else
01463             fix[i] = 10;
01464           break;
01465         case ON::ctProxy: // proxy
01466           fix[i] = 1000; // cannot change
01467           break;
01468         case ON::ctPolycurve: // polycurve
01469           fix[i] = 1000; // if this happens, something is bad
01470           break;
01471         case ON::ctNurbs: // nurbs
01472           {
01473             if ( c->Degree() == 1 )
01474             {
01475               if ( c->SpanCount() == 1 )
01476               {
01477                 // same as a line
01478                 fix[i] = 20; 
01479                 Q0 = c->PointAtStart();
01480                 Q1 = c->PointAtEnd();
01481               }
01482               else
01483               {
01484                 // same as a polyline
01485                 fix[i] = 10;
01486               }
01487             }
01488             else
01489               fix[i] = 0;
01490           }
01491           break;
01492         case ON::ctOnsurface: // curve on surface
01493           fix[i] = 1000; // cannot change
01494           break;
01495         default: // ???
01496           fix[i] = 50;
01497           break;
01498         }
01499 
01500         int j = 0;
01501         if ( ON_UNSET_VALUE != Q0.x && Q0.x == Q1.x )
01502         {
01503           fixPoint[i].x = Q0.x;
01504           j++;
01505         }
01506         if ( ON_UNSET_VALUE != Q0.y && Q0.y == Q1.y )
01507         {
01508           fixPoint[i].y = Q0.y;
01509           j++;
01510         }
01511         if ( ON_UNSET_VALUE != Q0.z && Q0.z == Q1.z )
01512         {
01513           fixPoint[i].z = Q0.z;
01514           j++;
01515         }
01516         if ( 2 == j )
01517           fix[i] += 9;
01518         else if ( 1 == j )
01519           fix[i] += 1;
01520       }
01521 
01522       if ( fix[0] >= 1000 || fix[1] < fix[0] )
01523       {
01524         if ( fix[1] >= 1000 )
01525           return false;
01526         rc =  end1
01527            ? curve1->SetEndPoint(P0)
01528            : curve1->SetStartPoint(P0);
01529       }
01530       else if ( fix[1] >= 1000 || fix[0] < fix[1] )
01531       {
01532         rc =  end0
01533            ? curve0->SetEndPoint(P1)
01534            : curve0->SetStartPoint(P1);
01535       }
01536       else 
01537       {
01538         ON_3dPoint P = 0.5*(P0+P1);
01539         if ( P0.x == P1.x )
01540           P.x = P0.x;
01541         else if ( fixPoint[0].x != ON_UNSET_VALUE && fixPoint[1].x == ON_UNSET_VALUE )
01542           P.x = fixPoint[0].x;
01543         else if ( fixPoint[0].x == ON_UNSET_VALUE && fixPoint[1].x != ON_UNSET_VALUE )
01544           P.x = fixPoint[1].x;
01545         if ( P0.y == P1.y )
01546           P.y = P0.y;
01547         else if ( fixPoint[0].y != ON_UNSET_VALUE && fixPoint[1].y == ON_UNSET_VALUE )
01548           P.y = fixPoint[0].y;
01549         else if ( fixPoint[0].y == ON_UNSET_VALUE && fixPoint[1].y != ON_UNSET_VALUE )
01550           P.y = fixPoint[1].y;
01551         if ( P0.z == P1.z )
01552           P.z = P0.z;
01553         else if ( fixPoint[0].z != ON_UNSET_VALUE && fixPoint[1].z == ON_UNSET_VALUE )
01554           P.z = fixPoint[0].z;
01555         else if ( fixPoint[0].z == ON_UNSET_VALUE && fixPoint[1].z != ON_UNSET_VALUE )
01556           P.z = fixPoint[1].z;   
01557         ON_BOOL32 rc0 =  end0
01558            ? curve0->SetEndPoint(P)
01559            : curve0->SetStartPoint(P);
01560         ON_BOOL32 rc1 =  end1
01561            ? curve1->SetEndPoint(P)
01562            : curve1->SetStartPoint(P);
01563         rc = (rc0 && rc1);
01564       }
01565       if ( rc )
01566       {
01567         P0 = end0 ? curve0->PointAtEnd() : curve0->PointAtStart();
01568         P1 = end1 ? curve1->PointAtEnd() : curve1->PointAtStart();
01569         double d = P0.DistanceTo(P1);
01570         rc = ( gap_tolerance <= 0.0 ) // <= is correct here
01571            ? (0==ON_ComparePoint( 3, false, &P0.x, &P1.x ) )
01572            : (d <= gap_tolerance);
01573       }
01574     }
01575   }
01576   return rc?true:false;
01577 }
01578 
01579 static bool ForceMatchArcs(ON_ArcCurve& Arc0, int end0, ON_ArcCurve& Arc1, int end1)
01580 
01581 {
01582   ON_3dPoint P0 = (end0) ? Arc0.PointAtEnd() : Arc0.PointAtStart();
01583   ON_3dPoint P1 = (end1) ? Arc1.PointAtEnd() : Arc1.PointAtStart();
01584   ON_3dPoint P = 0.5*(P0+P1);
01585 
01586   bool bMove[2] = {true,true};
01587   bool rc = true;
01588 
01589   if (bMove[0]){
01590     ON_BOOL32 brc = (end0) ? Arc0.SetEndPoint(P) : Arc0.SetStartPoint(P);
01591     if (!brc)
01592       rc = false;
01593   }
01594   if (bMove[1]){
01595     ON_BOOL32 brc = (end1) ? Arc1.SetEndPoint(P) : Arc1.SetStartPoint(P);
01596     if (!brc)
01597       rc = false;
01598   }
01599 
01600   return rc;
01601 
01602 
01603 }
01604 
01605 bool ON_ForceMatchCurveEnds(ON_Curve& Crv0, int end0, ON_Curve& Crv1, int end1)
01606 
01607 {
01608 
01609   if (end0)
01610     end0 = 1;
01611   if (end1)
01612     end1 = 1;
01613 
01614   int i;
01615   ON::eCurveType ct[2];
01616   ON_Curve* seg[2];
01617   bool bIsArc[2];
01618   for ( i = 0; i<2; i++ )
01619   {
01620     ON_Curve* c = i ? &Crv1 : &Crv0;
01621     ct[i] = ON_CurveType(c);
01622     int e = i ? end1 : end0;
01623     while ( ON::ctPolycurve == ct[i] )
01624     {
01625       c->DestroyRuntimeCache();
01626       ON_PolyCurve* polycurve = ON_PolyCurve::Cast(c);
01627       if ( 0 == polycurve )
01628         break;
01629       c = polycurve->SegmentCurve(e?(polycurve->Count()-1):0);
01630       if( 0 == c )
01631         return false;
01632       ct[i] = ON_CurveType(c);
01633     }
01634     if (c->IsClosed())
01635       return false;
01636     if (ct[i] == ON::ctArc)
01637       bIsArc[i] = true;
01638     else {
01639       if (ct[i] == ON::ctLine || ct[i] == ON::ctNurbs || ct[i] == ON::ctPolyline)
01640         bIsArc[i] = false;
01641       else
01642         return false;
01643     }
01644     seg[i] = c;
01645   }
01646 
01647   ON_3dPoint P;
01648   bool bMove[2] = {true,true};
01649   if (bIsArc[0]){
01650     if (!bIsArc[1]){
01651       P = (end0) ? seg[0]->PointAtEnd() : seg[0]->PointAtStart();
01652       bMove[0] = false;
01653     }
01654     else {
01655       ON_ArcCurve* pArc0 = ON_ArcCurve::Cast(seg[0]);
01656       if (!pArc0)
01657         return false;
01658       ON_ArcCurve* pArc1 = ON_ArcCurve::Cast(seg[1]);
01659       if (!pArc1)
01660         return false;
01661       return ForceMatchArcs(*pArc0, end0, *pArc1, end1);
01662     }
01663   }
01664   else {
01665     if (bIsArc[1]){
01666       P = (end1) ? seg[1]->PointAtEnd() : seg[1]->PointAtStart();
01667       bMove[1] = false;
01668     }
01669     else {
01670       ON_3dPoint P0 = (end0) ? seg[0]->PointAtEnd() : seg[0]->PointAtStart();
01671       ON_3dPoint P1 = (end1) ? seg[1]->PointAtEnd() : seg[1]->PointAtStart();
01672       P = 0.5*(P0+P1);
01673     }
01674   }
01675 
01676   bool rc = true;
01677   if (bMove[0]){
01678     ON_BOOL32 brc = (end0) ? seg[0]->SetEndPoint(P) : seg[0]->SetStartPoint(P);
01679     if (!brc)
01680       rc = false;
01681   }
01682   if (bMove[1]){
01683     ON_BOOL32 brc = (end1) ? seg[1]->SetEndPoint(P) : seg[1]->SetStartPoint(P);
01684     if (!brc)
01685       rc = false;
01686   }
01687 
01688   return rc;
01689 }
01690 
01691 bool ON_NurbsCurve::RepairBadKnots( double knot_tolerance, bool bRepair )
01692 {
01693   bool rc = false;
01694   if ( m_order >= 2 && m_cv_count > m_order
01695        && 0 != m_cv && 0 != m_knot 
01696        && m_dim > 0
01697        && m_cv_stride >= (m_is_rat)?(m_dim+1):m_dim
01698        && 0 != m_cv
01699        && 0 != m_knot
01700        && m_knot[m_cv_count-1] - m_knot[m_order-2] > knot_tolerance
01701        )
01702   {
01703     // save domain so it does not change
01704     ON_Interval domain = Domain();
01705     //const int cv_count0 = m_cv_count;
01706 
01707     const int sizeof_cv = CVSize()*sizeof(*m_cv);
01708     //int knot_count = KnotCount();
01709     int i, j0, j1;
01710 
01711     ON_BOOL32 bIsPeriodic = IsPeriodic();
01712 
01713     if ( !bIsPeriodic )
01714     {
01715       if ( m_knot[0] != m_knot[m_order-2] || m_knot[m_cv_count-1] != m_knot[m_cv_count+m_order-3] )
01716       {
01717         rc = true;
01718         if ( bRepair )
01719           ClampEnd(2);
01720         else
01721           return rc;
01722       }
01723     }
01724 
01725     // make sure last span has m_knot[m_cv_count-1] - m_knot[m_cv_count-2] > knot_tolerance
01726     for ( i = m_cv_count-2; i > m_order-2; i-- )
01727     {
01728       if ( m_knot[m_cv_count-1] - m_knot[i] > knot_tolerance )
01729       {
01730         if ( i < m_cv_count-2 )
01731         {
01732           rc = true;
01733           if ( bRepair )
01734           {
01735             // remove extra knots but do not change end point location
01736             DestroyRuntimeCache();
01737             double* cv = (double*)onmalloc(sizeof_cv);
01738             ClampEnd(2);
01739             memcpy( cv, CV(m_cv_count-1), sizeof_cv );
01740             m_cv_count = i+2;
01741             ClampEnd(2);
01742             memcpy( CV(m_cv_count-1), cv, sizeof_cv );
01743             for ( i = m_cv_count-1; i < m_cv_count+m_order-2; i++ )
01744               m_knot[i] = domain[1];
01745             onfree(cv);
01746             cv = 0;
01747           }
01748           else
01749             return rc;
01750         }
01751         break; // there at least one valid span
01752       }
01753     }
01754 
01755     // make sure first span has m_knot[m_order-1] - m_knot[m_order-2] > knot_tolerance
01756     for ( i = m_order-1; i < m_cv_count-1; i++ )
01757     {
01758       if ( m_knot[i] - m_knot[m_order-2] > knot_tolerance )
01759       {
01760         if ( i > m_order-1 )
01761         {
01762           rc = true;
01763           if ( bRepair )
01764           {
01765             // remove extra knots but do not change end point location
01766             DestroyRuntimeCache();
01767             i -= (m_order-1);
01768             double* cv = (double*)onmalloc(sizeof_cv);
01769             ClampEnd(2);
01770             memcpy(cv,CV(0),sizeof_cv);
01771             for ( j0 = 0, j1 = i; j1 < m_cv_count; j0++, j1++ )
01772               memcpy(CV(j0),CV(j1),sizeof_cv);
01773             for ( j0 = 0, j1 = i; j1 < m_cv_count+m_order-2; j0++, j1++ )
01774               m_knot[j0] = m_knot[j1];
01775             m_cv_count -= i;
01776             ClampEnd(2);
01777             memcpy( CV(0), cv, sizeof_cv );
01778             for ( i = 0; i <= m_order-2; i++ )
01779               m_knot[i] = domain[0];
01780             onfree(cv);
01781             cv = 0;
01782           }
01783           else
01784             return rc;
01785         }
01786         break; // there at least one valid span
01787       }
01788     }
01789 
01790 
01791     if (    m_knot[m_order-1]-m_knot[m_order-2] > knot_tolerance 
01792          && m_knot[m_cv_count-1]-m_knot[m_cv_count-2] > knot_tolerance )
01793     {
01794       // Remove interior knots with mulitiplicity >= m_order
01795       for ( i = m_cv_count-m_order-1; i >= m_order; i-- )
01796       {
01797         if ( m_knot[i+m_order-1] - m_knot[i] <= knot_tolerance )
01798         {
01799           rc = true;
01800           if ( bRepair )
01801           {
01802             // empty evaluation span - remove CV and knot
01803             DestroyRuntimeCache();
01804             for ( j0 = i, j1 = i+1; j1 < m_cv_count; j0++, j1++ )
01805               memcpy( CV(j0), CV(j1), sizeof_cv );
01806             for ( j0 = i, j1 = i+1; j1 < m_cv_count+m_order-2; j0++, j1++ )
01807               m_knot[j0] = m_knot[j1];
01808             m_cv_count--;
01809           }
01810           else
01811           {
01812             // query mode
01813             return rc;
01814           }
01815         }
01816       }
01817     }
01818 
01819     if ( bRepair && bIsPeriodic && rc )
01820     {
01821       if ( !IsPeriodic() )
01822         ClampEnd(2);
01823     }
01824   }
01825   return rc;
01826 }
01827 
01828 
01829 
01830 bool ON_Curve::FirstSpanIsLinear( 
01831   double min_length,
01832   double tolerance
01833   ) const
01834 {
01835   return FirstSpanIsLinear(min_length,tolerance,0);
01836 }
01837 
01838 bool ON_Curve::FirstSpanIsLinear( 
01839   double min_length,
01840   double tolerance,
01841   ON_Line* span_line
01842   ) const
01843 {
01844   const ON_NurbsCurve* nurbs_curve = ON_NurbsCurve::Cast(this);
01845   if ( 0 != nurbs_curve )
01846   {
01847     return nurbs_curve->SpanIsLinear(0,min_length,tolerance,span_line);
01848   }
01849 
01850   const ON_PolylineCurve* polyline_curve = ON_PolylineCurve::Cast(this);
01851   if ( 0 != polyline_curve )
01852   {
01853     bool rc = (polyline_curve->PointCount() >= 2);
01854     if (rc && 0 != span_line )
01855     {
01856       span_line->from = polyline_curve->m_pline[0];
01857       span_line->to = polyline_curve->m_pline[1];
01858     }
01859     return rc;
01860   }
01861 
01862   const ON_LineCurve* line_curve = ON_LineCurve::Cast(this);
01863   if ( 0 != line_curve )
01864   {
01865     if ( span_line )
01866       *span_line = line_curve->m_line;
01867     return true;
01868   }
01869 
01870   const ON_PolyCurve* poly_curve = ON_PolyCurve::Cast(this);
01871   if ( 0 != poly_curve )
01872   {
01873     const ON_Curve* segment = poly_curve->SegmentCurve(0);
01874     return (0 != segment) ? segment->FirstSpanIsLinear(min_length,tolerance,span_line) : false;
01875   }
01876 
01877   const ON_CurveProxy* proxy_curve = ON_CurveProxy::Cast(this);
01878   if ( 0 != proxy_curve )
01879   {
01880     const ON_Curve* curve = proxy_curve->ProxyCurve();
01881     if ( 0 == curve )
01882       return false;
01883     bool bProxyCurveIsReversed = proxy_curve->ProxyCurveIsReversed();
01884     bool rc = bProxyCurveIsReversed
01885             ? curve->FirstSpanIsLinear(min_length,tolerance,span_line) 
01886             : curve->LastSpanIsLinear(min_length,tolerance,span_line);
01887     if ( rc && bProxyCurveIsReversed && 0 != span_line )
01888       span_line->Reverse();
01889     return rc;
01890   }
01891 
01892   return false;
01893 }
01894 
01895 
01896 
01897 bool ON_Curve::LastSpanIsLinear( 
01898   double min_length,
01899   double tolerance
01900   ) const
01901 {
01902   return LastSpanIsLinear(min_length,tolerance,0);
01903 }
01904 
01905 bool ON_Curve::LastSpanIsLinear( 
01906   double min_length,
01907   double tolerance,
01908   ON_Line* span_line
01909   ) const
01910 {
01911   const ON_NurbsCurve* nurbs_curve = ON_NurbsCurve::Cast(this);
01912   if ( 0 != nurbs_curve )
01913   {
01914     return nurbs_curve->SpanIsLinear(nurbs_curve->m_cv_count-nurbs_curve->m_order,min_length,tolerance,span_line);
01915   }
01916 
01917   const ON_PolylineCurve* polyline_curve = ON_PolylineCurve::Cast(this);
01918   if ( 0 != polyline_curve )
01919   {
01920     int count = polyline_curve->PointCount();
01921     if ( count >= 2 && 0 != span_line )
01922     {
01923       span_line->from = polyline_curve->m_pline[count-2];
01924       span_line->to = polyline_curve->m_pline[count-1];
01925     }
01926     return ( count >= 2);
01927   }
01928 
01929   const ON_LineCurve* line_curve = ON_LineCurve::Cast(this);
01930   if ( 0 != line_curve )
01931   {
01932     if ( span_line )
01933       *span_line = line_curve->m_line;
01934     return true;
01935   }
01936 
01937   const ON_PolyCurve* poly_curve = ON_PolyCurve::Cast(this);
01938   if ( 0 != poly_curve )
01939   {
01940     const ON_Curve* segment = poly_curve->SegmentCurve(poly_curve->Count()-1);
01941     return (0 != segment) ? segment->LastSpanIsLinear(min_length,tolerance,span_line) : false;
01942   }
01943 
01944   const ON_CurveProxy* proxy_curve = ON_CurveProxy::Cast(this);
01945   if ( 0 != proxy_curve )
01946   {
01947     const ON_Curve* curve = proxy_curve->ProxyCurve();
01948     if ( 0 == curve )
01949       return false;
01950     bool bProxyCurveIsReversed = proxy_curve->ProxyCurveIsReversed();
01951     bool rc = bProxyCurveIsReversed
01952            ? curve->LastSpanIsLinear(min_length,tolerance,span_line) 
01953            : curve->FirstSpanIsLinear(min_length,tolerance,span_line);
01954     if ( rc && bProxyCurveIsReversed && 0 != span_line )
01955       span_line->Reverse();
01956     return rc;
01957   }
01958 
01959   return false;
01960 }
01961 
01962 
01963 
01964 bool ON_NurbsCurve::SpanIsLinear(
01965     int span_index, 
01966     double min_length,
01967     double tolerance
01968     ) const
01969 {
01970   return SpanIsLinear(span_index,min_length,tolerance,0);
01971 }
01972 
01973 bool ON_NurbsCurve::SpanIsLinear(
01974     int span_index, 
01975     double min_length,
01976     double tolerance,
01977     ON_Line* span_line
01978     ) const
01979 {
01980   if ( m_dim < 2 || m_dim > 3 )
01981     return false;
01982 
01983   if ( -1 == span_index && m_cv_count-m_order+1+span_index >= 0 )
01984   {
01985     // negative span indices work from the back
01986     span_index += m_cv_count-m_order+1;
01987   }
01988   else if ( span_index < 0 || span_index > m_cv_count-m_order )
01989   {
01990     ON_ERROR("span_index out of range.");
01991     return false;
01992   }
01993 
01994   if ( !(m_knot[span_index+m_order-2] < m_knot[span_index+m_order-1]) )
01995   {
01996     // empty span
01997     ON_ERROR("empty span.");
01998     return false;
01999   }
02000 
02001   if (    m_knot[span_index] == m_knot[span_index+m_order-2]
02002        && m_knot[span_index+m_order-1] == m_knot[span_index+2*m_order-3]
02003      )
02004   {
02005     ON_3dPoint P, Q;
02006     ON_Line line;
02007     const int i1 = span_index+m_order-1;
02008     if ( !GetCV(span_index,line.from) )
02009       return false;
02010     if ( !GetCV(i1,line.to) )
02011       return false;
02012     if ( !(line.Length() >= min_length) ) 
02013       return false;
02014     double t0, t, d;
02015     t0 = t = 0.0;
02016     for ( int i = span_index+1; i < i1; i++ )
02017     {
02018       if ( !GetCV(i,P) )
02019         return false;
02020       if ( !line.ClosestPointTo( P, &t ) )
02021         return false;
02022       if ( !(t0 < t) )
02023         return false;
02024                         if ( !(t <= 1.0 + ON_SQRT_EPSILON) )
02025                                 return false;
02026       Q = line.PointAt(t);
02027       if ( false == ON_PointsAreCoincident(3,0,&P.x,&Q.x) )
02028       {
02029         d = P.DistanceTo( line.PointAt(t) );
02030         if ( !(d <= tolerance) )
02031           return false;
02032       }
02033       t0 = t;
02034     }
02035     if ( span_line )
02036       *span_line = line;
02037     return true;
02038   }
02039 
02040   return false;
02041 }
02042 
02043 ON_BOOL32 ON_Curve::Trim( const ON_Interval& in )
02044 {
02045   // TODO - make this pure virtual
02046   return false;
02047 }
02048 
02049 
02050 bool ON_Curve::Extend(
02051   const ON_Interval& domain
02052   )
02053 
02054 {
02055   return false;
02056 }
02057 
02058 
02059 ON_Curve* ON_TrimCurve( 
02060             const ON_Curve& curve,
02061             ON_Interval trim_parameters
02062             )
02063 {
02064   ON_Curve* trimmed_curve = 0;
02065 
02066   const ON_Interval curve_domain = curve.Domain();
02067   ON_BOOL32 bDecreasing = trim_parameters.IsDecreasing();
02068   trim_parameters.Intersection( curve_domain ); // trim_parameters will be increasing or empty
02069   if ( bDecreasing )
02070   {
02071     trim_parameters.Swap();
02072     if ( trim_parameters[0] == curve_domain[1] )
02073     {
02074       if ( trim_parameters[1] == curve_domain[0] )
02075         return 0;
02076       trim_parameters[0] = curve_domain[0];
02077     }
02078     else if ( trim_parameters[1] == curve_domain[0] )
02079       trim_parameters[1] = curve_domain[1];
02080     else if ( !trim_parameters.IsDecreasing() )
02081       return 0;
02082   }
02083 
02084   if ( trim_parameters.IsDecreasing() && curve.IsClosed() )
02085   {
02086     ON_Curve* left_crv = curve.DuplicateCurve();
02087     if ( !left_crv->Trim(ON_Interval(trim_parameters[0],curve_domain[1])) )
02088     {
02089       delete left_crv;
02090       return 0;
02091     }
02092     ON_Curve* right_crv = curve.DuplicateCurve();
02093     if ( !right_crv->Trim(ON_Interval(curve_domain[0],trim_parameters[1])) )
02094     {
02095       delete left_crv;
02096       delete right_crv;
02097       return 0;
02098     }
02099     ON_PolyCurve* polycurve = ON_PolyCurve::Cast(left_crv);
02100     if ( polycurve == NULL )
02101     {
02102       polycurve = new ON_PolyCurve();      
02103       polycurve->Append( left_crv );
02104     }
02105 
02106     ON_PolyCurve* ptmp = ON_PolyCurve::Cast(right_crv);
02107     if ( ptmp )
02108     {
02109       int i;
02110       for ( i = 0; i < ptmp->Count(); i++ )
02111       {
02112         ON_Interval sdom = ptmp->SegmentDomain(i);
02113         ON_Curve* segment = ptmp->HarvestSegment(i);
02114         segment->SetDomain(sdom[0],sdom[1]); // to keep relative parameterization unchanged
02115         polycurve->Append( segment );
02116       }
02117       delete right_crv;
02118       ptmp = 0;
02119       right_crv = 0;
02120     }
02121     else
02122     {
02123       polycurve->Append( right_crv );
02124     }
02125 
02126     polycurve->SetDomain( trim_parameters[0], trim_parameters[1] + curve_domain.Length() );
02127 
02128     trimmed_curve = polycurve;
02129   }
02130   else if ( trim_parameters.IsIncreasing() )
02131   {
02132     trimmed_curve = curve.DuplicateCurve();
02133     if( !trimmed_curve->Trim(trim_parameters) )
02134     {
02135       delete trimmed_curve;
02136       trimmed_curve = 0;
02137     }
02138   }
02139 
02140   return trimmed_curve;
02141 }
02142 
02143 ON_BOOL32 ON_Curve::Split(
02144     double, // t = curve parameter to split curve at
02145     ON_Curve*&, // left portion returned here (can pass "this" as the pointer)
02146     ON_Curve*&  // right portion returned here (can pass "this" as the pointer)
02147   ) const
02148 {
02149   // override if curve can split itself
02150   return false;
02151 }
02152 
02153 // virtual
02154 int ON_Curve::GetNurbForm(
02155       ON_NurbsCurve& nurbs_curve,
02156       double tolerance,
02157       const ON_Interval* subdomain
02158       ) const
02159 {
02160   return 0;
02161 }
02162 
02163 // virtual
02164 int ON_Curve::HasNurbForm() const
02165 {
02166   return 0;
02167 }
02168 
02169 ON_NurbsCurve* ON_Curve::NurbsCurve(
02170       ON_NurbsCurve* pNurbsCurve,
02171       double tolerance,
02172       const ON_Interval* subdomain
02173       ) const
02174 {
02175   ON_NurbsCurve* nurbs_curve = pNurbsCurve;
02176   if ( !nurbs_curve )
02177     nurbs_curve = new ON_NurbsCurve();
02178   int rc = GetNurbForm( *nurbs_curve, tolerance, subdomain );
02179   if ( !rc )
02180   {
02181     if (!pNurbsCurve)
02182       delete nurbs_curve;
02183     nurbs_curve = NULL;
02184   }
02185   return nurbs_curve;
02186 }
02187 
02188 ON_BOOL32 ON_Curve::GetCurveParameterFromNurbFormParameter(
02189       double nurbs_t,
02190       double* curve_t
02191       ) const
02192 {
02193   *curve_t = nurbs_t;
02194   return false;
02195 }
02196 
02197 ON_BOOL32 ON_Curve::GetNurbFormParameterFromCurveParameter(
02198       double curve_t,
02199       double* nurbs_t
02200       ) const
02201 {
02202   *nurbs_t = curve_t;
02203   return false;
02204 }
02205 
02206 ON_CurveArray::ON_CurveArray( int initial_capacity ) 
02207               : ON_SimpleArray<ON_Curve*>(initial_capacity)
02208 {}
02209 
02210 ON_CurveArray::~ON_CurveArray()
02211 {
02212   Destroy();
02213 }
02214 
02215 void ON_CurveArray::Destroy()
02216 {
02217   int i = m_capacity;
02218   while ( i-- > 0 ) {
02219     if ( m_a[i] ) {
02220       delete m_a[i];
02221       m_a[i] = NULL;
02222     }
02223   }
02224   Empty();
02225 }
02226 
02227 bool ON_CurveArray::Duplicate( ON_CurveArray& dst ) const
02228 {
02229   dst.Destroy();
02230   dst.SetCapacity( Capacity() );
02231 
02232   const int count = Count();
02233   int i;
02234   ON_Curve* curve;
02235   for ( i = 0; i < count; i++ ) 
02236   {
02237     curve = 0;
02238     if ( m_a[i] ) 
02239     {
02240       curve = m_a[i]->Duplicate();
02241     }
02242     dst.Append(curve);      
02243   }
02244   return true;
02245 }
02246 
02247 bool ON_CurveArray::Write( ON_BinaryArchive& file ) const
02248 {
02249   bool rc = file.BeginWrite3dmChunk( TCODE_ANONYMOUS_CHUNK, 0 );
02250   if (rc) rc = file.Write3dmChunkVersion(1,0);
02251   if (rc ) {
02252     int i;
02253     rc = file.WriteInt( Count() );
02254     for ( i = 0; rc && i < Count(); i++ ) {
02255       if ( m_a[i] ) {
02256         rc = file.WriteInt(1);
02257         if ( rc ) 
02258           rc = file.WriteObject( *m_a[i] ); // polymorphic curves
02259       }
02260       else {
02261         // NULL curve
02262         rc = file.WriteInt(0);
02263       }
02264     }
02265     if ( !file.EndWrite3dmChunk() )
02266       rc = false;
02267   }
02268   return rc;
02269 }
02270 
02271 
02272 bool ON_CurveArray::Read( ON_BinaryArchive& file )
02273 {
02274   int major_version = 0;
02275   int minor_version = 0;
02276   ON__UINT32 tcode = 0;
02277   ON__INT64 big_value = 0;
02278   int flag;
02279   Destroy();
02280   bool rc = file.BeginRead3dmBigChunk( &tcode, &big_value );
02281   if (rc) 
02282   {
02283     rc = ( tcode == TCODE_ANONYMOUS_CHUNK );
02284     if (rc) rc = file.Read3dmChunkVersion(&major_version,&minor_version);
02285     if (rc && major_version == 1) 
02286     {
02287       ON_Object* p;
02288       int count;
02289       rc = file.ReadInt( &count );
02290       if (rc) 
02291       {
02292         SetCapacity(count);
02293         SetCount(count);
02294         Zero();
02295         int i;
02296         for ( i = 0; rc && i < count && rc; i++ ) 
02297         {
02298           flag = 0;
02299           rc = file.ReadInt(&flag);
02300           if (rc && flag==1) 
02301           {
02302             p = 0;
02303             rc = file.ReadObject( &p ) ? true : false; // polymorphic curves
02304             m_a[i] = ON_Curve::Cast(p);
02305             if ( !m_a[i] )
02306               delete p;
02307           }
02308         }
02309       }
02310     }
02311     else 
02312     {
02313       rc = false;
02314     }
02315     if ( !file.EndRead3dmChunk() )
02316     {
02317       rc = false;
02318     }
02319   }
02320   return rc;
02321 }
02322 
02326 
02327 
02328 struct CurveJoinSeg {
02329   int id;
02330   bool bRev;
02331 };
02332 
02333 //distance from curve[id[0] end[end[0]] to curve[id[1]] end[end[1]] is dist.
02334 struct CurveJoinEndData {
02335   int id[2];  //index into array of curves
02336   int end[2]; //0 for start, 1 for end
02337   double dist;
02338 };
02339 
02340 
02341 static int CompareEndData(const CurveJoinEndData* a, const CurveJoinEndData* b)
02342 
02343 {
02344   if (a->dist < b->dist) return -1;
02345   if (a->dist > b->dist) return 1;
02346   return 0;
02347 }
02348 
02349 static void ReverseSegs(ON_SimpleArray<CurveJoinSeg>& SArray)
02350 
02351 {
02352   int i;
02353   for (i=0; i<SArray.Count(); i++)
02354     SArray[i].bRev = !SArray[i].bRev;
02355   SArray.Reverse();
02356   return;
02357 }
02358 
02362 
02363 
02364 bool ON_Curve::IsClosable(double tolerance, 
02365                           double min_abs_size, //0.0
02366                           double min_rel_size  //10.0
02367                           ) const
02368 
02369 {
02370   if (IsClosed()) return true;
02371   if (Degree() + SpanCount() < 4) return false;
02372 
02373   ON_3dPoint P[6];
02374 
02375   P[0] = PointAtStart();
02376   P[5] = PointAtEnd();
02377 
02378   double gap = P[0].DistanceTo(P[5]);
02379   if (gap > tolerance) return false;
02380 
02381   bool abs_ok = (min_abs_size < 0.0) ? true : false;
02382   bool rel_ok = (min_rel_size <= 1.0) ? true : false;
02383   bool ok = abs_ok && rel_ok;
02384 
02385   if (!ok){
02386 
02387     //make sure curve is long enough to close off.
02388     int i;
02389     double len = 0.0;
02390 
02391     for (i=1; i<6; i++){
02392       if (i!=5)
02393         P[i] = PointAt(Domain().ParameterAt(0.2*i));
02394       if (!abs_ok && P[i].DistanceTo(P[0]) > min_abs_size)
02395         abs_ok = true;
02396       if (!rel_ok){
02397         len += P[i-1].DistanceTo(P[i]);
02398         if (len >= min_rel_size*gap)
02399           rel_ok = true;
02400       }
02401       ok = abs_ok && rel_ok;
02402       if (ok) break;
02403     }
02404   }
02405 
02406   return ok;
02407 }
02408 
02409 //Makes continuous ON_PolyCurves and ON_Polycurves. Appends results to OutCurves.
02410 //returns number of curves appended.
02411 
02412 
02413 
02414 int 
02415 ON_JoinCurves(const ON_SimpleArray<const ON_Curve*>& InCurves,
02416                       ON_SimpleArray<ON_Curve*>& OutCurves,
02417                       double join_tol,
02418                       bool bPreserveDirection, // = false
02419                       ON_SimpleArray<int>* key //=0
02420                       )
02421 
02422 {
02423 
02424   int i, count = OutCurves.Count();
02425   if (InCurves.Count() < 1)
02426     return 0;
02427 
02428   int dim = InCurves[0]->Dimension();
02429   for (i=1; i<InCurves.Count(); i++){
02430     if (InCurves[i]->Dimension() != dim) return 0;
02431   }
02432 
02433   if (key) {
02434     key->Reserve(InCurves.Count());
02435     for (i=0; i<InCurves.Count(); i++) key->Append(-1);
02436   }
02437 
02438   //Copy curves, take out closed curves. 
02439   OutCurves.Reserve(InCurves.Count());
02440   ON_SimpleArray<ON_Curve*> IC(InCurves.Count());
02441   ON_SimpleArray<int> cmap(InCurves.Count());
02442   for (i=0; i<InCurves.Count(); i++){
02443     ON_Curve* C = InCurves[i]->DuplicateCurve();
02444     if (!C) continue;
02445     if (C->IsClosed()) {
02446       if (key) (*key)[i] = OutCurves.Count();
02447       OutCurves.Append(C);
02448     }
02449     else {
02450       cmap.Append(i);
02451       IC.Append(C);
02452     }
02453   }
02454 
02455   //IC is a list of copies of all open curves.  match endpoints and join into polycurves.
02456   //copy curves that are not joined.
02457   ON_3dPointArray Start(IC.Count());
02458   Start.SetCount(IC.Count());
02459   ON_3dPointArray End(IC.Count());
02460   End.SetCount(IC.Count());
02461   for (i=0; i<IC.Count(); i++){
02462     Start[i] = IC[i]->PointAtStart();
02463     End[i] = IC[i]->PointAtEnd();
02464   }
02465 
02466   //get a list of all possible joins
02467   ON_SimpleArray<CurveJoinEndData> EData(IC.Count());
02468   for (i=0; i<IC.Count(); i++){
02469     int j;
02470     for (j=0; j<IC.Count(); j++){
02471       if (i==j) continue;
02472       double dist = Start[i].DistanceTo(End[j]);
02473       if (dist <= join_tol){
02474         CurveJoinEndData& ED = EData.AppendNew();
02475         ED.id[0] = i;
02476         ED.end[0] = 0;
02477         ED.id[1] = j;
02478         ED.end[1] = 1;
02479         ED.dist = dist;
02480       }
02481       if (!bPreserveDirection && i<j){
02482         dist = Start[i].DistanceTo(Start[j]);
02483         if (dist <= join_tol){
02484           CurveJoinEndData& ED = EData.AppendNew();
02485           ED.id[0] = i;
02486           ED.end[0] = 0;
02487           ED.id[1] = j;
02488           ED.end[1] = 0;
02489           ED.dist = dist;
02490         }
02491         dist = End[i].DistanceTo(End[j]);
02492         if (dist <= join_tol){
02493           CurveJoinEndData& ED = EData.AppendNew();
02494           ED.id[0] = i;
02495           ED.end[0] = 1;
02496           ED.id[1] = j;
02497           ED.end[1] = 1;
02498           ED.dist = dist;
02499         }
02500       }
02501     }
02502   }
02503 
02504   //sort possiblities by distance
02505   EData.QuickSort(CompareEndData);
02506 
02507   int* endspace = (int*)onmalloc(2*sizeof(int)*IC.Count());
02508   memset(endspace, 0, 2*sizeof(int)*IC.Count());
02509   int** endarray = (int**)onmalloc(sizeof(int*)*IC.Count());
02510 
02511   //endarray[i] is an int[2].  if endarray[i][0] > 0, then IC[i] is part of
02512   //polycurve endarray[i][0] - 1, and the start of IC[i] is interior to the polycurve.
02513   //if endarray[i][1] > 0, then the end of IC[i] is interior.  if both endarray[i][0] > 0
02514   //and endarray[i][1] > 0, then they are equal.
02515 
02516   for (i=0; i<IC.Count(); i++)
02517     endarray[i] = &endspace[2*i];
02518 
02519   ON_ClassArray<ON_SimpleArray<CurveJoinSeg> > SegsArray(IC.Count());
02520 
02521   for (i=0; i<EData.Count(); i++){
02522     const CurveJoinEndData& ED = EData[i];
02523     if (endarray[ED.id[0]][ED.end[0]] > 0 || endarray[ED.id[1]][ED.end[1]] > 0)
02524       continue; //one of these endpoints has already been join to a closer end
02525     if (endarray[ED.id[0]][1 - ED.end[0]] == 0){
02526       if (endarray[ED.id[1]][1 - ED.end[1]] == 0){//new curve
02527         endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = SegsArray.Count()+1;
02528         ON_SimpleArray<CurveJoinSeg>& SArray = SegsArray.AppendNew();
02529         SArray.Reserve(4);
02530         CurveJoinSeg& Seg0 = SArray.AppendNew();
02531         CurveJoinSeg& Seg1 = SArray.AppendNew();
02532         if (ED.end[0]) {
02533           Seg0.id = ED.id[0];
02534           Seg0.bRev = false;
02535           Seg1.id = ED.id[1];
02536           Seg1.bRev = (ED.end[1]) ? true : false;
02537         }
02538         else {
02539           Seg1.id = ED.id[0];
02540           Seg1.bRev = false;
02541           Seg0.id = ED.id[1];
02542           Seg0.bRev = (ED.end[1]) ? false : true;
02543         }
02544       }
02545 
02546       else {
02547 
02548         //second curve is part of an existing sequence. Insert or append first curve.
02549         ON_SimpleArray<CurveJoinSeg>& SArray = SegsArray[endarray[ED.id[1]][1 - ED.end[1]] - 1];
02550         endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = 
02551           endarray[ED.id[1]][1 - ED.end[1]];
02552 
02553         if (SArray[0].id == ED.id[1]){
02554           CurveJoinSeg Seg;
02555           Seg.id = ED.id[0];
02556           Seg.bRev = (ED.end[0]) ? false : true;
02557           SArray.Insert(0, Seg);
02558         }
02559         else {
02560           CurveJoinSeg& Seg = SArray.AppendNew();
02561           Seg.id = ED.id[0];
02562           Seg.bRev = (ED.end[0]) ? true : false;
02563         }
02564       }
02565     }
02566     else if (endarray[ED.id[1]][1 - ED.end[1]] == 0){
02567     //first curve is part of an existing sequence. Insert or append second curve.
02568       ON_SimpleArray<CurveJoinSeg>& SArray = SegsArray[endarray[ED.id[0]][1 - ED.end[0]] - 1];
02569       endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = 
02570         endarray[ED.id[0]][1 - ED.end[0]];
02571 
02572       if (SArray[0].id == ED.id[0]){
02573         CurveJoinSeg Seg;
02574         Seg.id = ED.id[1];
02575         Seg.bRev = (ED.end[1]) ? false : true;
02576         SArray.Insert(0, Seg);
02577       }
02578       else {
02579         CurveJoinSeg& Seg = SArray.AppendNew();
02580         Seg.id = ED.id[1];
02581         Seg.bRev = (ED.end[1]) ? true : false;
02582       }
02583     }
02584     else {
02585       //both are in existing sequences.  join the sequences.
02586       if (endarray[ED.id[0]][1 - ED.end[0]] == endarray[ED.id[1]][1 - ED.end[1]])
02587         //closes off this curve
02588         endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = 
02589           endarray[ED.id[0]][1 - ED.end[0]];
02590       else {
02591         int segid0 = endarray[ED.id[0]][1 - ED.end[0]];
02592         int segid1 = endarray[ED.id[1]][1 - ED.end[1]];
02593         ON_SimpleArray<CurveJoinSeg>& SArray0 = SegsArray[segid0 - 1];
02594         ON_SimpleArray<CurveJoinSeg>& SArray1 = SegsArray[segid1 - 1];
02595         if (SArray0[0].id == ED.id[0]){
02596           if (SArray1[0].id == ED.id[1]){
02597             ReverseSegs(SArray0);
02598             int j;
02599             for (j=0; j<SArray1.Count(); j++){
02600               if (endarray[SArray1[j].id][0] > 0) endarray[SArray1[j].id][0] = segid0;
02601               if (endarray[SArray1[j].id][1] > 0) endarray[SArray1[j].id][1] = segid0;
02602               SArray0.Append(SArray1[j]);
02603             }
02604             endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = segid0;
02605             SArray1.SetCount(0);
02606           }
02607           else {
02608             int j;
02609             for (j=0; j<SArray0.Count(); j++){
02610               if (endarray[SArray0[j].id][0] > 0) endarray[SArray0[j].id][0] = segid1;
02611               if (endarray[SArray0[j].id][1] > 0) endarray[SArray0[j].id][1] = segid1;
02612               SArray1.Append(SArray0[j]);
02613             }
02614             endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = segid1;
02615             SArray0.SetCount(0);
02616           }
02617         }
02618         else if (SArray1[0].id == ED.id[1]){
02619           int j;
02620           for (j=0; j<SArray1.Count(); j++){
02621             if (endarray[SArray1[j].id][0] > 0) endarray[SArray1[j].id][0] = segid0;
02622             if (endarray[SArray1[j].id][1] > 0) endarray[SArray1[j].id][1] = segid0;
02623             SArray0.Append(SArray1[j]);
02624           }
02625           endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = segid0;
02626           SArray1.SetCount(0);
02627         }
02628         else {
02629           ReverseSegs(SArray1);
02630           int j;
02631           for (j=0; j<SArray1.Count(); j++) {
02632             if (endarray[SArray1[j].id][0] > 0) endarray[SArray1[j].id][0] = segid0;
02633             if (endarray[SArray1[j].id][1] > 0) endarray[SArray1[j].id][1] = segid0;
02634             SArray0.Append(SArray1[j]);
02635           }
02636           endarray[ED.id[0]][ED.end[0]] = endarray[ED.id[1]][ED.end[1]] = segid0;
02637           SArray1.SetCount(0);
02638         }
02639       }
02640     }
02641   }
02642 
02643   //make polycurves out of sequences
02644 
02645   for (i=0; i<SegsArray.Count(); i++){
02646     ON_SimpleArray<CurveJoinSeg>& SArray = SegsArray[i];
02647     if (SArray.Count() < 2) continue;
02648     if (!bPreserveDirection){//if number of reversed segs is more than half, reverse.
02649       int count= 0;
02650       int j;
02651       for (j=0; j<SArray.Count(); j++) {
02652         if (SArray[j].bRev)
02653           count++;
02654       }
02655       if (2*count > SArray.Count())
02656         ReverseSegs(SArray);
02657     }
02658     ON_PolyCurve* PC = new ON_PolyCurve(SArray.Count());
02659     bool pc_added = false;
02660     int j;
02661     int min_seg = 0;
02662     int min_id = -1;
02663     for (j=0; j<SArray.Count(); j++){
02664       if (key) 
02665         (*key)[cmap[SArray[j].id]] = OutCurves.Count();
02666       ON_Curve* C = IC[SArray[j].id];
02667       if (min_id < 0 || SArray[j].id < min_id){
02668         min_id = SArray[j].id;
02669         min_seg = j;
02670       }
02671       if (SArray[j].bRev) C->Reverse();
02672       if (PC->Count()){
02673         bool bSet = true;
02674         if (!ON_ForceMatchCurveEnds(*PC, 1, *C, 0)) {
02675           ON_3dPoint P = PC->PointAtEnd();
02676           ON_3dPoint Q = C->PointAtStart();
02677           P = 0.5*(P+Q);
02678           if (!PC->SetEndPoint(P) || !C->SetStartPoint(P)) {
02679             ON_NurbsCurve* NC = C->NurbsCurve();
02680             if (NC && NC->SetStartPoint(P)){
02681               delete C;
02682               C = NC;
02683             }
02684             else {
02685               bSet = false;
02686               delete NC;
02687               if (PC->Count()) {
02688                 pc_added = true;
02689                 OutCurves.Append(PC);
02690               }
02691               if (key)
02692                 (*key)[cmap[SArray[j].id]]++;
02693               OutCurves.Append(C);
02694               int k;
02695               for (k=j+1; k<SArray.Count(); k++){
02696                 if (key)
02697                   (*key)[cmap[SArray[k].id]] = OutCurves.Count();
02698                 OutCurves.Append(IC[SArray[k].id]);
02699               }
02700               break;
02701             }
02702           }
02703         }
02704       }
02705       ON_PolyCurve* pPoly = ON_PolyCurve::Cast(C);
02706       if( pPoly){
02707         int si;
02708         for (si=0; si<pPoly->Count(); si++){
02709           const ON_Curve* SC = pPoly->SegmentCurve(si);
02710           ON_Curve* SCCopy = SC->DuplicateCurve();
02711           if (SCCopy) PC->Append(SCCopy);
02712         }
02713         delete pPoly;
02714       }
02715       else PC->Append(C);
02716     }
02717     if (!PC->Count()) delete PC;
02718     else if (!pc_added) {
02719       if (!PC->IsClosed() && PC->IsClosable(join_tol)){
02720         if (!ON_ForceMatchCurveEnds(*PC, 0, *PC, 1))
02721           PC->SetEndPoint(PC->PointAtStart());
02722       }
02723       if (PC->IsClosed() && min_id >= 0){
02724         //int sc = PC->SpanCount();
02725         double t = PC->SegmentDomain(min_seg)[0];
02726         PC->ChangeClosedCurveSeam(t);
02727       }
02728 
02729       // 28 October 2010 Dale Lear
02730       //    I added the RemoveNesting() and SynchronizeSegmentDomains()
02731       //    lines so Rhino will create higher quality geometry.
02732       PC->RemoveNesting();
02733       PC->SynchronizeSegmentDomains();
02734 
02735       OutCurves.Append(PC);
02736     }
02737   }
02738 
02739   //add in singletons
02740   for (i=0; i<IC.Count(); i++){
02741     if (endarray[i][0] == 0 && endarray[i][1] == 0){
02742       if (key) (*key)[cmap[i]] = OutCurves.Count();
02743       OutCurves.Append(IC[i]);
02744     }
02745   }
02746 
02747         /* This was added by greg to fix big curves that are nearly, but not quite, closed.
02748      It causes problems when the curve is tiny.
02749         for(i=0; i<OutCurves.Count(); i++){
02750                 if(!OutCurves[i]->IsClosed()){
02751                         ON_3dPoint s= OutCurves[i]->PointAtStart();
02752                         ON_3dPoint e = OutCurves[i]->PointAtEnd();
02753                         if(s.DistanceTo(e)<join_tol)
02754                                 OutCurves[i]->SetEndPoint( s );                 
02755                 }
02756         }
02757   */
02758 
02759   //Chuck added this, 1/16/03.
02760   for(i=0; i<OutCurves.Count(); i++){
02761     ON_Curve* C = OutCurves[i];
02762     if (!C || C->IsClosed()) continue;
02763     if (C->IsClosable(join_tol))
02764       C->SetEndPoint(C->PointAtStart());
02765   }
02766     
02767   onfree((void*)endarray);
02768   onfree((void*)endspace);
02769 
02770   return OutCurves.Count() - count;
02771 }
02772 
02773 // returns true if t is sufficiently close to m_t[index]
02774 // -1 <= index <= m_t.Count()
02775 bool ON_Curve::ParameterSearch(double t, int& index, bool bEnableSnap,
02776                                                                                                                                 const ON_SimpleArray<double>& m_t, double RelTol) const{
02777 
02778   // 24 October 2003 Dale Lear - added comments and fixed bugs when t < m_t[0]
02779   //    If you make changes to this code, please discuss them with Dale Lear.
02780 
02781         bool rc = false;
02782         int count = m_t.Count();
02783         ON_Interval c_dom = Domain();
02784         index = -1;
02785         if(count>1 && ON_IsValid(t))
02786   {
02787                 
02788     index = ON_SearchMonotoneArray(m_t, count, t);
02789     // index < 0       : means t < m_t[0]
02790     // index == count-1: means t == m_t[count-1]
02791     // index == count  : means t > m_t[count-1]
02792 
02793                 rc  = (index>=0 && index<=count-1  && t == m_t[index]);
02794                 if( bEnableSnap && !rc)
02795     {
02796       // see if t is within "ktol" of a value in m_t[]
02797                         double ktol = RelTol*( ON_Max(fabs(c_dom[0]) ,fabs(c_dom[1])));
02798 
02799       if (index >= 0 && index < count-1 )
02800       {
02801         // If we get here, then m_t[index] < t < m_t[index+1]
02802         double middle_t = 0.5*(m_t[index] + m_t[index+1]);
02803                           if( t < middle_t && t - m_t[index] <= ktol)
02804         {
02805           // t is a hair bigger than m_t[index]
02806                                         rc = true;
02807                           } 
02808         else if( t > middle_t && m_t[index+1]-t <= ktol)
02809         {
02810           // t is a hair smaller than m_t[index+1]
02811                                         rc = true;
02812                                         index ++;
02813                           }     
02814       }
02815       else if (index == count)
02816       {
02817         // If we get here, then t > m_t[count-1]
02818                                 if( t-m_t[count-1]<=ktol)
02819         {
02820           // t is a hair bigger than m_t[count-1]
02821                                         rc = true;
02822                                         index = count-1;
02823                                 }
02824                         }
02825       else if (index<0)
02826       {
02827         // 22 October 2003 Dale Lear - added this case to match the index==count case above.
02828         // If we get here, then t < m_t[0]
02829                                 if( m_t[0]-t <= ktol)
02830         {
02831           // t is a hair smaller than m_t[count-1]
02832                                         rc = true;
02833                                         index = 0;
02834                                 }
02835                         }
02836                 }
02837         }
02838         return rc;
02839 }
02840 
02841 bool ON_SortLines( 
02842         int line_count, 
02843         const ON_Line* line_list, 
02844         int* index, 
02845         bool* bReverse 
02846         )
02847 {
02848   ON_3dPoint StartP, EndP, Q;
02849   double d, startd, endd;
02850   int Ni, start_i, start_end, end_i, end_end, i, end;
02851 
02852   if ( index ) 
02853   {
02854     for ( i = 0; i < line_count; i++ ) 
02855       index[i] = i;
02856   }
02857   if ( bReverse  )
02858   {
02859     for ( i = 0; i < line_count; i++ ) 
02860       bReverse[i] = false;
02861   }
02862   if ( line_count < 1 || 0 == line_list || 0 == index || 0 == bReverse )
02863   {
02864     ON_ERROR("ON_SortLines - illegal input");
02865     return false;
02866   }
02867   if ( 1 == line_count )
02868   {
02869     return true;
02870   }
02871 
02872   // sort lines
02873   for ( Ni = 1; Ni < line_count; Ni++ ) 
02874   {
02875     /* index[] = some permutation of {0,...,line_count-1}
02876     // N[index[0]], ..., N[index[Ni-1]] are in order
02877     // N[index[j]] needs to be reversed if bReverse[j] is true
02878     */
02879     start_i = end_i = Ni;
02880     start_end = end_end = 0;
02881     StartP = line_list[ index[0]    ][(bReverse[0])    ? 1 : 0];
02882     EndP   = line_list[ index[Ni-1] ][(bReverse[Ni-1]) ? 0 : 1];
02883     startd = StartP.DistanceTo( line_list[index[start_i]].from ); // "from" is correct here
02884     endd   = EndP.DistanceTo( line_list[index[end_i]].from );     // "from" is correct here
02885 
02886     for ( i = Ni; i < line_count; i++ ) 
02887     {
02888       Q = line_list[index[i]].from;
02889       for ( end = 0; end < 2; end++ ) 
02890       {
02891         d = StartP.DistanceTo( Q );
02892         if ( d < startd ) 
02893         {
02894           start_i = i;
02895           start_end = end;
02896           startd = d;
02897         }
02898 
02899         d = EndP.DistanceTo( Q );
02900         if ( d < endd ) 
02901         {
02902           end_i = i;
02903           end_end = end;
02904           endd = d;
02905         }
02906 
02907         Q = line_list[index[i]].to;
02908       }
02909     }
02910 
02911     if ( startd < endd ) 
02912     {
02913       // N[index[start_i]] will be first in list
02914       i = index[Ni];
02915       index[Ni] = index[start_i];
02916       index[start_i] = i;
02917       start_i = index[Ni];
02918       for ( i = Ni; i > 0; i-- ) 
02919       {
02920         index[i] = index[i-1];
02921         bReverse[i] = bReverse[i-1];
02922       }
02923       index[0] = start_i;
02924       bReverse[0] = (start_end != 1);
02925     }
02926     else 
02927     {
02928       // N[index[end_i]] will be next in the list
02929       i = index[Ni];
02930       index[Ni] = index[end_i];
02931       index[end_i] = i;
02932       bReverse[Ni] = (end_end == 1);
02933     }
02934   }
02935 
02936   return true;
02937 }
02938 
02939 
02940 
02941 bool ON_SortLines( 
02942         const ON_SimpleArray<ON_Line>& line_list,
02943         int* index, 
02944         bool* bReverse 
02945         )
02946 {
02947   return ON_SortLines(line_list.Count(),line_list.Array(),index,bReverse);
02948 }
02949 
02950 bool ON_SortCurves( int curve_count, const ON_Curve* const* curve_list, int* index, bool* bReverse )
02951 {
02952   int i;
02953 
02954   if ( curve_count < 1 || 0 == curve_list || 0 == curve_list[0] || 0 == index || 0 == bReverse )
02955   {
02956     if ( index ) 
02957     {
02958       for ( i = 0; i < curve_count; i++ ) 
02959         index[i] = i;
02960     }
02961     if ( bReverse  )
02962     {
02963       for ( i = 0; i < curve_count; i++ ) 
02964         bReverse[i] = false;
02965     }
02966     ON_ERROR("ON_SortCurves - illegal input");
02967     return false;
02968   }
02969 
02970   if ( 1 == curve_count )
02971   {
02972     index[0] = 0;
02973     bReverse[0] = false;
02974     return true;
02975   }
02976 
02977   // get start and end points
02978   ON_SimpleArray< ON_Line > line_list(curve_count);
02979   ON_Interval d;
02980   bool rc = true;
02981   for ( i = 0; i < curve_count; i++ ) 
02982   {
02983     index[i] = i;
02984     bReverse[0] = false;
02985     if ( !rc )
02986       continue;
02987     const ON_Curve* curve = curve_list[i];
02988     if ( !curve )
02989     {
02990       rc = false;
02991       continue;
02992     }
02993     d = curve->Domain();
02994     if ( !d.IsIncreasing() )
02995     {
02996       rc = false;
02997       continue;
02998     }
02999     ON_Line& line = line_list.AppendNew();
03000     if ( !curve->EvPoint(d[0],line.from,1) || !curve->EvPoint(d[1],line.to,-1) )
03001     {
03002       rc = false;
03003     }
03004   }
03005 
03006   if ( !rc )
03007   {
03008     ON_ERROR("ON_SortCurves - illegal input curve");
03009   }
03010   else
03011   {
03012     rc = ON_SortLines( curve_count, line_list, index, bReverse );
03013   }
03014   return rc;
03015 }
03016 
03017 
03018 bool ON_SortCurves( const ON_SimpleArray<const ON_Curve*>& curves, ON_SimpleArray<int>& index, ON_SimpleArray<bool>& bReverse )
03019 {
03020   const int curve_count = curves.Count();
03021   index.Reserve(curve_count);
03022   index.SetCount(curve_count);
03023   bReverse.Reserve(curve_count);
03024   bReverse.SetCount(curve_count);
03025   return ON_SortCurves( curve_count,curves.Array(),index.Array(),bReverse.Array());
03026 }
03027 
03028 bool ON_SortCurves( const ON_SimpleArray<ON_Curve*>& curves, ON_SimpleArray<int>& index, ON_SimpleArray<bool>& bReverse )
03029 {
03030   const int curve_count = curves.Count();
03031   index.Reserve(curve_count);
03032   index.SetCount(curve_count);
03033   bReverse.Reserve(curve_count);
03034   bReverse.SetCount(curve_count);
03035   return ON_SortCurves( curve_count,curves.Array(),index.Array(),bReverse.Array());
03036 }
03037 


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