opennurbs_arccurve.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_OBJECT_IMPLEMENT(ON_ArcCurve,ON_Curve,"CF33BE2A-09B4-11d4-BFFB-0010830122F0");
00020 
00021 
00022 ON_ArcCurve::ON_ArcCurve()
00023 {
00024   m_arc.Create(ON_xy_plane, 1.0, 2.0*ON_PI);
00025   m_t.m_t[0] = 0.0;
00026   m_t.m_t[1] = m_arc.Length();
00027   m_dim = 3;
00028 }
00029 
00030 ON_ArcCurve::ON_ArcCurve( const ON_Arc& A )
00031 {
00032   m_arc = A;
00033   m_t.m_t[0] = 0.0;
00034   m_t.m_t[1] = m_arc.Length();
00035   if ( m_t.m_t[1] <= 0.0 )
00036     m_t.m_t[1] = 1.0;
00037   m_dim = 3;
00038 }
00039 
00040 ON_ArcCurve::ON_ArcCurve( const ON_Circle& circle )
00041 {
00042   ON_ArcCurve::operator=(circle);
00043 }
00044 
00045 ON_ArcCurve::ON_ArcCurve( const ON_Arc& A, double t0, double t1 )
00046 {
00047   m_arc = A;
00048   m_t.m_t[0] = t0;
00049   m_t.m_t[1] = t1;
00050   m_dim = 3;
00051 }
00052 
00053 ON_ArcCurve::ON_ArcCurve( const ON_Circle& circle, double t0, double t1 )
00054 {
00055   m_arc = circle;
00056   m_t.m_t[0] = t0;
00057   m_t.m_t[1] = t1;
00058   m_dim = 3;
00059 }
00060 
00061 ON_ArcCurve::ON_ArcCurve( const ON_ArcCurve& src ) : ON_Curve(src)
00062 {
00063   m_arc = src.m_arc;
00064   m_t = src.m_t;
00065   m_dim  = src.m_dim;
00066 }
00067 
00068 ON_ArcCurve::~ON_ArcCurve()
00069 {
00070 }
00071 
00072 
00073 unsigned int ON_ArcCurve::SizeOf() const
00074 {
00075   unsigned int sz = ON_Curve::SizeOf();
00076   sz += sizeof(*this) - sizeof(ON_Curve);
00077   return sz;
00078 }
00079 
00080 ON__UINT32 ON_ArcCurve::DataCRC(ON__UINT32 current_remainder) const
00081 {
00082   current_remainder = ON_CRC32(current_remainder,sizeof(m_arc),&m_arc);
00083   current_remainder = ON_CRC32(current_remainder,sizeof(m_t),&m_t);
00084   current_remainder = ON_CRC32(current_remainder,sizeof(m_dim),&m_dim);
00085   return current_remainder;
00086 }
00087 
00088 ON_ArcCurve& ON_ArcCurve::operator=( const ON_ArcCurve& src )
00089 {
00090   if ( this != &src ) {
00091     ON_Curve::operator=(src);
00092     m_arc = src.m_arc;
00093     m_t = src.m_t;
00094     m_dim  = src.m_dim;
00095   }
00096   return *this;
00097 }
00098 
00099 ON_ArcCurve& ON_ArcCurve::operator=( const ON_Arc& A )
00100 {
00101   m_arc = A;
00102   m_t.m_t[0] = 0.0;
00103   m_t.m_t[1] = A.Length();
00104   if ( m_t.m_t[1] == 0.0 )
00105     m_t.m_t[1] = 1.0;
00106   m_dim = 3;
00107   return *this;
00108 }
00109 
00110 ON_ArcCurve& ON_ArcCurve::operator=(const ON_Circle& circle)
00111 {
00112   m_arc = circle;
00113   m_t.m_t[0] = 0.0;
00114   m_t.m_t[1] = m_arc.Length();
00115   if ( m_t.m_t[1] <= 0.0 )
00116     m_t.m_t[1] = 1.0;
00117   m_dim = 3;
00118   return *this;
00119 }
00120 
00121 int ON_ArcCurve::Dimension() const
00122 {
00123   return m_dim;
00124 }
00125 
00126 ON_BOOL32 
00127 ON_ArcCurve::GetBBox( // returns true if successful
00128          double* boxmin,    // minimum
00129          double* boxmax,    // maximum
00130          ON_BOOL32 bGrowBox
00131          ) const
00132 {
00133   ON_BOOL32 rc = m_arc.IsValid();
00134   if (rc) {
00135     ON_BoundingBox bbox = m_arc.BoundingBox();
00136     if ( bGrowBox ) {
00137       if ( boxmin[0] > bbox.m_min.x ) boxmin[0] = bbox.m_min.x;
00138       if ( boxmin[1] > bbox.m_min.y ) boxmin[1] = bbox.m_min.y;
00139       if ( boxmax[0] < bbox.m_max.x ) boxmax[0] = bbox.m_max.x;
00140       if ( boxmax[1] < bbox.m_max.y ) boxmax[1] = bbox.m_max.y;
00141       if ( m_dim > 2 ) {
00142         if ( boxmin[2] > bbox.m_min.z ) boxmin[2] = bbox.m_min.z;
00143         if ( boxmax[2] < bbox.m_max.z ) boxmax[2] = bbox.m_max.z;
00144       }
00145     }
00146     else {
00147       boxmin[0] = bbox.m_min.x;
00148       boxmin[1] = bbox.m_min.y;
00149       boxmax[0] = bbox.m_max.x;
00150       boxmax[1] = bbox.m_max.y;
00151       if ( m_dim > 2 ) {
00152         boxmin[2] = bbox.m_min.z;
00153         boxmax[2] = bbox.m_max.z;
00154       }
00155     }
00156   }
00157   return rc;
00158 }
00159 
00160 ON_BOOL32
00161 ON_ArcCurve::Transform( const ON_Xform& xform )
00162 {
00163   TransformUserData(xform);
00164         DestroyCurveTree();
00165   return m_arc.Transform( xform );
00166 }
00167 
00168 ON_BOOL32 ON_ArcCurve::IsValid( ON_TextLog* text_log ) const
00169 {
00170   if ( !m_t.IsIncreasing() )
00171   {
00172     if ( 0 != text_log )
00173       text_log->Print("ON_ArcCurve - m_t=(%g,%g) - it should be an increasing interval.\n",m_t[0],m_t[1]);
00174     return false;
00175   }
00176 
00177   if ( !m_arc.IsValid() )
00178   {
00179     if ( 0 != text_log )
00180       text_log->Print("ON_ArcCurve m_arc is not valid\n");
00181     return false;
00182   }
00183 
00184   return true;
00185 }
00186 
00187 void ON_ArcCurve::Dump( ON_TextLog& dump ) const
00188 {
00189   dump.Print( "ON_ArcCurve:  domain = [%g,%g]\n",m_t[0],m_t[1]);
00190   dump.PushIndent();
00191   dump.Print( "center = ");
00192   dump.Print( m_arc.plane.origin );
00193   dump.Print( "\nradius = %g\n",m_arc.radius);
00194   dump.Print( "length = %g\n",m_arc.Length());
00195   ON_3dPoint start = PointAtStart();
00196   ON_3dPoint end = PointAtEnd();
00197   dump.Print( "start = "); dump.Print(start);
00198   dump.Print( "\nend = "); dump.Print(end); dump.Print("\n");
00199   dump.PopIndent();
00200 }
00201 
00202 ON_BOOL32 ON_ArcCurve::Write(
00203        ON_BinaryArchive& file // open binary file
00204      ) const
00205 {
00206   ON_BOOL32 rc = file.Write3dmChunkVersion(1,0);
00207   if (rc) 
00208   {
00209     rc = file.WriteArc( m_arc );
00210     if (rc) rc = file.WriteInterval( m_t );
00211     if (rc) rc = file.WriteInt(m_dim);
00212   }
00213   return rc;
00214 }
00215 
00216 ON_BOOL32 ON_ArcCurve::Read(
00217        ON_BinaryArchive& file // open binary file
00218      )
00219 {
00220   int major_version = 0;
00221   int minor_version = 0;
00222   ON_BOOL32 rc = file.Read3dmChunkVersion(&major_version,&minor_version);
00223   if (rc)
00224   {
00225     if (major_version==1) 
00226     {
00227       // common to all 1.x  versions
00228       rc = file.ReadArc( m_arc );
00229       if (rc) 
00230         rc = file.ReadInterval( m_t );
00231       if (rc) 
00232         rc = file.ReadInt(&m_dim);
00233       if ( m_dim != 2 && m_dim != 3 )
00234         m_dim = 3;
00235     }
00236     else
00237       rc = 0;
00238   }
00239   return rc;
00240 }
00241 
00242 
00243 ON_BOOL32 ON_ArcCurve::SetDomain( double t0, double t1 )
00244 {
00245   ON_BOOL32 rc = false;
00246   if ( t0 < t1 )
00247   {
00248     m_t.Set(t0,t1);
00249     rc = true;
00250   }
00251         DestroyCurveTree();
00252   return rc;
00253 }
00254 
00255 
00256 bool ON_ArcCurve::ChangeDimension( int desired_dimension )
00257 {
00258   bool rc = (desired_dimension>=2 && desired_dimension<=3);
00259   if ( rc && m_dim != desired_dimension )
00260   {
00261         DestroyCurveTree();
00262     if ( desired_dimension == 2 )
00263       m_dim = 2;
00264     else
00265       m_dim = 3;
00266   }
00267   return rc;
00268 }
00269 
00270 
00271 ON_Interval ON_ArcCurve::Domain() const
00272 {
00273   return m_t;
00274 }
00275 
00276 ON_BOOL32 ON_ArcCurve::ChangeClosedCurveSeam( 
00277             double t ){
00278         bool rc = false;
00279         if( IsCircle() ){
00280                 double angle_delta = m_t.NormalizedParameterAt(t);
00281                 angle_delta*= 2*ON_PI;
00282                 
00283                 m_arc.Rotate(angle_delta, m_arc.plane.Normal());
00284                 m_t = ON_Interval( t, m_t[1] + t - m_t[0]);
00285                 rc = true;
00286         }
00287         return rc;
00288 }
00289 
00290 
00291 int ON_ArcCurve::SpanCount() const
00292 {
00293   return 1;
00294 }
00295 
00296 ON_BOOL32 ON_ArcCurve::GetSpanVector( double* s ) const
00297 {
00298   s[0] = m_t[0];
00299   s[1] = m_t[1];
00300   return m_t.IsIncreasing();
00301 }
00302 
00303 int ON_ArcCurve::Degree() const
00304 {
00305   return 2;
00306 }
00307 
00308 
00309 ON_BOOL32
00310 ON_ArcCurve::IsLinear(  // true if curve locus is a line segment
00311       double // tolerance - formal parameter intentionally ignored in this virtual function
00312       ) const
00313 {
00314   return false;
00315 }
00316 
00317 ON_BOOL32
00318 ON_ArcCurve::IsArc( // true if curve locus in an arc or circle
00319       const ON_Plane* plane, // if not NULL, test is performed in this plane
00320       ON_Arc* arc,         // if not NULL and true is returned, then arc
00321                               // arc parameters are filled in
00322       double tolerance // tolerance to use when checking linearity
00323       ) const
00324 {
00325   ON_BOOL32 rc = (plane) ? IsInPlane(*plane,tolerance) : true;
00326   if (arc) 
00327     *arc = m_arc;
00328   if (rc)
00329     rc = IsValid();
00330   return rc;
00331 }
00332 
00333 ON_BOOL32
00334 ON_ArcCurve::IsPlanar(
00335       ON_Plane* plane, // if not NULL and true is returned, then plane parameters
00336                          // are filled in
00337       double tolerance // tolerance to use when checking linearity
00338       ) const
00339 {
00340   if ( m_dim == 2 )
00341   {
00342     return ON_Curve::IsPlanar(plane,tolerance);
00343   }
00344   
00345   if ( plane ) 
00346     *plane = m_arc.plane;
00347 
00348   return true;
00349 }
00350 
00351 ON_BOOL32
00352 ON_ArcCurve::IsInPlane(
00353       const ON_Plane& plane, // plane to test
00354       double tolerance // tolerance to use when checking linearity
00355       ) const
00356 {
00357   return m_arc.IsInPlane( plane, tolerance );
00358 }
00359 
00360 ON_BOOL32 
00361 ON_ArcCurve::IsClosed() const
00362 {
00363   return m_arc.IsCircle();
00364 }
00365 
00366 ON_BOOL32 
00367 ON_ArcCurve::IsPeriodic() const
00368 {
00369   return m_arc.IsCircle();
00370 }
00371 
00372 ON_BOOL32
00373 ON_ArcCurve::Reverse()
00374 {
00375   ON_BOOL32 rc = m_arc.Reverse();
00376   if (rc)
00377         {
00378     m_t.Reverse();
00379                 DestroyCurveTree();
00380         }       
00381   return true;
00382 }
00383 
00384 
00385 ON_BOOL32 ON_ArcCurve::SetStartPoint(ON_3dPoint start_point)
00386 {
00387   if (IsCircle())
00388     return false;
00389   ON_BOOL32 rc = false;
00390   if ( m_dim == 3 || start_point.z == 0.0 )
00391   {
00392     ON_3dPoint P;
00393     ON_3dVector T;
00394     double t = Domain()[1];
00395     Ev1Der( t, P, T );
00396     T.Reverse();
00397     ON_Arc a;
00398     rc = a.Create( P, T, start_point );
00399     if ( rc )
00400     {
00401       a.Reverse();
00402       m_arc = a;
00403     }
00404     else {
00405       ON_3dPoint end_point = PointAt(Domain()[1]);
00406       if (end_point.DistanceTo(start_point) < ON_ZERO_TOLERANCE*m_arc.Radius()){
00407         //make arc into circle
00408         m_arc.plane.xaxis = end_point - m_arc.Center();
00409         m_arc.plane.xaxis.Unitize();
00410         m_arc.plane.yaxis = ON_CrossProduct(m_arc.Normal(), m_arc.plane.xaxis);
00411         m_arc.plane.yaxis.Unitize();
00412         m_arc.SetAngleRadians(2.0*ON_PI);
00413         rc = true;
00414       }
00415     }
00416   }
00417         DestroyCurveTree();
00418   return rc;  
00419 }
00420 
00421 
00422 ON_BOOL32 ON_ArcCurve::SetEndPoint(ON_3dPoint end_point)
00423 {
00424   if (IsCircle())
00425     return false;
00426   ON_BOOL32 rc = false;
00427   if ( m_dim == 3 || end_point.z == 0.0 )
00428   {
00429     ON_3dPoint P;
00430     ON_3dVector T;
00431     double t = Domain()[0];
00432     Ev1Der( t, P, T );
00433     ON_Arc a;
00434     rc = a.Create( P, T, end_point );
00435     if ( rc )
00436     {
00437       m_arc = a;
00438     }
00439     else {
00440       ON_3dPoint start_point = PointAt(Domain()[0]);
00441       if (end_point.DistanceTo(start_point) < ON_ZERO_TOLERANCE*m_arc.Radius()){
00442         //make arc into circle
00443         m_arc.plane.xaxis = start_point - m_arc.Center();
00444         m_arc.plane.xaxis.Unitize();
00445         m_arc.plane.yaxis = ON_CrossProduct(m_arc.Normal(), m_arc.plane.xaxis);
00446         m_arc.plane.yaxis.Unitize();
00447         m_arc.SetAngleRadians(2.0*ON_PI);
00448         rc = true;
00449       }
00450     }
00451   }
00452         DestroyCurveTree();
00453   return rc;  
00454 }
00455 
00456 ON_BOOL32 ON_ArcCurve::Evaluate( // returns false if unable to evaluate
00457        double t,       // evaluation parameter
00458        int der_count,  // number of derivatives (>=0)
00459        int v_stride,   // v[] array stride (>=Dimension())
00460        double* v,      // v[] array of length stride*(ndir+1)
00461        int, // side - formal parameter intentionally ignored in this virtual function
00462        int* // hint - formal parameter intentionally ignored in this virtual function
00463        ) const
00464 {
00465   ON_3dVector d;
00466   ON_BOOL32 rc = false;
00467   if ( m_t[0] < m_t[1] ) 
00468   {
00469     double rat = m_arc.DomainRadians().Length()/m_t.Length();
00470     double scale = 1.0;
00471     double a = m_arc.DomainRadians().ParameterAt( m_t.NormalizedParameterAt(t) );
00472 
00473     // 12 July 2012 Dale Lear
00474     //   When making a sphere with center (0,0,0) and radius = 1.0e9,
00475     //   a = ON_PI = 3.1415926535897931, c = -1.0 and s = 1.2246467991473532e-016
00476     //   so I'm adding the if ... statements to keep arc evaluations more precise
00477     //   at multiples of 1/2 pi.
00478     double c = cos(a);
00479     double s = sin(a);
00480     if ( fabs(c) < ON_EPSILON || fabs(s) > 1.0-ON_EPSILON )
00481     {
00482       c = 0.0;
00483       s = s < 0.0 ? -1.0 : 1.0;
00484     }
00485     else if ( fabs(s) < ON_EPSILON || fabs(c) > 1.0-ON_EPSILON )
00486     {
00487       s = 0.0;
00488       c = c < 0.0 ? -1.0 : 1.0;
00489     }
00490 
00491     c *= m_arc.radius;
00492     s *= m_arc.radius;
00493 
00494     ON_3dPoint p = m_arc.plane.origin + c*m_arc.plane.xaxis + s*m_arc.plane.yaxis;
00495     v[0] = p.x;
00496     v[1] = p.y;
00497     if ( m_dim == 3 )
00498       v[2] = p.z;
00499     for ( int di = 1; di <= der_count; di++ ) {
00500       scale*=rat;
00501       a =  c;
00502       c = -s;
00503       s =  a;
00504       d =  c*m_arc.plane.xaxis + s*m_arc.plane.yaxis;
00505       v += v_stride;
00506       v[0] = d.x*scale;
00507       v[1] = d.y*scale;
00508       if ( m_dim == 3 )
00509         v[2] = d.z*scale;
00510     }
00511     rc = true;
00512   }
00513   return rc;
00514 }
00515 
00516 ON_BOOL32 ON_ArcCurve::Trim( const ON_Interval& in )
00517 {
00518   ON_BOOL32 rc = in.IsIncreasing();
00519   if (rc) 
00520   {
00521     double t0 = m_t.NormalizedParameterAt(in.m_t[0]);
00522     double t1 = m_t.NormalizedParameterAt(in.m_t[1]);
00523     const ON_Interval arc_angle0 = m_arc.DomainRadians();
00524     double a0 = arc_angle0.ParameterAt(t0);
00525     double a1 = arc_angle0.ParameterAt(t1);
00526                 // Resulting ON_Arc must pass IsValid()
00527     if ( a1 - a0 > ON_ZERO_TOLERANCE && m_arc.SetAngleIntervalRadians(ON_Interval(a0,a1)) ) 
00528     {
00529       m_t = in;
00530     }
00531     else
00532     {
00533       rc = false;
00534     }
00535     DestroyCurveTree();
00536   }
00537   return rc;
00538 }
00539 
00540 bool ON_ArcCurve::Extend(
00541   const ON_Interval& domain
00542   )
00543 
00544 {
00545   if (IsClosed()) return false;
00546 
00547   double s0, s1;
00548   bool changed = false;
00549   GetDomain(&s0, &s1);
00550   if (domain[0] < s0){
00551     s0 = domain[0];
00552     changed = true;
00553   }
00554   if (domain[1] > s1){
00555     s1 = domain[1];
00556     changed = true;
00557   }
00558   if (!changed) return false;
00559 
00560   DestroyCurveTree();
00561 
00562   double a0 = m_arc.Domain().ParameterAt(Domain().NormalizedParameterAt(s0));
00563   double a1 = m_arc.Domain().ParameterAt(Domain().NormalizedParameterAt(s1));
00564   if (a1 > a0+2.0*ON_PI) {
00565     a1 = a0+2.0*ON_PI;
00566     s1 = Domain().ParameterAt(m_arc.Domain().NormalizedParameterAt(a1));
00567   }
00568   m_arc.Trim(ON_Interval(a0, a1));
00569   SetDomain(s0, s1);
00570   return true;
00571 }
00572 
00573 ON_BOOL32 ON_ArcCurve::Split(
00574     double t,
00575     ON_Curve*& left_side,
00576     ON_Curve*& right_side
00577   ) const
00578 {
00579   // make sure t is strictly inside the arc's domain
00580   ON_Interval arc_domain = Domain();
00581   ON_Interval arc_angles = m_arc.DomainRadians();
00582   if ( !arc_domain.Includes(t) )
00583     return false;
00584   double a = (arc_domain == arc_angles)
00585            ? t
00586            : arc_angles.ParameterAt(arc_domain.NormalizedParameterAt(t));
00587   if ( !arc_angles.Includes(a) )
00588     return false;
00589 
00590   // make sure input curves are ok.
00591   ON_ArcCurve* left_arc = 0;
00592   ON_ArcCurve* right_arc = 0;
00593 
00594   if ( 0 != left_side )
00595   {
00596     if ( left_side == right_side )
00597       return false;
00598     left_arc = ON_ArcCurve::Cast(left_side);
00599     if ( 0 == left_arc )
00600       return false;
00601     left_arc->DestroyCurveTree();
00602   }
00603 
00604   if ( 0 != right_side )
00605   {
00606     right_arc = ON_ArcCurve::Cast(right_side);
00607     if ( 0 == right_arc )
00608       return false;
00609     right_arc->DestroyCurveTree();
00610   }
00611 
00612   if ( 0 == left_arc )
00613   {
00614     left_arc = new ON_ArcCurve( *this );
00615   }
00616   else if ( this != left_arc )
00617   {
00618     left_arc->operator=(*this);
00619   }
00620 
00621   if ( 0 == right_arc )
00622   {
00623     right_arc = new ON_ArcCurve( *this );
00624   }
00625   else if ( this != right_arc )
00626   {
00627     right_arc->operator=(*this);
00628   }
00629 
00630   ON_BOOL32 rc = false;
00631   if ( this != left_arc )
00632   {
00633     rc = left_arc->Trim( ON_Interval( arc_domain[0], t ) );
00634     if (rc)
00635      rc = right_arc->Trim( ON_Interval( t, arc_domain[1] ) );
00636   }
00637   else
00638   {
00639     rc = right_arc->Trim( ON_Interval( t, arc_domain[1] ) );
00640     if (rc)
00641       rc = left_arc->Trim( ON_Interval( arc_domain[0], t ) );
00642   }
00643 
00644   if ( rc )
00645   {
00646     if ( 0 == left_side )
00647       left_side = left_arc;
00648     if ( 0 == right_side )
00649       right_side = right_arc;
00650   }
00651   else
00652   {
00653     if ( 0 == left_side && this != left_arc )
00654     {
00655       delete left_arc;
00656       left_arc = 0;
00657     }
00658     if ( 0 == right_side && this != right_arc )
00659     {
00660       delete right_arc;
00661       right_arc = 0;
00662     }
00663   }
00664   return rc;
00665 }
00666 
00667 
00668 
00669 
00670 static double ArcDeFuzz( double d )
00671 {
00672   // 0.0078125 = 1.0/128.0 exactly
00673   // Using 2^n scale factors insures no loss of precision
00674   // but preserves fractional values that are multiples of 1/128.
00675   //
00676   // Fuzz tol should be scale * 2^m * ON_EPSILON for m >= 1
00677 
00678   double f, i;
00679   f = modf( d*128.0, &i );
00680   if ( f != 0.0 && fabs(f) <= 1024.0*ON_EPSILON ) {
00681     d = i*0.0078125;
00682   }  
00683   return d;
00684 }
00685 
00686 static ON_BOOL32 NurbsCurveArc ( const ON_Arc& arc, int dim, ON_NurbsCurve& nurb )
00687 { 
00688   if ( !arc.IsValid() )
00689     return false;
00690   // makes a quadratic nurbs arc
00691   const ON_3dPoint center = arc.Center();
00692   double angle = arc.AngleRadians();
00693   ON_Interval dom = arc.DomainRadians();
00694   const double angle0 = dom[0];
00695   const double angle1 = dom[1];
00696   ON_3dPoint start_point = arc.StartPoint();
00697   //ON_3dPoint mid_point   = arc.PointAt(angle0 + 0.5*angle);
00698   ON_3dPoint end_point   = arc.IsCircle() ? start_point : arc.EndPoint();
00699 
00700   ON_4dPoint CV[9];
00701   double knot[10];
00702 
00703         double a, b, c, w, winv;
00704         double *cv;
00705         int    j, span_count, cv_count;
00706 
00707         a = (0.5 + ON_SQRT_EPSILON)*ON_PI;
00708 
00709         if (angle <= a)
00710                 span_count = 1;
00711         else if (angle <= 2.0*a)
00712                 span_count = 2;
00713         else if (angle <= 3.0*a)
00714                 span_count = 4; // TODO - make a 3 span case
00715         else
00716                 span_count = 4;
00717 
00718         cv_count = 2*span_count + 1;
00719         
00720         switch(span_count) {
00721         case 1:
00722     CV[0] = start_point;
00723     CV[1] = arc.PointAt(angle0 + 0.50*angle);
00724     CV[2] = end_point;
00725                 break;
00726         case 2:
00727     CV[0] = start_point;
00728     CV[1] = arc.PointAt(angle0 + 0.25*angle);
00729     CV[2] = arc.PointAt(angle0 + 0.50*angle);
00730     CV[3] = arc.PointAt(angle0 + 0.75*angle);
00731     CV[4] = end_point;
00732                 angle *= 0.5;
00733                 break;
00734         default: // 4 spans
00735     CV[0] = start_point;
00736     CV[1] = arc.PointAt(angle0 + 0.125*angle);
00737     CV[2] = arc.PointAt(angle0 + 0.250*angle);
00738     CV[3] = arc.PointAt(angle0 + 0.375*angle);
00739     CV[4] = arc.PointAt(angle0 + 0.500*angle);
00740     CV[5] = arc.PointAt(angle0 + 0.625*angle);
00741     CV[6] = arc.PointAt(angle0 + 0.750*angle);
00742     CV[7] = arc.PointAt(angle0 + 0.875*angle);
00743     CV[8] = end_point;
00744                 angle *= 0.25;
00745                 break;
00746         }
00747 
00748         a = cos(0.5*angle);
00749         b = a - 1.0;
00750         //c = (radius > 0.0) ? radius*angle : angle;
00751   c = angle;
00752 
00753         span_count *= 2;
00754         knot[0] = knot[1] = angle0; //0.0;
00755         for (j = 1; j < span_count; j += 2) {
00756     CV[j].x += b * center.x;
00757     CV[j].y += b * center.y;
00758     CV[j].z += b * center.z;
00759     CV[j].w = a;
00760                 CV[j+1].w = 1.0;
00761                 knot[j+1] = knot[j+2] = knot[j-1] + c;
00762         }
00763   knot[cv_count-1] = knot[cv_count] = angle1;
00764   for ( j = 1; j < span_count; j += 2 ) {
00765     w = CV[j].w;
00766     winv = 1.0/w;
00767     a = CV[j].x*winv;
00768     b = ArcDeFuzz(a);
00769     if ( a != b ) {
00770       CV[j].x = b*w;
00771     }
00772     a = CV[j].y*winv;
00773     b = ArcDeFuzz(a);
00774     if ( a != b ) {
00775       CV[j].y = b*w;
00776     }
00777     a = CV[j].z*winv;
00778     b = ArcDeFuzz(a);
00779     if ( a != b ) {
00780       CV[j].z = b*w;
00781     }
00782   }
00783 
00784   nurb.m_dim = (dim==2) ? 2 : 3;
00785   nurb.m_is_rat = 1;
00786   nurb.m_order = 3;
00787   nurb.m_cv_count = cv_count;
00788   nurb.m_cv_stride = (dim==2 ? 3 : 4);
00789   nurb.ReserveCVCapacity( nurb.m_cv_stride*cv_count );
00790   nurb.ReserveKnotCapacity( cv_count+1 );
00791   for ( j = 0; j < cv_count; j++ ) {
00792     cv = nurb.CV(j);
00793     cv[0] = CV[j].x;
00794     cv[1] = CV[j].y;
00795     if ( dim == 2 ) {
00796       cv[2] = CV[j].w;
00797     }
00798     else {
00799       cv[2] = CV[j].z;
00800       cv[3] = CV[j].w;
00801     }
00802     nurb.m_knot[j] = knot[j];
00803   }
00804   nurb.m_knot[cv_count] = knot[cv_count];
00805   return true;
00806 }
00807 
00808 
00809 int ON_Arc::GetNurbForm( ON_NurbsCurve& nurbscurve ) const
00810 
00811 {
00812   ON_BOOL32 rc = NurbsCurveArc ( *this, 3, nurbscurve );
00813   return (rc) ? 2 : 0;
00814 }
00815 
00816 bool ON_Arc::GetRadianFromNurbFormParameter(double NurbParameter, double* RadianParameter  ) const
00817 {
00818         //  TRR#53994.
00819         // 16-Sept-09  Replaced this code so we dont use LocalClosestPoint.
00820         // In addition to being slower than neccessary the old method suffered from getting the
00821         // wrong answer at the seam of a full circle,  This probably only happened with large 
00822         // coordinates where many digits of precision get lost.
00823 
00824         ON_NurbsCurve crv;
00825         
00826         if( !IsValid()|| RadianParameter==NULL) 
00827                 return false;
00828 
00829         ON_Interval dom= Domain();
00830 
00831         if( fabs(NurbParameter- dom[0])<=2.0*ON_EPSILON*fabs(dom[0]))
00832         {
00833                 *RadianParameter=dom[0];
00834                 return true;
00835         } 
00836         else if(  fabs(NurbParameter- dom[1])<=2.0*ON_EPSILON*fabs(dom[1]))
00837         {
00838                 *RadianParameter=dom[1];
00839                 return true;
00840         }
00841 
00842         if( !dom.Includes(NurbParameter) )
00843                 return false;
00844 
00845         if( !GetNurbForm(crv) )
00846                 return false;
00847                 
00848         ON_3dPoint cp;
00849         cp = crv.PointAt(NurbParameter);
00850         cp -= Center();
00851 
00852         double x = ON_DotProduct(Plane().Xaxis(), cp);
00853         double y = ON_DotProduct(Plane().Yaxis(), cp);
00854         double theta = atan2(y,x);
00855 
00856         theta -= floor( (theta-dom[0])/(2*ON_PI)) * 2* ON_PI;
00857         if( theta<dom[0] || theta>dom[1])
00858         {
00859                 // 24-May-2010 GBA 
00860                 // We got outside of the domain because of a numerical error somewhere.
00861                 // The only case that matters is because we are right near an endpoint.
00862                 // So we need to decide which endpoint to return.  (Other possibilities 
00863                 // are that the radius is way to small relative to the coordinates of the center.
00864                 // In this case the circle is just numerical noise around the center anyway.)
00865                 if( NurbParameter< (dom[0]+dom[1])/2.0)
00866                         theta = dom[0];
00867                 else 
00868                         theta = dom[1];
00869         }
00870 
00871 
00872         // Carefully handle the potential discontinuity of this function
00873         //  when the domain is a full circle
00874         if(dom.Length()>.99999*2.0*ON_PI)
00875         {
00876                 double np_theta = dom.NormalizedParameterAt(theta);
00877                 double np_nurb = dom.NormalizedParameterAt(NurbParameter);
00878                 if( np_nurb<.01 && np_theta>.99)
00879                         theta = dom[0];
00880                 else if( np_nurb>.99 && np_theta<.01)
00881                         theta = dom[1];
00882         }
00883 
00884         *RadianParameter = theta;
00885 
00886 //#if defined(ON_DEBUG)
00887 //      double np2;
00888 //      ON_3dPoint AP = PointAt(*RadianParameter);
00889 //
00890 //      GetNurbFormParameterFromRadian( *RadianParameter, &np2);
00891 //      ON_ASSERT(fabs(np2-NurbParameter)<=100* ON_EPSILON*( fabs(NurbParameter) + AP.MaximumCoordinate()+1.0) ); 
00892 //#endif
00893 
00894         return true;
00895   
00896 }
00897 
00898 
00899 bool ON_Arc::GetNurbFormParameterFromRadian(double RadianParameter, double* NurbParameter ) const
00900 {
00901         if(!IsValid() || NurbParameter==NULL) 
00902                 return false;
00903 
00904   ON_Interval ADomain = DomainRadians();
00905 
00906   double endtol = 10.0*ON_EPSILON*(fabs(ADomain[0]) + fabs(ADomain[1]));
00907 
00908   double del = RadianParameter - ADomain[0];
00909         if(del <= endtol && del >= -ON_SQRT_EPSILON)
00910   {
00911                 *NurbParameter=ADomain[0];
00912                 return true;
00913         } 
00914   else {
00915     del = ADomain[1] - RadianParameter;
00916     if(del <= endtol && del >= -ON_SQRT_EPSILON){
00917                   *NurbParameter=ADomain[1];
00918                   return true;
00919     }
00920         }
00921 
00922         if( !ADomain.Includes(RadianParameter ) )
00923                 return false;
00924 
00925 
00926         ON_NurbsCurve crv;
00927 
00928         if( !GetNurbForm(crv))
00929                 return false;
00930 
00931         //Isolate a bezier that contains the solution
00932         int cnt = crv.SpanCount();      
00933         int si =0;      //get span index
00934         int ki=0;               //knot index
00935         double ang = ADomain[0];
00936         ON_3dPoint cp;
00937         cp = crv.PointAt( crv.Knot(0) ) - Center();
00938         double x = ON_DotProduct(Plane().Xaxis(),cp);
00939         double y = ON_DotProduct(Plane().Yaxis(),cp);
00940         double at = atan2( y, x);       //todo make sure we dont go to far
00941 
00942         for( si=0, ki=0; si<cnt; si++, ki+=crv.KnotMultiplicity(ki) ){
00943                 cp = crv.PointAt( crv.Knot(ki+2)) - Center();
00944                 x = ON_DotProduct(Plane().Xaxis(),cp);
00945                 y = ON_DotProduct(Plane().Yaxis(),cp);
00946                 double at2 = atan2(y,x);
00947                 if(at2>at)
00948                         ang+=(at2-at);
00949                 else
00950                         ang += (2*ON_PI + at2 - at);
00951                 at = at2;
00952                 if( ang>RadianParameter)
00953                         break;
00954         } 
00955 
00956         // Crash Protection trr#55679
00957         if( ki+2>= crv.KnotCount())
00958         {
00959                  *NurbParameter=ADomain[1];
00960                  return true;           
00961         }
00962         ON_Interval BezDomain(crv.Knot(ki), crv.Knot(ki+2));
00963 
00964         ON_BezierCurve bez;
00965         if(!crv.ConvertSpanToBezier(ki,bez))
00966                 return false;
00967 
00968         ON_Xform COC;
00969         COC.ChangeBasis( ON_Plane(),Plane());   
00970 
00971         
00972         bez.Transform(COC);     // change coordinates to circles local frame
00973         double a[3];                                                    // Bez coefficients of a quadratic to solve
00974         for(int i=0; i<3; i++)
00975                 a[i] = tan(RadianParameter)* bez.CV(i)[0] - bez.CV(i)[1];
00976 
00977         //Solve the Quadratic
00978         double descrim = (a[1]*a[1]) - a[0]*a[2];
00979         double squared = a[0]-2*a[1]+a[2];
00980         double tbez;
00981         if(fabs(squared)> ON_ZERO_TOLERANCE){
00982                 ON_ASSERT(descrim>=0);
00983                 descrim = sqrt(descrim);
00984                 tbez = (a[0]-a[1] + descrim)/(a[0]-2*a[1]+a[2]);
00985                 if( tbez<0 || tbez>1){
00986                         double tbez2 = (a[0]-a[1]-descrim)/(a[0] - 2*a[1] + a[2]);
00987                         if( fabs(tbez2 - .5)<fabs(tbez-.5) )
00988                                 tbez = tbez2;
00989                 }
00990 
00991                 ON_ASSERT(tbez>=-ON_ZERO_TOLERANCE && tbez<=1+ON_ZERO_TOLERANCE);
00992         }
00993         else{
00994                 // Quadratic degenerates to linear
00995                 tbez = 1.0;
00996                 if(a[0]-a[2])
00997                         tbez = a[0]/(a[0]-a[2]);
00998         }       
00999         if(tbez<0)
01000                 tbez=0.0;
01001         else if(tbez>1.0)
01002                 tbez=1.0;
01003 
01004 
01005                 //Debug ONLY Code  - check the result
01006 //              double aa = a[0]*(1-tbez)*(1-tbez)  + 2*a[1]*tbez*(1-tbez) + a[2]*tbez*tbez;
01007 //              double tantheta= tan(RadianParameter);
01008 //              ON_3dPoint bezp;
01009 //              bez.Evaluate(tbez, 0, 3, bezp);
01010 //              double yx = bezp.y/bezp.x;
01011 
01012 
01013         *NurbParameter = BezDomain.ParameterAt(tbez);
01014         return true;
01015 
01016 }
01017 
01018 int ON_ArcCurve::GetNurbForm( // returns 0: unable to create NURBS representation
01019                  //            with desired accuracy.
01020                  //         1: success - returned NURBS parameterization
01021                  //            matches the curve's to wthe desired accuracy
01022                  //         2: success - returned NURBS point locus matches
01023                  //            the curve's to the desired accuracy but, on
01024                  //            the interior of the curve's domain, the 
01025                  //            curve's parameterization and the NURBS
01026                  //            parameterization may not match to the 
01027                  //            desired accuracy.
01028       ON_NurbsCurve& c,
01029       double tolerance,
01030       const ON_Interval* subdomain  // OPTIONAL subdomain of arc
01031       ) const
01032 {
01033   int rc = 0;
01034   if ( subdomain ) 
01035   {
01036     ON_ArcCurve trimmed_arc(*this);
01037     if ( trimmed_arc.Trim(*subdomain) ) 
01038     {
01039       rc = trimmed_arc.GetNurbForm( c, tolerance, NULL );
01040     }
01041   }
01042   else if ( m_t.IsIncreasing() && m_arc.IsValid() ) 
01043   {
01044     if ( NurbsCurveArc( m_arc, m_dim, c ) )
01045     {
01046       rc = 2;
01047       c.SetDomain( m_t[0], m_t[1] );
01048     }
01049   }
01050   return rc;
01051 }
01052 
01053 int ON_ArcCurve::HasNurbForm( // returns 0: unable to create NURBS representation
01054                  //            with desired accuracy.
01055                  //         1: success - returned NURBS parameterization
01056                  //            matches the curve's to wthe desired accuracy
01057                  //         2: success - returned NURBS point locus matches
01058                  //            the curve's to the desired accuracy but, on
01059                  //            the interior of the curve's domain, the 
01060                  //            curve's parameterization and the NURBS
01061                  //            parameterization may not match to the 
01062                  //            desired accuracy.
01063                  ) const
01064 
01065 {
01066   if (!IsValid())
01067     return 0;
01068   return 2;
01069 }
01070 
01071 ON_BOOL32 ON_ArcCurve::GetCurveParameterFromNurbFormParameter(
01072       double nurbs_t,
01073       double* curve_t
01074       ) const
01075 {
01076   double radians;
01077 
01078   double arcnurb_t = m_arc.DomainRadians().ParameterAt(m_t.NormalizedParameterAt(nurbs_t));
01079 
01080   ON_BOOL32 rc = m_arc.GetRadianFromNurbFormParameter(arcnurb_t,&radians);
01081   *curve_t = m_t.ParameterAt( m_arc.DomainRadians().NormalizedParameterAt(radians) );
01082   
01083   return rc;
01084 }
01085 
01086 ON_BOOL32 ON_ArcCurve::GetNurbFormParameterFromCurveParameter(
01087       double curve_t,
01088       double* nurbs_t
01089       ) const
01090 {
01091   double radians = m_arc.DomainRadians().ParameterAt(m_t.NormalizedParameterAt(curve_t));
01092   double arcnurb_t;
01093   ON_BOOL32 rc = m_arc.GetNurbFormParameterFromRadian(radians,&arcnurb_t);
01094   if (rc)        // Oct 29, 2009 - Dale Lear added condition to set *nurbs_t = curve_t
01095     *nurbs_t = m_t.ParameterAt(m_arc.DomainRadians().NormalizedParameterAt(arcnurb_t));
01096   else
01097     *nurbs_t = curve_t;
01098   return rc;
01099 }
01100 
01101 bool ON_ArcCurve::IsCircle() const
01102 {
01103   return m_arc.IsCircle() ? true : false;
01104 }
01105 
01106 double ON_ArcCurve::Radius() const
01107 {
01108         return m_arc.Radius();
01109 }
01110   
01111 double ON_ArcCurve::AngleRadians() const
01112 {
01113         return m_arc.AngleRadians();
01114 }
01115 
01116 double ON_ArcCurve::AngleDegrees() const
01117 {
01118         return m_arc.AngleDegrees();
01119 }
01120 
01121 /*
01122 Description:
01123   ON_CircleCurve is obsolete.  
01124   This code exists so v2 files can be read.
01125 */
01126 class ON__OBSOLETE__CircleCurve : public ON_ArcCurve
01127 {
01128 public: 
01129   static const ON_ClassId m_ON_CircleCurve_class_id;
01130   const ON_ClassId* ClassId() const;
01131   ON_BOOL32 Read(
01132          ON_BinaryArchive&  // open binary file
01133        );
01134 };
01135 
01136 static ON_Object* CreateNewON_CircleCurve() 
01137 {
01138 
01139   // must create an ON_CircleCurve so virtual 
01140   // ON_CircleCurve::Read will be used to read
01141   // archive objects with uuid CF33BE29-09B4-11d4-BFFB-0010830122F0
01142   return new ON__OBSOLETE__CircleCurve();
01143 } 
01144 
01145 const ON_ClassId ON__OBSOLETE__CircleCurve::m_ON_CircleCurve_class_id("ON__OBSOLETE__CircleCurve",
01146                                                            "ON_ArcCurve",
01147                                                            CreateNewON_CircleCurve,0,
01148                                                            "CF33BE29-09B4-11d4-BFFB-0010830122F0");
01149 
01150 const ON_ClassId* ON__OBSOLETE__CircleCurve::ClassId() const 
01151 {
01152   // so write will save ON_ArcCurve uuid
01153   return &ON_ArcCurve::m_ON_ArcCurve_class_id;
01154 }
01155 
01156 ON_BOOL32 ON__OBSOLETE__CircleCurve::Read(
01157        ON_BinaryArchive& file // open binary file
01158      )
01159 {
01160   int major_version = 0;
01161   int minor_version = 0;
01162   ON_BOOL32 rc = file.Read3dmChunkVersion(&major_version,&minor_version);
01163   if (rc)
01164   {
01165     if (major_version==1) 
01166     {
01167       // common to all 1.x versions
01168       ON_Circle circle;
01169       rc = file.ReadCircle( circle );
01170       m_arc = circle;
01171       if (rc) 
01172         rc = file.ReadInterval( m_t );
01173       if (rc) 
01174         rc = file.ReadInt(&m_dim);
01175       if ( m_dim != 2 && m_dim != 3 )
01176         m_dim = 3;
01177     }
01178   }
01179 
01180   return rc;
01181 }


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