00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00015
00016
00017 #include "pcl/surface/3rdparty/opennurbs/opennurbs.h"
00018
00019 ON_VIRTUAL_OBJECT_IMPLEMENT(ON_Surface,ON_Geometry,"4ED7D4E1-E947-11d3-BFE5-0010830122F0");
00020
00021 ON_Surface::ON_Surface()
00022 : ON_Geometry()
00023 {}
00024
00025 ON_Surface::ON_Surface(const ON_Surface& src)
00026 : ON_Geometry(src)
00027 {}
00028
00029 unsigned int ON_Surface::SizeOf() const
00030 {
00031 unsigned int sz = ON_Geometry::SizeOf();
00032 sz += (sizeof(*this) - sizeof(ON_Geometry));
00033
00034
00035
00036
00037
00038
00039 return sz;
00040 }
00041
00042 ON_Surface& ON_Surface::operator=(const ON_Surface& src)
00043 {
00044 DestroySurfaceTree();
00045 ON_Geometry::operator=(src);
00046 return *this;
00047 }
00048
00049 ON_Surface::~ON_Surface()
00050 {
00051
00052
00053
00054
00055
00056
00057 }
00058
00059 ON_Surface* ON_Surface::DuplicateSurface() const
00060 {
00061 return Duplicate();
00062 }
00063
00064 ON::object_type ON_Surface::ObjectType() const
00065 {
00066 return ON::surface_object;
00067 }
00068
00069 ON_BOOL32 ON_Surface::GetDomain( int dir, double* t0, double* t1 ) const
00070 {
00071 ON_Interval d = Domain(dir);
00072 if ( t0 ) *t0 = d[0];
00073 if ( t1 ) *t1 = d[1];
00074 return d.IsIncreasing();
00075 }
00076
00077 ON_BOOL32 ON_Surface::GetSurfaceSize(
00078 double* width,
00079 double* height
00080 ) const
00081 {
00082 if ( width )
00083 *width = 0.0;
00084 if ( height )
00085 *height = 0.0;
00086 return false;
00087 }
00088
00089 bool ON_Surface::SetDomain( int dir, ON_Interval domain )
00090 {
00091 return ( dir >= 0
00092 && dir <= 1
00093 && domain.IsIncreasing()
00094 && SetDomain( dir, domain[0], domain[1] )) ? true : false;
00095 }
00096
00097 ON_BOOL32 ON_Surface::SetDomain(
00098 int,
00099 double, double
00100 )
00101 {
00102
00103 return false;
00104 }
00105
00107
00108
00109
00110
00111
00112
00113 ON_BOOL32 ON_Surface::GetSpanVectorIndex(
00114 int dir,
00115 double t,
00116 int side,
00117 int* span_vector_i,
00118 ON_Interval* span_domain
00119 ) const
00120 {
00121 ON_BOOL32 rc = false;
00122 int i;
00123 int span_count = SpanCount(dir);
00124 if ( span_count > 0 ) {
00125 double* span_vector = (double*)onmalloc((span_count+1)*sizeof(span_vector[0]));
00126 rc = GetSpanVector( dir, span_vector );
00127 if (rc) {
00128 i = ON_NurbsSpanIndex( 2, span_count, span_vector, t, side, 0 );
00129 if ( i >= 0 && i <= span_count ) {
00130 if ( span_vector_i )
00131 *span_vector_i = i;
00132 if ( span_domain )
00133 span_domain->Set( span_vector[i], span_vector[i+1] );
00134 }
00135 else
00136 rc = false;
00137 }
00138 onfree(span_vector);
00139 }
00140 return rc;
00141 }
00142
00143 ON_BOOL32 ON_Surface::GetParameterTolerance(
00144 int dir,
00145 double t,
00146 double* tminus,
00147 double* tplus
00148 ) const
00149 {
00150 ON_BOOL32 rc = false;
00151 ON_Interval d = Domain( dir );
00152 if ( d.IsIncreasing() )
00153 rc = ON_GetParameterTolerance( d.Min(), d.Max(), t, tminus, tplus );
00154 return rc;
00155 }
00156
00157 ON_Surface::ISO
00158 ON_Surface::IsIsoparametric( const ON_Curve& curve, const ON_Interval* subdomain ) const
00159 {
00160 ISO iso = not_iso;
00161
00162 if ( subdomain )
00163 {
00164 ON_Interval cdom = curve.Domain();
00165 double t0 = cdom.NormalizedParameterAt(subdomain->Min());
00166 double t1 = cdom.NormalizedParameterAt(subdomain->Max());
00167 if ( t0 < t1-ON_SQRT_EPSILON )
00168 {
00169 if ( (t0 > ON_SQRT_EPSILON && t0 < 1.0-ON_SQRT_EPSILON) || (t1 > ON_SQRT_EPSILON && t1 < 1.0-ON_SQRT_EPSILON) )
00170 {
00171 cdom.Intersection(*subdomain);
00172 if ( cdom.IsIncreasing() )
00173 {
00174 ON_NurbsCurve nurbs_curve;
00175 if ( curve.GetNurbForm( nurbs_curve, 0.0,&cdom) )
00176 {
00177 return IsIsoparametric( nurbs_curve, 0 );
00178 }
00179 }
00180 }
00181 }
00182 }
00183
00184
00185 ON_BoundingBox bbox;
00186 double tolerance = 0.0;
00187 const int dim = curve.Dimension();
00188 if ( (dim == 2 || dim==3) && curve.GetBoundingBox(bbox) )
00189 {
00190 iso = IsIsoparametric( bbox );
00191 switch (iso) {
00192 case x_iso:
00193 case W_iso:
00194 case E_iso:
00195
00196
00197 tolerance = bbox.m_max.x - bbox.m_min.x;
00198 if ( tolerance < ON_ZERO_TOLERANCE && ON_ZERO_TOLERANCE*1024.0 <= (bbox.m_max.y-bbox.m_min.y) )
00199 {
00200
00201
00202
00203
00204
00205 tolerance = ON_ZERO_TOLERANCE;
00206 }
00207 if ( !curve.IsLinear( tolerance ) )
00208 iso = not_iso;
00209 break;
00210 case y_iso:
00211 case S_iso:
00212 case N_iso:
00213
00214
00215 tolerance = bbox.m_max.y - bbox.m_min.y;
00216 if ( tolerance < ON_ZERO_TOLERANCE && ON_ZERO_TOLERANCE*1024.0 <= (bbox.m_max.x-bbox.m_min.x) )
00217 {
00218
00219
00220
00221
00222
00223 tolerance = ON_ZERO_TOLERANCE;
00224 }
00225 if ( !curve.IsLinear( tolerance ) )
00226 iso = not_iso;
00227 break;
00228 default:
00229
00230 break;
00231 }
00232 }
00233 return iso;
00234 }
00235
00236 ON_Surface::ISO
00237 ON_Surface::IsIsoparametric( const ON_BoundingBox& bbox ) const
00238 {
00239 ISO iso = not_iso;
00240 if ( bbox.m_min.z == bbox.m_max.z ) {
00241 const double ds = bbox.m_max.x - bbox.m_min.x;
00242 const double dt = bbox.m_max.y - bbox.m_min.y;
00243 double a, b, s0, s1, t0, t1;
00244 ON_Interval d = Domain(0);
00245 s0 = d.Min();
00246 s1 = d.Max();
00247 d = Domain(1);
00248 t0 = d.Min();
00249 t1 = d.Max();
00250 double stol = (s1-s0)/32.0;
00251 double ttol = (t1-t0)/32.0;
00252 if ( s0 < s1 && t0 < t1 && ( ds <= stol || dt <= ttol) )
00253 {
00254 if ( ds*(t1-t0) <= dt*(s1-s0) )
00255 {
00256
00257 if ( bbox.m_max.x <= s0+stol )
00258 {
00259
00260 GetParameterTolerance( 0, s0, &a, &b);
00261 if ( a <= bbox.m_min.x && bbox.m_max.x <= b )
00262 iso = W_iso;
00263 }
00264 else if ( bbox.m_min.x >= s1-stol )
00265 {
00266
00267 GetParameterTolerance( 0, s1, &a, &b);
00268 if ( a <= bbox.m_min.x && bbox.m_max.x <= b )
00269 iso = E_iso;
00270 }
00271
00272 if ( iso == not_iso && (s0 < bbox.m_max.x || bbox.m_min.x < s1) )
00273 {
00274
00275 GetParameterTolerance( 0, 0.5*(bbox.m_min.x+bbox.m_max.x), &a, &b);
00276 if ( a <= bbox.m_min.x && bbox.m_max.x <= b )
00277 iso = x_iso;
00278 }
00279 }
00280 else
00281 {
00282
00283 if ( bbox.m_max.y <= t0+ttol )
00284 {
00285
00286 GetParameterTolerance( 1, t0, &a, &b);
00287 if ( a < bbox.m_min.y && bbox.m_max.y <= b )
00288 iso = S_iso;
00289 }
00290 else if ( bbox.m_min.y >= t1-ttol )
00291 {
00292
00293 GetParameterTolerance( 1, t1, &a, &b);
00294 if ( a < bbox.m_min.y && bbox.m_max.y <= b )
00295 iso = N_iso;
00296 }
00297
00298 if ( iso == not_iso && (t0 < bbox.m_max.x || bbox.m_min.x < t1) )
00299 {
00300
00301 GetParameterTolerance( 1, 0.5*(bbox.m_min.y+bbox.m_max.y), &a, &b);
00302 if ( a < bbox.m_min.y && bbox.m_max.y <= b )
00303 iso = y_iso;
00304 }
00305 }
00306 }
00307 }
00308 return iso;
00309 }
00310
00311
00312 ON_BOOL32 ON_Surface::IsPlanar( ON_Plane* plane, double tolerance ) const
00313 {
00314 return false;
00315 }
00316
00317 ON_BOOL32
00318 ON_Surface::IsClosed(int dir) const
00319 {
00320 ON_Interval d = Domain(dir);
00321 if ( d.IsIncreasing() && Dimension() <= 3 ) {
00322 const int span_count = SpanCount(dir?0:1);
00323 const int span_degree = Degree(dir?0:1);
00324 if ( span_count > 0 && span_degree > 0 )
00325 {
00326 ON_SimpleArray<double> s(span_count+1);
00327 s.SetCount(span_count+1);
00328 int n = 2*span_degree+1;
00329 double delta = 1.0/n;
00330 ON_3dPoint P, Q;
00331 P.Zero();
00332 Q.Zero();
00333 int hintP[2] = {0,0};
00334 int hintQ[2] = {0,0};
00335 double *u0, *u1, *v0, *v1;
00336 double t;
00337 ON_Interval sp;
00338 if ( dir ) {
00339 v0 = &d.m_t[0];
00340 v1 = &d.m_t[1];
00341 u0 = &t;
00342 u1 = &t;
00343 }
00344 else {
00345 u0 = &d.m_t[0];
00346 u1 = &d.m_t[1];
00347 v0 = &t;
00348 v1 = &t;
00349 }
00350 if ( GetSpanVector( dir?0:1, s.Array() ) )
00351 {
00352 int span_index, i;
00353 for ( span_index = 0; span_index < span_count; span_index++ ) {
00354 sp.Set(s[span_index],s[span_index+1]);
00355 for ( i = 0; i < n; i++ ) {
00356 t = sp.ParameterAt(i*delta);
00357 if ( !Evaluate( *u0, *v0, 1, 3, P, 0, hintP ) )
00358 return false;
00359 if ( !Evaluate( *u1, *v1, 2, 3, Q, 0, hintQ ) )
00360 return false;
00361 if ( false == ON_PointsAreCoincident( 3, 0, &P.x, &Q.x ) )
00362 return false;
00363 }
00364 }
00365 return true;
00366 }
00367 }
00368 }
00369 return false;
00370 }
00371
00372 ON_BOOL32 ON_Surface::IsPeriodic(int dir) const
00373 {
00374 return false;
00375 }
00376
00377 bool ON_Surface::GetNextDiscontinuity(
00378 int dir,
00379 ON::continuity c,
00380 double t0,
00381 double t1,
00382 double* t,
00383 int* hint,
00384 int* dtype,
00385 double cos_angle_tolerance,
00386 double curvature_tolerance
00387 ) const
00388 {
00389
00390
00391
00392
00393
00394 bool rc = false;
00395
00396
00397 int tmp_dtype = 0;
00398 if ( !dtype )
00399 dtype = &tmp_dtype;
00400 *dtype = 0;
00401
00402 if ( t0 != t1 )
00403 {
00404 bool bTestC0 = false;
00405 bool bTestD1 = false;
00406 bool bTestD2 = false;
00407 bool bTestT = false;
00408 bool bTestK = false;
00409 switch(c)
00410 {
00411 case ON::C0_locus_continuous:
00412 bTestC0 = true;
00413 break;
00414 case ON::C1_locus_continuous:
00415 bTestC0 = true;
00416 bTestD1 = true;
00417 break;
00418 case ON::C2_locus_continuous:
00419 bTestC0 = true;
00420 bTestD1 = true;
00421 bTestD2 = true;
00422 break;
00423 case ON::G1_locus_continuous:
00424 bTestC0 = true;
00425 bTestT = true;
00426 break;
00427 case ON::G2_locus_continuous:
00428 bTestC0 = true;
00429 bTestT = true;
00430 bTestK = true;
00431 break;
00432 default:
00433
00434 break;
00435 }
00436
00437 if ( bTestC0 )
00438 {
00439
00440
00441
00442
00443
00444
00445 int hinta[2], hintb[2], span_index, j;
00446 ON_Interval domain = Domain(dir);
00447 ON_Interval span_domain;
00448 ON_2dPoint st0, st1;
00449 ON_3dVector Da[6], Db[6];
00450 ON_3dVector& D1a = Da[1+dir];
00451 ON_3dVector& D1b = Db[1+dir];
00452 ON_3dVector& D2a = Da[3+2*dir];
00453 ON_3dVector& D2b = Db[3+2*dir];
00454
00455 if ( t0 < domain[1] && t1 >= domain[1] )
00456 t1 = domain[1];
00457 else if ( t0 > domain[0] && t1 <= domain[0] )
00458 t1 = domain[0];
00459
00460 if ( (t0 < domain[1] && t1 >= domain[1]) || (t0 > domain[0] && t1 <= domain[0]) )
00461 {
00462 if ( IsClosed(dir) )
00463 {
00464 int span_count = SpanCount(1-dir);
00465 double* span_vector = (span_count>0) ? ((double*)onmalloc((span_count+1)*sizeof(*span_vector))) : 0;
00466 if (!GetSpanVector(1-dir,span_vector))
00467 span_count = 0;
00468 st0[dir] = domain[0];
00469 st1[dir] = domain[1];
00470
00471 for ( span_index = 0; span_index < span_count && 1 != *dtype; span_index++ )
00472 {
00473 span_domain.Set(span_vector[span_index],span_vector[span_index+1]);
00474 for ( j = (span_index?1:0); j <= 2 && 1 != *dtype; j++ )
00475 {
00476 st0[1-dir] = span_domain.ParameterAt(0.5*j);
00477 st1[1-dir] = st0[1-dir];
00478 if ( bTestD1 || bTestT )
00479 {
00480
00481 if ( Evaluate(st0.x,st0.y,2,3,&Da[0].x,1,hinta)
00482 && Evaluate(st1.x,st1.y,2,3,&Db[0].x,2,hintb) )
00483 {
00484 if ( bTestD1 )
00485 {
00486 if ( !(D1a-D1b).IsTiny(D1b.MaximumCoordinate()*ON_SQRT_EPSILON ) )
00487 {
00488 if ( dtype )
00489 *dtype = 1;
00490 *t = t1;
00491 rc = true;
00492 }
00493 else if ( bTestD2 && !(D2a-D2b).IsTiny(D2b.MaximumCoordinate()*ON_SQRT_EPSILON) )
00494 {
00495 if ( dtype )
00496 *dtype = 2;
00497 *t = t1;
00498 rc = true;
00499 }
00500
00501 }
00502 else if ( bTestT )
00503 {
00504 ON_3dVector Ta, Tb, Ka, Kb;
00505 ON_EvCurvature( D1a, D2a, Ta, Ka );
00506 ON_EvCurvature( D1b, D2b, Tb, Kb );
00507 if ( Ta*Tb < cos_angle_tolerance )
00508 {
00509 if ( dtype )
00510 *dtype = 1;
00511 *t = t1;
00512 rc = true;
00513 }
00514 else if ( bTestK && (Ka-Kb).Length() > curvature_tolerance )
00515 {
00516 if ( dtype )
00517 *dtype = 2;
00518 *t = t1;
00519 rc = true;
00520 }
00521 }
00522 }
00523 }
00524 }
00525 }
00526
00527 if ( span_vector)
00528 {
00529 onfree(span_vector);
00530 }
00531 }
00532 else
00533 {
00534
00535 *dtype = 0;
00536 *t = t1;
00537 rc = true;
00538 }
00539 }
00540 }
00541 }
00542
00543 return rc;
00544 }
00545
00546 static
00547 bool PrincipalCurvaturesAreContinuous(
00548 bool bSmoothTest,
00549 double k1a, double k2a,
00550 double k1b, double k2b,
00551 double curvature_tolerance
00552 )
00553 {
00554 ON_3dVector K[2];
00555 K[0].x = k1a;
00556 K[0].y = 0.0;
00557 K[0].z = 0.0;
00558 K[1].x = k1b;
00559 K[1].y = 0.0;
00560 K[1].z = 0.0;
00561
00562 bool rc = ( bSmoothTest )
00563 ? ON_IsGsmoothCurvatureContinuous(K[0],K[1],0.0,curvature_tolerance)
00564 : ON_IsG2CurvatureContinuous(K[0],K[1],0.0,curvature_tolerance);
00565 if ( rc )
00566 {
00567
00568 K[0].x = k2a;
00569 K[1].x = k2b;
00570 rc = ( bSmoothTest )
00571 ? ON_IsGsmoothCurvatureContinuous(K[0],K[1],0.0,curvature_tolerance)
00572 : ON_IsG2CurvatureContinuous(K[0],K[1],0.0,curvature_tolerance);
00573 }
00574 return rc;
00575 }
00576
00577 bool ON_Surface::IsContinuous(
00578 ON::continuity desired_continuity,
00579 double s,
00580 double t,
00581 int* hint,
00582 double point_tolerance,
00583 double d1_tolerance,
00584 double d2_tolerance,
00585 double cos_angle_tolerance,
00586 double curvature_tolerance
00587 ) const
00588 {
00589 int qi, span_count[2];
00590 span_count[0] = SpanCount(0);
00591 span_count[1] = SpanCount(1);
00592 if ( span_count[0] <= 1 && span_count[1] <= 1 )
00593 return true;
00594
00595 ON_3dPoint P[4];
00596 ON_3dVector Ds[4], Dt[4], Dss[4], Dst[4], Dtt[4], N[4], K1[4], K2[4];
00597 double gauss[4], mean[4], kappa1[4], kappa2[4], sq[4], tq[4];
00598
00599
00600 switch ( desired_continuity )
00601 {
00602 case ON::C0_locus_continuous:
00603 case ON::C1_locus_continuous:
00604 case ON::C2_locus_continuous:
00605 case ON::G1_locus_continuous:
00606 case ON::G2_locus_continuous:
00607 {
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619 ON_Interval d = Domain(0);
00620 if ( s == d[1] )
00621 {
00622 sq[0] = sq[1] = d[0];
00623 sq[2] = sq[3] = d[1];
00624 }
00625 else
00626 {
00627 sq[0] = sq[1] = sq[2] = sq[3] = s;
00628 }
00629
00630 d = Domain(1);
00631
00632
00633
00634
00635
00636 if ( t == d[1] )
00637 {
00638 tq[0] = tq[3] = d[0];
00639 tq[1] = tq[2] = d[1];
00640 }
00641 else
00642 {
00643 tq[0] = tq[1] = tq[2] = tq[3] = t;
00644 }
00645 }
00646 break;
00647
00648 default:
00649 sq[0] = sq[1] = sq[2] = sq[3] = s;
00650 tq[0] = tq[1] = tq[2] = tq[3] = t;
00651 break;
00652 }
00653
00654 desired_continuity = ON::ParametricContinuity(desired_continuity);
00655
00656
00657
00658
00659 switch ( desired_continuity )
00660 {
00661
00662 case ON::C0_continuous:
00663 for ( qi = 0; qi < 4; qi++ )
00664 {
00665 if ( !EvPoint( sq[qi], tq[qi], P[qi], qi+1 ) )
00666 return false;
00667 if ( qi )
00668 {
00669 if ( !(P[qi]-P[qi-1]).IsTiny(point_tolerance) )
00670 return false;
00671 }
00672 }
00673 if ( !(P[3]-P[0]).IsTiny(point_tolerance) )
00674 return false;
00675 break;
00676
00677 case ON::C1_continuous:
00678 for ( qi = 0; qi < 4; qi++ )
00679 {
00680 if ( !Ev1Der( sq[qi], tq[qi], P[qi], Ds[qi], Dt[qi], qi+1, hint ) )
00681 return false;
00682 if ( qi )
00683 {
00684 if ( !(P[qi]-P[qi-1]).IsTiny(point_tolerance) )
00685 return false;
00686 if ( !(Ds[qi]-Ds[qi-1]).IsTiny(d1_tolerance) )
00687 return false;
00688 if ( !(Dt[qi]-Dt[qi-1]).IsTiny(d1_tolerance) )
00689 return false;
00690 }
00691 }
00692 if ( !(P[3]-P[0]).IsTiny(point_tolerance) )
00693 return false;
00694 if ( !(Ds[3]-Ds[0]).IsTiny(d1_tolerance) )
00695 return false;
00696 if ( !(Dt[3]-Dt[0]).IsTiny(d1_tolerance) )
00697 return false;
00698 break;
00699
00700 case ON::C2_continuous:
00701 for ( qi = 0; qi < 4; qi++ )
00702 {
00703 if ( !Ev2Der( sq[qi], tq[qi], P[qi], Ds[qi], Dt[qi],
00704 Dss[qi], Dst[qi], Dtt[qi],
00705 qi+1, hint ) )
00706 return false;
00707 if ( qi )
00708 {
00709 if ( !(P[qi]-P[qi-1]).IsTiny(point_tolerance) )
00710 return false;
00711 if ( !(Ds[qi]-Ds[qi-1]).IsTiny(d1_tolerance) )
00712 return false;
00713 if ( !(Dt[qi]-Dt[qi-1]).IsTiny(d1_tolerance) )
00714 return false;
00715 if ( !(Dss[qi]-Dss[qi-1]).IsTiny(d2_tolerance) )
00716 return false;
00717 if ( !(Dst[qi]-Dst[qi-1]).IsTiny(d2_tolerance) )
00718 return false;
00719 if ( !(Dtt[qi]-Dtt[qi-1]).IsTiny(d2_tolerance) )
00720 return false;
00721 }
00722 }
00723 if ( !(P[3]-P[0]).IsTiny(point_tolerance) )
00724 return false;
00725 if ( !(Ds[3]-Ds[0]).IsTiny(d1_tolerance) )
00726 return false;
00727 if ( !(Dt[3]-Dt[0]).IsTiny(d1_tolerance) )
00728 return false;
00729 if ( !(Dss[3]-Dss[0]).IsTiny(d2_tolerance) )
00730 return false;
00731 if ( !(Dst[3]-Dst[0]).IsTiny(d2_tolerance) )
00732 return false;
00733 if ( !(Dtt[3]-Dtt[0]).IsTiny(d2_tolerance) )
00734 return false;
00735 break;
00736
00737 case ON::G1_continuous:
00738 for ( qi = 0; qi < 4; qi++ )
00739 {
00740 if ( !EvNormal( sq[qi], tq[qi], P[qi], N[qi], qi+1 ) )
00741 return false;
00742 if ( qi )
00743 {
00744 if ( !(P[qi]-P[qi-1]).IsTiny(point_tolerance) )
00745 return false;
00746 if ( N[qi]*N[qi-1] < cos_angle_tolerance )
00747 return false;
00748 }
00749 }
00750 if ( !(P[3]-P[0]).IsTiny(point_tolerance) )
00751 return false;
00752 if ( N[3]*N[0] < cos_angle_tolerance )
00753 return false;
00754 break;
00755
00756 case ON::G2_continuous:
00757 case ON::Gsmooth_continuous:
00758 {
00759 bool bSmoothCon = (ON::Gsmooth_continuous == desired_continuity);
00760 for ( qi = 0; qi < 4; qi++ )
00761 {
00762 if ( !Ev2Der( sq[qi], tq[qi], P[qi], Ds[qi], Dt[qi],
00763 Dss[qi], Dst[qi], Dtt[qi],
00764 qi+1, hint ) )
00765 return false;
00766 ON_EvPrincipalCurvatures( Ds[qi], Dt[qi], Dss[qi], Dst[qi], Dtt[qi], N[qi],
00767 &gauss[qi], &mean[qi], &kappa1[qi], &kappa2[qi],
00768 K1[qi], K2[qi] );
00769 if ( qi )
00770 {
00771 if ( !(P[qi]-P[qi-1]).IsTiny(point_tolerance) )
00772 return false;
00773 if ( N[qi]*N[qi-1] < cos_angle_tolerance )
00774 return false;
00775 if ( !PrincipalCurvaturesAreContinuous(bSmoothCon,kappa1[qi],kappa2[qi],kappa1[qi-1],kappa2[qi-1],curvature_tolerance) )
00776 return false;
00777 }
00778 if ( !(P[3]-P[0]).IsTiny(point_tolerance) )
00779 return false;
00780 if ( N[3]*N[0] < cos_angle_tolerance )
00781 return false;
00782 if ( !PrincipalCurvaturesAreContinuous(bSmoothCon,kappa1[3],kappa2[3],kappa1[0],kappa2[0],curvature_tolerance) )
00783 return false;
00784 }
00785 }
00786 break;
00787
00788 default:
00789
00790 break;
00791
00792 }
00793
00794 return true;
00795 }
00796
00797 ON_BOOL32
00798 ON_Surface::IsSingular(int side) const
00799 {
00800 return false;
00801 }
00802
00803 bool ON_Surface::IsSolid() const
00804 {
00805 const bool bIsClosed0 = ( IsClosed(0) || ( IsSingular(1) && IsSingular(3) ) );
00806 const bool bIsClosed1 = ( IsClosed(1) || ( IsSingular(0) && IsSingular(2) ) );
00807
00808 if ( bIsClosed0 && bIsClosed1 )
00809 return true;
00810
00811 const ON_Extrusion* extrusion = ON_Extrusion::Cast(this);
00812 if ( 0 != extrusion && extrusion->IsSolid() )
00813 return true;
00814
00815 return false;
00816 }
00817
00818
00819 bool
00820 ON_Surface::IsAtSingularity(double s, double t,
00821 bool bExact
00822 ) const
00823
00824 {
00825 if (bExact){
00826 if (s == Domain(0)[0]){
00827 if (IsSingular(3))
00828 return true;
00829 }
00830 else if (s == Domain(0)[1]){
00831 if (IsSingular(1))
00832 return true;
00833 }
00834 if (t == Domain(1)[0]){
00835 if (IsSingular(0))
00836 return true;
00837 }
00838 else if (t == Domain(1)[1]){
00839 if (IsSingular(2))
00840 return true;
00841 }
00842 return false;
00843 }
00844
00845 if (IsAtSingularity(s, t, true))
00846 return true;
00847
00848 bool bCheckPartials[2] = {false, false};
00849 int i;
00850
00851 double m[2];
00852 for (i=0; i<2; i++)
00853 m[i] = Domain(i).Mid();
00854 if (s < m[0]){
00855 if (IsSingular(3))
00856 bCheckPartials[1] = true;
00857 }
00858 else {
00859 if (IsSingular(1))
00860 bCheckPartials[1] = true;
00861 }
00862 if (!bCheckPartials[0] && !bCheckPartials[1]){
00863 if (t < m[1]){
00864 if (IsSingular(0))
00865 bCheckPartials[0] = true;
00866 }
00867 else {
00868 if (IsSingular(2))
00869 bCheckPartials[0] = true;
00870 }
00871 }
00872
00873 if (!bCheckPartials[0] && !bCheckPartials[1])
00874 return false;
00875
00876 ON_3dPoint P;
00877 ON_3dVector M[2], S[2];
00878 if (!Ev1Der(s, t, P, S[0], S[1]))
00879 return false;
00880 if (!Ev1Der(m[0], m[1], P, M[0], M[1]))
00881 return false;
00882
00883 for (i=0; i<2; i++){
00884 if (!bCheckPartials[i])
00885 continue;
00886 if (S[i].Length() < 1.0e-6 * M[i].Length())
00887 return true;
00888 }
00889
00890 return false;
00891
00892 }
00893
00894 int ON_Surface::IsAtSeam(double s, double t) const
00895
00896 {
00897 int rc = 0;
00898 int i;
00899 for (i=0; i<2; i++){
00900 if (!IsClosed(i))
00901 continue;
00902 double p = (i) ? t : s;
00903 if (p == Domain(i)[0] || p == Domain(i)[1])
00904 rc += (i+1);
00905 }
00906
00907 return rc;
00908 }
00909
00910
00911 ON_3dPoint
00912 ON_Surface::PointAt( double s, double t ) const
00913 {
00914 ON_3dPoint p(0.0,0.0,0.0);
00915 EvPoint( s, t, p );
00916 return p;
00917 }
00918
00919 ON_3dVector
00920 ON_Surface::NormalAt( double s, double t ) const
00921 {
00922 ON_3dVector N(0.0,0.0,0.0);
00923 EvNormal( s, t, N );
00924 return N;
00925 }
00926
00927 ON_BOOL32 ON_Surface::FrameAt( double u, double v, ON_Plane& frame) const
00928 {
00929 ON_BOOL32 rc = false;
00930 ON_3dPoint origin;
00931 ON_3dVector udir, vdir, normal;
00932 if( EvNormal( u, v, origin, udir, vdir, normal))
00933 {
00934 if ( udir.Unitize() )
00935 vdir = ON_CrossProduct( normal, udir);
00936 else if ( vdir.Unitize() )
00937 udir = ON_CrossProduct( vdir, normal);
00938 frame.CreateFromFrame( origin, udir, vdir);
00939 rc = frame.IsValid();
00940 }
00941 return rc;
00942 }
00943
00944
00945
00946 ON_BOOL32
00947 ON_Surface::EvPoint(
00948 double s, double t,
00949 ON_3dPoint& point,
00950 int side,
00951
00952
00953
00954
00955
00956 int* hint
00957
00958 ) const
00959 {
00960 ON_BOOL32 rc = false;
00961 double ws[128];
00962 double* v;
00963 if ( Dimension() <= 3 ) {
00964 v = &point.x;
00965 point.x = 0.0;
00966 point.y = 0.0;
00967 point.z = 0.0;
00968 }
00969 else if ( Dimension() <= 128 ) {
00970 v = ws;
00971 }
00972 else {
00973 v = (double*)onmalloc(Dimension()*sizeof(*v));
00974 }
00975 rc = Evaluate( s, t, 0, Dimension(), v, side, hint );
00976 if ( Dimension() > 3 ) {
00977 point.x = v[0];
00978 point.y = v[1];
00979 point.z = v[2];
00980 if ( Dimension() > 128 )
00981 onfree(v);
00982 }
00983 return rc;
00984 }
00985
00986 ON_BOOL32
00987 ON_Surface::Ev1Der(
00988 double s, double t,
00989 ON_3dPoint& point,
00990 ON_3dVector& ds,
00991 ON_3dVector& dt,
00992 int side,
00993
00994
00995
00996
00997
00998 int* hint
00999
01000 ) const
01001 {
01002 ON_BOOL32 rc = false;
01003 const int dim = Dimension();
01004 double ws[3*32];
01005 double* v;
01006 point.x = 0.0;
01007 point.y = 0.0;
01008 point.z = 0.0;
01009 ds.x = 0.0;
01010 ds.y = 0.0;
01011 ds.z = 0.0;
01012 dt.x = 0.0;
01013 dt.y = 0.0;
01014 dt.z = 0.0;
01015 if ( dim <= 32 ) {
01016 v = ws;
01017 }
01018 else {
01019 v = (double*)onmalloc(3*dim*sizeof(*v));
01020 }
01021 rc = Evaluate( s, t, 1, dim, v, side, hint );
01022 point.x = v[0];
01023 ds.x = v[dim];
01024 dt.x = v[2*dim];
01025 if ( dim > 1 ) {
01026 point.y = v[1];
01027 ds.y = v[dim+1];
01028 dt.y = v[2*dim+1];
01029 if ( dim > 2 ) {
01030 point.z = v[2];
01031 ds.z = v[dim+2];
01032 dt.z = v[2*dim+2];
01033 if ( dim > 32 )
01034 onfree(v);
01035 }
01036 }
01037
01038 return rc;
01039 }
01040
01041 ON_BOOL32
01042 ON_Surface::Ev2Der(
01043 double s, double t,
01044 ON_3dPoint& point,
01045 ON_3dVector& ds,
01046 ON_3dVector& dt,
01047 ON_3dVector& dss,
01048 ON_3dVector& dst,
01049 ON_3dVector& dtt,
01050 int side,
01051
01052
01053
01054
01055
01056 int* hint
01057
01058 ) const
01059 {
01060 ON_BOOL32 rc = false;
01061 const int dim = Dimension();
01062 double ws[6*16];
01063 double* v;
01064 point.x = 0.0;
01065 point.y = 0.0;
01066 point.z = 0.0;
01067 ds.x = 0.0;
01068 ds.y = 0.0;
01069 ds.z = 0.0;
01070 dt.x = 0.0;
01071 dt.y = 0.0;
01072 dt.z = 0.0;
01073 dss.x = 0.0;
01074 dss.y = 0.0;
01075 dss.z = 0.0;
01076 dst.x = 0.0;
01077 dst.y = 0.0;
01078 dst.z = 0.0;
01079 dtt.x = 0.0;
01080 dtt.y = 0.0;
01081 dtt.z = 0.0;
01082 if ( dim <= 16 ) {
01083 v = ws;
01084 }
01085 else {
01086 v = (double*)onmalloc(6*dim*sizeof(*v));
01087 }
01088 rc = Evaluate( s, t, 2, dim, v, side, hint );
01089 point.x = v[0];
01090 ds.x = v[dim];
01091 dt.x = v[2*dim];
01092 dss.x = v[3*dim];
01093 dst.x = v[4*dim];
01094 dtt.x = v[5*dim];
01095 if ( dim > 1 ) {
01096 point.y = v[1];
01097 ds.y = v[dim+1];
01098 dt.y = v[2*dim+1];
01099 dss.y = v[3*dim+1];
01100 dst.y = v[4*dim+1];
01101 dtt.y = v[5*dim+1];
01102 if ( dim > 2 ) {
01103 point.z = v[2];
01104 ds.z = v[dim+2];
01105 dt.z = v[2*dim+2];
01106 dss.z = v[3*dim+2];
01107 dst.z = v[4*dim+2];
01108 dtt.z = v[5*dim+2];
01109 if ( dim > 16 )
01110 onfree(v);
01111 }
01112 }
01113
01114 return rc;
01115 }
01116
01117
01118 ON_BOOL32
01119 ON_Surface::EvNormal(
01120 double s, double t,
01121 ON_3dVector& normal,
01122 int side,
01123
01124
01125
01126
01127
01128 int* hint
01129
01130 ) const
01131 {
01132 ON_3dPoint point;
01133 ON_3dVector ds, dt;
01134 return EvNormal( s, t, point, ds, dt, normal, side, hint );
01135 }
01136
01137 ON_BOOL32
01138 ON_Surface::EvNormal(
01139 double s, double t,
01140 ON_3dPoint& point,
01141 ON_3dVector& normal,
01142 int side,
01143
01144
01145
01146
01147
01148 int* hint
01149
01150 ) const
01151 {
01152 ON_3dVector ds, dt;
01153 return EvNormal( s, t, point, ds, dt, normal, side, hint );
01154 }
01155
01156 ON_BOOL32
01157 ON_Surface::EvNormal(
01158 double s, double t,
01159 ON_3dPoint& point,
01160 ON_3dVector& ds,
01161 ON_3dVector& dt,
01162 ON_3dVector& normal,
01163 int side,
01164
01165
01166
01167
01168
01169 int* hint
01170
01171 ) const
01172 {
01173
01174 ON_BOOL32 rc = Ev1Der( s, t, point, ds, dt, side, hint );
01175 if ( rc ) {
01176 const double len_ds = ds.Length();
01177 const double len_dt = dt.Length();
01178
01179
01180
01181 if ( len_ds > ON_SQRT_EPSILON*len_dt && len_dt > ON_SQRT_EPSILON*len_ds )
01182 {
01183 ON_3dVector a = ds/len_ds;
01184 ON_3dVector b = dt/len_dt;
01185 normal = ON_CrossProduct( a, b );
01186 rc = normal.Unitize();
01187 }
01188 else
01189 {
01190
01191 double v[6][3];
01192 int normal_side = side;
01193 ON_BOOL32 bOnSide = false;
01194 ON_Interval sdom = Domain(0);
01195 ON_Interval tdom = Domain(1);
01196 if (s == sdom.Min()) {
01197 normal_side = (normal_side >= 3) ? 4 : 1;
01198 bOnSide = true;
01199 }
01200 else if (s == sdom.Max()) {
01201 normal_side = (normal_side >= 3) ? 3 : 2;
01202 bOnSide = true;
01203 }
01204 if (t == tdom.Min()) {
01205 normal_side = (normal_side == 2 || normal_side == 3) ? 2 : 1;
01206 bOnSide = true;
01207 }
01208 else if (t == tdom.Max()) {
01209 normal_side = (normal_side == 2 || normal_side == 3) ? 3 : 4;
01210 bOnSide = true;
01211 }
01212 if ( !bOnSide )
01213 {
01214
01215
01216 if ( len_ds > ON_EPSILON*len_dt && len_dt > ON_EPSILON*len_ds )
01217 {
01218 ON_3dVector a = ds/len_ds;
01219 ON_3dVector b = dt/len_dt;
01220 normal = ON_CrossProduct( a, b );
01221 rc = normal.Unitize();
01222 }
01223 else
01224 {
01225 rc = false;
01226 }
01227 }
01228 else {
01229 rc = Evaluate( s, t, 2, 3, &v[0][0], normal_side, hint );
01230 if ( rc ) {
01231 rc = ON_EvNormal( normal_side, v[1], v[2], v[3], v[4], v[5], normal);
01232 }
01233 }
01234 }
01235 }
01236 if ( !rc ) {
01237 normal.Zero();
01238 }
01239 return rc;
01240 }
01241
01242
01243 ON_Curve* ON_Surface::IsoCurve(
01244 int dir,
01245
01246
01247
01248 double c
01249 ) const
01250 {
01251 return NULL;
01252 }
01253
01254
01255 ON_BOOL32 ON_Surface::Trim(
01256 int dir,
01257 const ON_Interval& domain
01258 )
01259 {
01260 return false;
01261 }
01262
01263
01264 bool ON_Surface::Extend(
01265 int dir,
01266 const ON_Interval& domain
01267 )
01268 {
01269 return false;
01270 }
01271
01272
01273 ON_BOOL32 ON_Surface::Split(
01274 int dir,
01275 double c,
01276 ON_Surface*& west_or_south_side,
01277 ON_Surface*& east_or_north_side
01278 ) const
01279 {
01280 return false;
01281 }
01282
01283
01284 int ON_Surface::GetNurbForm(
01285 ON_NurbsSurface& nurbs_surface,
01286 double tolerance
01287 ) const
01288 {
01289 return 0;
01290 }
01291
01292
01293 int ON_Surface::HasNurbForm( ) const
01294 {
01295 return 0;
01296 }
01297
01298 bool ON_Surface::GetSurfaceParameterFromNurbFormParameter(
01299 double nurbs_s, double nurbs_t,
01300 double* surface_s, double* surface_t
01301 ) const
01302 {
01303
01304 *surface_s = nurbs_s;
01305 *surface_t = nurbs_t;
01306 return true;
01307 }
01308
01309 bool ON_Surface::GetNurbFormParameterFromSurfaceParameter(
01310 double surface_s, double surface_t,
01311 double* nurbs_s, double* nurbs_t
01312 ) const
01313 {
01314
01315 *nurbs_s = surface_s;
01316 *nurbs_t = surface_t;
01317 return true;
01318 }
01319
01320
01321 ON_NurbsSurface* ON_Surface::NurbsSurface(
01322 ON_NurbsSurface* pNurbsSurface,
01323 double tolerance,
01324 const ON_Interval* s_subdomain,
01325 const ON_Interval* t_subdomain
01326 ) const
01327 {
01328 ON_NurbsSurface* nurbs_surface = pNurbsSurface;
01329 if ( !nurbs_surface )
01330 nurbs_surface = new ON_NurbsSurface();
01331 int rc = GetNurbForm( *nurbs_surface, tolerance );
01332 if ( !rc )
01333 {
01334 if (!pNurbsSurface)
01335 delete nurbs_surface;
01336 nurbs_surface = NULL;
01337 }
01338 return nurbs_surface;
01339 }
01340
01341
01342
01343 ON_SurfaceArray::ON_SurfaceArray( int initial_capacity )
01344 : ON_SimpleArray<ON_Surface*>(initial_capacity)
01345 {}
01346
01347 ON_SurfaceArray::~ON_SurfaceArray()
01348 {
01349 Destroy();
01350 }
01351
01352 void ON_SurfaceArray::Destroy()
01353 {
01354 int i = m_capacity;
01355 while ( i-- > 0 ) {
01356 if ( m_a[i] ) {
01357 delete m_a[i];
01358 m_a[i] = NULL;
01359 }
01360 }
01361 Empty();
01362 }
01363
01364 ON_BOOL32 ON_SurfaceArray::Duplicate( ON_SurfaceArray& dst ) const
01365 {
01366 dst.Destroy();
01367 dst.SetCapacity( Capacity() );
01368
01369 const int count = Count();
01370 int i;
01371 ON_Surface* surface;
01372 for ( i = 0; i < count; i++ )
01373 {
01374 surface = 0;
01375 if ( m_a[i] )
01376 {
01377 surface = m_a[i]->Duplicate();
01378 }
01379 dst.Append(surface);
01380 }
01381 return true;
01382 }
01383
01384 ON_BOOL32 ON_SurfaceArray::Write( ON_BinaryArchive& file ) const
01385 {
01386 ON_BOOL32 rc = file.BeginWrite3dmChunk( TCODE_ANONYMOUS_CHUNK, 0 );
01387 if (rc) rc = file.Write3dmChunkVersion(1,0);
01388 if (rc )
01389 {
01390 int i;
01391 rc = file.WriteInt( Count() );
01392 for ( i = 0; rc && i < Count(); i++ ) {
01393 if ( m_a[i] )
01394 {
01395 rc = file.WriteInt(1);
01396 if ( rc )
01397 rc = file.WriteObject( *m_a[i] );
01398 }
01399 else
01400 {
01401
01402 rc = file.WriteInt(0);
01403 }
01404 }
01405 if ( !file.EndWrite3dmChunk() )
01406 rc = false;
01407 }
01408 return rc;
01409 }
01410
01411
01412 ON_BOOL32 ON_SurfaceArray::Read( ON_BinaryArchive& file )
01413 {
01414 int major_version = 0;
01415 int minor_version = 0;
01416 ON__UINT32 tcode = 0;
01417 ON__INT64 big_value = 0;
01418 int flag;
01419 Destroy();
01420 ON_BOOL32 rc = file.BeginRead3dmBigChunk( &tcode, &big_value );
01421 if (rc)
01422 {
01423 rc = ( tcode == TCODE_ANONYMOUS_CHUNK );
01424 if (rc) rc = file.Read3dmChunkVersion(&major_version,&minor_version);
01425 if (rc && major_version == 1) {
01426 ON_Object* p;
01427 int count;
01428 ON_BOOL32 rc = file.ReadInt( &count );
01429 if (rc) {
01430 SetCapacity(count);
01431 SetCount(count);
01432 Zero();
01433 int i;
01434 for ( i = 0; rc && i < count && rc; i++ ) {
01435 flag = 0;
01436 rc = file.ReadInt(&flag);
01437 if (rc && flag==1) {
01438 p = 0;
01439 rc = file.ReadObject( &p );
01440 m_a[i] = ON_Surface::Cast(p);
01441 if ( !m_a[i] )
01442 delete p;
01443 }
01444 }
01445 }
01446 }
01447 else {
01448 rc = false;
01449 }
01450 if ( !file.EndRead3dmChunk() )
01451 rc = false;
01452 }
01453 return rc;
01454 }
01455
01456 ON_BOOL32 ON_Surface::HasBrepForm() const
01457 {
01458 return true;
01459 }
01460
01461 ON_Brep* ON_Surface::BrepForm( ON_Brep* brep ) const
01462 {
01463 ON_Brep* pBrep = NULL;
01464 if ( brep )
01465 brep->Destroy();
01466
01467
01468
01469 ON_Surface* pSurface = DuplicateSurface();
01470 if ( pSurface )
01471 {
01472 if ( brep )
01473 pBrep = brep;
01474 else
01475 pBrep = new ON_Brep();
01476 if ( !pBrep->Create(pSurface) )
01477 {
01478 if ( pSurface )
01479 {
01480 delete pSurface;
01481 pSurface = NULL;
01482 }
01483 if ( !brep )
01484 delete pBrep;
01485 pBrep = NULL;
01486 }
01487 }
01488 return pBrep;
01489 }
01490
01491 void ON_Surface::DestroySurfaceTree()
01492 {
01493 DestroyRuntimeCache(true);
01494 }
01495
01496 ON_SurfaceProperties::ON_SurfaceProperties()
01497 {
01498 memset(this,0,sizeof(*this));
01499 }
01500
01501
01502 void ON_SurfaceProperties::Set( const ON_Surface* surface )
01503 {
01504 m_surface = surface;
01505
01506 if ( 0 == m_surface )
01507 {
01508 m_bIsSet = false;
01509
01510 m_bHasSingularity = false;
01511 m_bIsSingular[0] = false;
01512 m_bIsSingular[1] = false;
01513 m_bIsSingular[2] = false;
01514 m_bIsSingular[3] = false;
01515
01516 m_bHasSeam = false;
01517 m_bIsClosed[0] = false;
01518 m_bIsClosed[1] = false;
01519
01520 m_domain[0].Set(0.0,0.0);
01521 m_domain[1].Set(0.0,0.0);
01522 }
01523 else
01524 {
01525 m_bIsSet = true;
01526
01527 m_bIsSingular[0] = m_surface->IsSingular(0)?true:false;
01528 m_bIsSingular[1] = m_surface->IsSingular(1)?true:false;
01529 m_bIsSingular[2] = m_surface->IsSingular(2)?true:false;
01530 m_bIsSingular[3] = m_surface->IsSingular(3)?true:false;
01531 m_bHasSingularity = (m_bIsSingular[0] || m_bIsSingular[1] || m_bIsSingular[2] || m_bIsSingular[3]);
01532
01533 m_bIsClosed[0] = m_surface->IsClosed(0)?true:false;
01534 m_bIsClosed[1] = m_surface->IsClosed(1)?true:false;
01535 m_bHasSeam = (m_bIsClosed[0] || m_bIsClosed[1]);
01536
01537 m_domain[0] = m_surface->Domain(0);
01538 m_domain[1] = m_surface->Domain(1);
01539 }
01540
01541 }