opennurbs_offsetsurface.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_OffsetSurface,ON_SurfaceProxy,"00C61749-D430-4ecc-83A8-29130A20CF9C");
00020 
00021 ON_OffsetSurface::ON_OffsetSurface()
00022                  : m__pSrf(0)
00023 {
00024 }
00025 
00026 ON_OffsetSurface::~ON_OffsetSurface()
00027 {
00028   m_offset_function.SetBaseSurface( 0 );
00029   if ( 0 != m__pSrf && this != m__pSrf )
00030     delete m__pSrf;
00031   m__pSrf = 0;
00032   
00033 }
00034 
00035 ON_OffsetSurface::ON_OffsetSurface( const ON_OffsetSurface& src)
00036                  : ON_SurfaceProxy(src),
00037                    m__pSrf(0),
00038                    m_offset_function(src.m_offset_function)
00039 {
00040   if ( 0 != src.m__pSrf )
00041   {
00042     m__pSrf = src.ON_SurfaceProxy::DuplicateSurface();
00043     SetProxySurface(m__pSrf);
00044   }
00045   m_offset_function.SetBaseSurface( BaseSurface() );
00046 }
00047 
00048 ON_OffsetSurface& ON_OffsetSurface::operator=(const ON_OffsetSurface& src)
00049 {
00050   if ( this != &src )
00051   {
00052     if ( 0 != m__pSrf && this != m__pSrf )
00053       delete m__pSrf;
00054     m__pSrf = 0;
00055     if ( 0 != src.m__pSrf )
00056     {
00057       m__pSrf = src.ON_SurfaceProxy::DuplicateSurface();
00058       SetProxySurface(m__pSrf);
00059     }
00060     else
00061     {
00062       ON_SurfaceProxy::operator=(src);
00063     }
00064     m_offset_function = src.m_offset_function;
00065     m_offset_function.SetBaseSurface( BaseSurface() );
00066   }
00067   return *this;
00068 }
00069 
00070 ON_OffsetSurfaceFunction& ON_OffsetSurface::OffsetFunction()
00071 {
00072   return m_offset_function;
00073 }
00074 
00075 const ON_OffsetSurfaceFunction& ON_OffsetSurface::OffsetFunction() const
00076 {
00077   return m_offset_function;
00078 }
00079 
00080 
00081 bool ON_OffsetSurface::SetBaseSurface(
00082   const ON_Surface* base_surface
00083   )
00084 {
00085   bool rc = false;
00086   if ( this != base_surface )
00087   {
00088     rc = true;
00089     if ( 0 == base_surface )
00090     {
00091       if ( 0 != m__pSrf && this != m__pSrf )
00092         delete m__pSrf;
00093       m__pSrf = 0;
00094       ON_SurfaceProxy::SetProxySurface(0);
00095       m_offset_function.SetBaseSurface(0);
00096     }
00097     else if ( BaseSurface() != base_surface )
00098     {
00099       if ( 0 != m__pSrf && this != m__pSrf )
00100         delete m__pSrf;
00101       m__pSrf = 0;
00102       ON_SurfaceProxy::SetProxySurface(base_surface);
00103     }
00104     m_offset_function.SetBaseSurface( BaseSurface() );
00105   }
00106   return rc;
00107 }
00108 
00109 bool ON_OffsetSurface::SetBaseSurface(
00110       ON_Surface* base_surface, 
00111       bool bManage
00112       )
00113 {
00114   bool rc = SetBaseSurface(base_surface);
00115   if (rc && bManage )
00116     m__pSrf = base_surface;
00117   return rc;
00118 }
00119 
00120 const ON_Surface* ON_OffsetSurface::BaseSurface() const
00121 {
00122   return ProxySurface();
00123 }
00124 
00125 ON_BOOL32 ON_OffsetSurface::GetBBox(
00126        double* bbox_min,
00127        double* bbox_max,
00128        ON_BOOL32 bGrowBox
00129        ) const
00130 {
00131   ON_BOOL32 rc = ON_SurfaceProxy::GetBBox(bbox_min,bbox_max);
00132   if ( rc )
00133   {
00134     double d, distance = 0.0;
00135     int i, count = m_offset_function.m_offset_value.Count();
00136     for ( i = 0; i < count; i++ )
00137     {
00138       d = fabs(m_offset_function.m_offset_value[i].m_distance);
00139       if ( distance < d)
00140         distance = d;
00141     }
00142     distance *= 2;
00143     if ( 0 != bbox_min )
00144     {
00145       bbox_min[0] -= distance;
00146       bbox_min[1] -= distance;
00147       bbox_min[2] -= distance;
00148     }
00149     if ( 0 != bbox_max )
00150     {
00151       bbox_max[0] += distance;
00152       bbox_max[1] += distance;
00153       bbox_max[2] += distance;
00154     }
00155   }
00156   return rc;
00157 }
00158 
00159 
00160 ON_BOOL32 ON_OffsetSurface::Evaluate(
00161        double s, double t,
00162        int der_count,
00163        int v_stride,
00164        double* v,
00165        int side,
00166        int* hint
00167        ) const
00168 {
00169   int vv_stride = v_stride;
00170   double* vv = v;
00171   ON_3dVector srf_value[6];//, normal_value[3];
00172   if ( der_count < 2 )
00173   {
00174     vv = &srf_value[0].x;
00175     vv_stride = 3;
00176   }
00177 
00178   ON_BOOL32 rc = ON_SurfaceProxy::Evaluate(s,t,(der_count>2?der_count:2),vv_stride,vv,side,hint);
00179 
00180   if ( v != vv )
00181   {
00182     // save answer in v[] array
00183     v[0] = srf_value[0].x;
00184     v[1] = srf_value[0].y;
00185     v[2] = srf_value[0].z;
00186     if ( der_count > 0 )
00187     {
00188       v[v_stride]   = srf_value[1].x;
00189       v[v_stride+1] = srf_value[1].y;
00190       v[v_stride+2] = srf_value[1].z;
00191       v[2*v_stride]   = srf_value[2].x;
00192       v[2*v_stride+1] = srf_value[2].y;
00193       v[2*v_stride+2] = srf_value[2].z;
00194     }
00195   }
00196   else
00197   {
00198     srf_value[0] = v;
00199     srf_value[1] = v+v_stride;
00200     srf_value[2] = v+2*v_stride;
00201     srf_value[3] = v+3*v_stride;
00202     srf_value[4] = v+4*v_stride;
00203     srf_value[5] = v+5*v_stride;
00204   }
00205 
00206   if (rc)
00207   {
00208     double darray[21]; // 21 = ((5+1)*(5+2)/2) = enough room for der_count <= 5
00209     double* d = (der_count>5)
00210               ? (double*)onmalloc(((der_count+1)*(der_count+2))/2*sizeof(d[0]))
00211               : darray;
00212     rc = m_offset_function.EvaluateDistance(s,t,der_count,d);
00213     if (rc)
00214     {
00215       ON_3dVector N;
00216       ON_EvNormal(side,
00217                   srf_value[1], srf_value[2],
00218                   srf_value[3], srf_value[4], srf_value[5],
00219                   N);
00220       v[0] += d[0]*N.x;
00221       v[1] += d[0]*N.y;
00222       v[2] += d[0]*N.z;
00223 
00224       if ( der_count > 0 )
00225       {
00226         ON_3dVector Ns, Nt;
00227         ON_EvNormalPartials(srf_value[1], srf_value[2],
00228                   srf_value[3], srf_value[4], srf_value[5],
00229                   Ns, Nt);
00230         v[v_stride]   += d[0]*Ns.x + d[1]*N.x;
00231         v[v_stride+1] += d[0]*Ns.y + d[1]*N.y;
00232         v[v_stride+2] += d[0]*Ns.z + d[1]*N.z;
00233 
00234         v[2*v_stride]   += d[0]*Nt.x + d[2]*N.x;
00235         v[2*v_stride+1] += d[0]*Nt.y + d[2]*N.y;
00236         v[2*v_stride+2] += d[0]*Nt.z + d[2]*N.z;
00237       }
00238     }
00239     if ( d != darray )
00240       onfree(d);
00241   }
00242 
00243   return rc;
00244 }
00245 
00246 
00247 void ON_BumpFunction::EvaluateHelperLinearBump(double t, double dt, int der_count, double* value) const
00248 {
00249   value[0] = t;
00250   if (der_count>0)
00251   {
00252     value[1] = dt;
00253     if ( der_count > 1 )
00254     {
00255       der_count--;
00256       value += 2;
00257       while(der_count--)
00258         *value++ = 0.0;
00259     }
00260   }
00261 }
00262 
00263 void ON_BumpFunction::EvaluateHelperQuinticBump(double t, double dt, int der_count, double* value) const
00264 {
00265   // c(t) = (1-t)^3 * (1 + 3t + 6t^2)
00266   //bool neg = (t<0.0);
00267   //t = fabs(t);
00268   if ( fabs(t) < 1.0)
00269   {
00270     double a2 = (1-t);
00271     double a1 = a2*a2;
00272     double a = a1*a2;
00273     double b = 1.0 + t*(3.0 + 6.0*t);
00274     value[0] = a*b;
00275     if (der_count>0)
00276     {
00277       a1 *= -3.0;
00278       double b1 = 3.0 + 12.0*t;
00279       value[1] = dt*(a1*b + b1*a);
00280       //if ( neg )
00281       //  value[1] = - value[1];
00282       if ( der_count > 1 )
00283       {
00284         value[2] = dt*dt*(6.0*a2*b + 12.0*a + 2.0*a1*b1);
00285         if ( der_count > 2 )
00286         {
00287           der_count-=2;
00288           value += 3;
00289           while ( der_count-- )
00290             *value++ = 0.0;
00291         }
00292       }
00293     }
00294   }
00295   else
00296   {
00297     while ( der_count-- >= 0 )
00298       *value++ = 0.0;
00299   }
00300 }
00301 
00302 /*
00303 double EvaluateCubicBump(double t, double* d, double* dd) const
00304 {
00305   // c(t) = 1 - 3t^2 + 2t^3
00306   // CubicBump(t) = c(t)  if 0 <= t < 1
00307   //                c(-t) if -1 < t <= 0
00308   //                0 otherwise
00309   bool neg = (t<0.0);
00310   t = fabs(t);
00311   if ( t < 1.0)
00312   {
00313     // f(t) = 1 - 3t^2 + 2t^3
00314     if (d)
00315     {
00316       // f'(t) = -6t + 6t^2
00317       *d = (neg) ? (6.0*t*(1.0-t)) : (6.0*t*(t-1.0));
00318     }
00319     if (dd)
00320     {
00321       // f"(t) = -6 + 12t
00322       *dd = -6.0 + 12.0*t;
00323     }
00324     t = 1.0+t*(t*(2.0*t-3.0));
00325   }
00326   else
00327   {
00328     t = 0.0;
00329     if ( d )
00330       *d = 0.0
00331     if ( *dd )
00332       *dd = 0.0
00333   }
00334   return t
00335 }
00336 */
00337 
00338 void ON_BumpFunction::Evaluate(double s, double t, int der_count, double* value) const
00339 {
00340   double tmp[20];
00341   double* xvalue;
00342   double* yvalue;
00343   xvalue = ( der_count > 9 ) 
00344          ? ((double*)onmalloc((der_count+1)*2*sizeof(xvalue[0])))
00345          : &tmp[0];
00346   yvalue = xvalue + (der_count+1);
00347   double x = s-m_x0;
00348   const double dx = m_sx[x >= 0.0 ? 1 : 0];
00349   x *= dx;
00350   double y = t-m_y0;
00351   const double dy = m_sy[y >= 0.0 ? 1 : 0];
00352   y *= dy;
00353 
00354   if ( 5 == m_type[0] )
00355   {
00356     EvaluateHelperQuinticBump(x,dx,der_count,xvalue);
00357   }
00358   else
00359   {
00360     EvaluateHelperLinearBump(x,dx,der_count,xvalue);
00361   }
00362   if ( 5 == m_type[1] )
00363   {
00364     EvaluateHelperQuinticBump(y,dy,der_count,yvalue);
00365   }
00366   else
00367   {
00368     EvaluateHelperLinearBump(y,dy,der_count,yvalue);
00369   }
00370 
00371   int n, i, j;
00372   for ( n = 0; n <= der_count; n++ )
00373   {
00374     for ( i = n, j = 0; j <= n; i--, j++ )
00375     {
00376       *value++ = m_a*xvalue[i]*yvalue[j]; // d^nf/(ds^i dt^j)
00377     }
00378   }
00379 }
00380 
00381 
00382 bool ON_OffsetSurfaceFunction::SetBaseSurface( const ON_Surface* srf )
00383 {
00384   bool rc = false;
00385   Destroy();
00386   m_srf = srf;
00387   if ( 0 != m_srf )
00388   {
00389     m_domain[0] = m_srf->Domain(0);
00390     m_domain[1] = m_srf->Domain(1);
00391     rc = m_domain[0].IsIncreasing() && m_domain[1].IsIncreasing();
00392     if ( !rc )
00393       Destroy();
00394   }
00395   return rc;
00396 }
00397 
00398 bool ON_OffsetSurfaceFunction::SetSideTangency(
00399   int side,
00400   bool bEnable
00401   )
00402 {
00403   bool rc = false;
00404   if ( 0 <= side && side < 4 )
00405   {
00406     m_bZeroSideDerivative[side] = bEnable?true:false;
00407     m_bValid = false;
00408     rc = true;
00409   }
00410   return rc;
00411 }
00412 
00413 bool ON_OffsetSurfaceFunction::SideTangency(int side) const
00414 {
00415   bool rc = ( 0 <= side && side < 4 ) 
00416           ? m_bZeroSideDerivative[side] 
00417           : false;
00418   return rc;
00419 }
00420 
00421 int ON_OffsetSurfaceFunction::OffsetPointCount() const
00422 {
00423   return (0 != m_srf) ? m_offset_value.Count() : 0;
00424 }
00425 
00426 
00427 double ON_OffsetSurfaceFunction::DistanceAt(
00428   double s,
00429   double t
00430   ) const
00431 {
00432   double d = 0.0;
00433   EvaluateDistance( s, t, 0, &d );
00434   return d;
00435 }
00436 
00437 bool ON_OffsetSurfaceFunction::EvaluateDistance(
00438       double s,
00439       double t,
00440       int num_der,
00441       double* value
00442       ) const
00443 {
00444   const int vcnt = ((num_der+1)*(num_der+2))/2;
00445   int vi;
00446   for ( vi = 0; vi < vcnt; vi++ )
00447   {
00448     value[vi] = 0;
00449   }
00450 
00451   bool rc = const_cast<ON_OffsetSurfaceFunction*>(this)->Initialize();
00452 
00453   if (rc)
00454   {
00455     double barray[21];
00456     double* bump_value = (vcnt > 21)
00457                        ? (double*)onmalloc(vcnt*sizeof(bump_value[0]))
00458                        : barray;
00459     const int bump_count = m_bumps.Count();
00460     int bump_index, vi;
00461     for ( bump_index = 0; bump_index < bump_count; bump_index++ )
00462     {
00463       m_bumps[bump_index].Evaluate( s, t, num_der, bump_value );
00464       for ( vi = 0; vi < vcnt; vi++ )
00465       {
00466         value[vi] += bump_value[vi];
00467       }
00468     }
00469     if ( bump_value != barray )
00470       onfree(bump_value);
00471   }
00472   return rc;
00473 }
00474 
00475 
00476 ON_3dPoint ON_OffsetSurfaceFunction::PointAt(
00477   double s,
00478   double t
00479   ) const
00480 {
00481   //double d = 0.0;
00482   ON_3dPoint P;
00483   ON_3dVector N;
00484   if ( 0 != m_srf )
00485   {
00486     if ( m_srf->EvNormal(s,t,P,N) )
00487     {
00488       P = P + DistanceAt(s,t)*N;
00489     }
00490   }
00491   return P;
00492 }
00493 
00494 ON_2dPoint ON_OffsetSurfaceFunction::OffsetSurfaceParameter(int i) const
00495 {
00496   ON_2dPoint p(ON_UNSET_VALUE,ON_UNSET_VALUE);
00497   if ( 0 != m_srf && i >= 0 && i < m_offset_value.Count() )
00498   {
00499     p.x = m_offset_value[i].m_s;
00500     p.y = m_offset_value[i].m_t;
00501   }
00502   return p;
00503 }
00504 
00505 const ON_Surface* ON_OffsetSurfaceFunction::BaseSurface() const
00506 {
00507   return m_srf;
00508 }
00509 
00510 double ON_OffsetSurfaceFunction::OffsetDistance(int i) const
00511 {
00512   double d = ON_UNSET_VALUE;
00513   if ( 0 != m_srf && i >= 0 && i < m_offset_value.Count() )
00514   {
00515     d = m_offset_value[i].m_distance;
00516   }
00517   return d;
00518 }
00519 
00520 bool ON_OffsetSurfaceFunction::SetOffsetPoint(
00521   double s,
00522   double t,
00523   double distance,
00524   double radius
00525   )
00526 {
00527   bool rc = false;
00528   if ( ON_IsValid(s) && ON_IsValid(t) && ON_IsValid(distance) && ON_IsValid(radius) )
00529   {
00530     double u = m_domain[0].NormalizedParameterAt(s);
00531 
00532     // 14 Jan 2008, Mikko, TRR 29861:
00533     // Changing the clamping to happen when the 
00534     // point is outside or nearly outside the domain.
00535     const double dTol = ON_SQRT_EPSILON; // tiny border around untrimmed edges
00536 
00537     if ( u < dTol)
00538     {
00539       s = m_domain[0][0];
00540       u = 0.0;
00541     }
00542     if ( u > 1.0-dTol)
00543     {
00544       s = m_domain[0][1];
00545       u = 1.0;
00546     }
00547 
00548     double v = m_domain[1].NormalizedParameterAt(t);
00549     if ( v < dTol)
00550     {
00551       t = m_domain[1][0];
00552       v = 0.0;
00553     }
00554     if ( v > 1.0-dTol)
00555     {
00556       t = m_domain[1][1];
00557       v = 1.0;
00558     }
00559 
00560     if ( u >= 0.0 && u <= 1.0 && v >= 0.0 && v <= 1.0 )
00561     {
00562       ON_OffsetSurfaceValue offset_value;
00563       offset_value.m_s = s;
00564       offset_value.m_t = t;
00565       offset_value.m_distance = distance;
00566       offset_value.m_radius = (radius > 0.0) ? radius : 0.0;
00567       offset_value.m_index = (int)((u + v*4096.0)*4096.0);
00568       int i;
00569       for ( i = 0; i < m_offset_value.Count(); i++ )
00570       {
00571         if ( m_offset_value[i].m_index == offset_value.m_index )
00572         {
00573           m_offset_value[i] = offset_value;
00574           break;
00575         }
00576       }
00577       if (i == m_offset_value.Count())
00578       {
00579         m_offset_value.Append(offset_value);
00580         m_bumps.SetCount(0);
00581         m_bValid = false;
00582       }
00583       rc = true;
00584     }
00585   }
00586   return rc;
00587 }
00588 
00589 
00590 bool ON_OffsetSurfaceFunction::SetDistance( int index, double distance)
00591 {
00592   int count = m_offset_value.Count();
00593   if( index < 0 || index > count-1)
00594     return false;
00595 
00596   m_offset_value[index].m_distance = distance;
00597   m_bValid = false;
00598 
00599   return true;
00600 }
00601 
00602 
00603 bool ON_OffsetSurfaceFunction::SetPoint( int index, double s, double t)
00604 {
00605   int count = m_offset_value.Count();
00606   if( index < 0 || index > count-1)
00607     return false;
00608 
00609   m_offset_value[index].m_s = s;
00610   m_offset_value[index].m_t = t;
00611   m_bValid = false;
00612 
00613   return true;
00614 }
00615 
00616 
00617 void ON_OffsetSurfaceFunction::Destroy()
00618 {
00619   m_srf = 0;
00620   m_bZeroSideDerivative[0] = false;
00621   m_bZeroSideDerivative[1] = false;
00622   m_bZeroSideDerivative[2] = false;
00623   m_bZeroSideDerivative[3] = false;
00624 
00625   m_domain[0].Destroy();
00626   m_domain[1].Destroy();
00627   m_bumps.SetCount(0);
00628   m_bValid = false;
00629 }
00630 
00631 
00632 
00633 ON_OffsetSurfaceFunction::ON_OffsetSurfaceFunction()
00634 {
00635   Destroy();
00636 }
00637 
00638 ON_OffsetSurfaceFunction::~ON_OffsetSurfaceFunction()
00639 {
00640   Destroy();
00641 }
00642 
00643 bool ON_OffsetSurfaceFunction::Initialize()
00644 {
00645   const int count = m_offset_value.Count();
00646   if ( !m_bValid && 0 != m_srf && count > 0)
00647   {
00648     ON_Workspace ws;
00649     m_bumps.SetCount(0);
00650     m_bumps.Reserve(count);
00651     int i;
00652     double a,b,ds,dt;
00653     
00654     for (i = 0; i < count; i++ )
00655     {
00656       ON_BumpFunction& bump = m_bumps.AppendNew();
00657       ON_OffsetSurfaceValue offset_value = m_offset_value[i];
00658       double ds0 = offset_value.m_s - m_domain[0][0];
00659       double ds1 = m_domain[0][1] - offset_value.m_s;
00660       if ( 0.0 == ds0 )
00661         ds0 = -ds1;
00662       else if ( 0.0 == ds1 )
00663         ds1 = -ds0;
00664       double dt0 = offset_value.m_t - m_domain[1][0];
00665       double dt1 = m_domain[1][1] - offset_value.m_t;
00666       if ( 0.0 == dt0 )
00667         dt0 = -dt1;
00668       else if ( 0.0 == dt1 )
00669         dt1 = -dt0;
00670 
00671       // default is a large cubic bump
00672       bump.m_point.x = offset_value.m_s;
00673       bump.m_point.y = offset_value.m_t;
00674       bump.m_x0 = bump.m_point.x;
00675       bump.m_y0 = bump.m_point.y;
00676 
00677       bump.m_sx[0] = -1.0/ds0;
00678       bump.m_sx[1] =  1.0/ds1;
00679       bump.m_sy[0] = -1.0/dt0;
00680       bump.m_sy[1] =  1.0/dt1;
00681       bump.m_type[0] = 5;
00682       bump.m_type[1] = 5;
00683       bump.m_a = 1.0;
00684 
00685       if ( offset_value.m_radius > 0.0 )
00686       {
00687         // user specified cubic bump size
00688         ON_3dPoint Pt;
00689         ON_3dVector Ds, Dt;
00690         if ( m_srf->Ev1Der(offset_value.m_s,offset_value.m_t,Pt,Ds,Dt) )
00691         {
00692           ds = (ds0>ds1) ? ds0 : ds1;
00693           dt = (dt0>dt1) ? dt0 : dt1;
00694           a = Ds.Length();
00695           if ( a > ON_ZERO_TOLERANCE )
00696           {
00697             b = offset_value.m_radius/a;
00698             if ( b < ds )
00699               ds = b;
00700           }
00701           a = Dt.Length();
00702           if ( a > ON_ZERO_TOLERANCE )
00703           {
00704             b = offset_value.m_radius/a;
00705             if ( b < dt )
00706               dt = b;
00707           }
00708           if ( !m_bZeroSideDerivative[0] || dt < dt0 )
00709             dt0 = dt;
00710           if ( !m_bZeroSideDerivative[1] || ds < ds1 )
00711             ds1 = ds;
00712           if ( !m_bZeroSideDerivative[2] || dt < dt1 )
00713             dt1 = dt;
00714           if ( !m_bZeroSideDerivative[3] || ds < ds0 )
00715             ds0 = ds;
00716           bump.m_sx[0] = -1.0/ds0;
00717           bump.m_sx[1] =  1.0/ds1;
00718           bump.m_sy[0] = -1.0/dt0;
00719           bump.m_sy[1] =  1.0/dt1;
00720         }
00721       }
00722       else if ( bump.m_point.x == m_domain[0][0] &&  bump.m_point.y == m_domain[1][0] )
00723       {
00724         // SW corner bilinear bump
00725         if ( !m_bZeroSideDerivative[1] && !m_bZeroSideDerivative[3] )
00726         {
00727           bump.m_type[0] = 1;
00728           bump.m_x0 = m_domain[0][1];
00729           bump.m_sx[0] = -1.0/m_domain[0].Length();
00730           bump.m_sx[1] = -1.0/m_domain[0].Length();
00731         }
00732         if ( !m_bZeroSideDerivative[0] && !m_bZeroSideDerivative[2] )
00733         {
00734           bump.m_type[1] = 1;
00735           bump.m_y0 = m_domain[1][1];
00736           bump.m_sy[0] = -1.0/m_domain[1].Length();
00737           bump.m_sy[1] = -1.0/m_domain[1].Length();
00738         }
00739       }
00740       else if ( bump.m_point.x == m_domain[0][1] &&  bump.m_point.y == m_domain[1][0] )
00741       {
00742         // SE corner bilinear bump
00743         if ( !m_bZeroSideDerivative[1] && !m_bZeroSideDerivative[3] )
00744         {
00745           bump.m_type[0] = 1;
00746           bump.m_x0 = m_domain[0][0];
00747           bump.m_sx[0] = 1.0/m_domain[0].Length();
00748           bump.m_sx[1] = 1.0/m_domain[0].Length();
00749         }
00750         if ( !m_bZeroSideDerivative[0] && !m_bZeroSideDerivative[2] )
00751         {
00752           bump.m_type[1] = 1;
00753           bump.m_y0 = m_domain[1][1];
00754           bump.m_sy[0] = -1.0/m_domain[1].Length();
00755           bump.m_sy[1] = -1.0/m_domain[1].Length();
00756         }
00757       }
00758       else if ( bump.m_point.x == m_domain[0][1] &&  bump.m_point.y == m_domain[1][1] )
00759       {
00760         // NE corner bilinear bump
00761         if ( !m_bZeroSideDerivative[1] && !m_bZeroSideDerivative[3] )
00762         {
00763           bump.m_type[0] = 1;
00764           bump.m_x0 = m_domain[0][0];
00765           bump.m_sx[0] = 1.0/m_domain[0].Length();
00766           bump.m_sx[1] = 1.0/m_domain[0].Length();
00767         }
00768         if ( !m_bZeroSideDerivative[0] && !m_bZeroSideDerivative[2] )
00769         {
00770           bump.m_type[1] = 1;
00771           bump.m_y0 = m_domain[1][0];
00772           bump.m_sy[0] = 1.0/m_domain[1].Length();
00773           bump.m_sy[1] = 1.0/m_domain[1].Length();
00774         }
00775       }
00776       else if ( bump.m_point.x == m_domain[0][0] &&  bump.m_point.y == m_domain[1][1] )
00777       {
00778         // NW corner bilinear bump
00779         if ( !m_bZeroSideDerivative[1] && !m_bZeroSideDerivative[3] )
00780         {
00781           bump.m_x0 = m_domain[0][1];
00782           bump.m_sx[0] = -1.0/m_domain[0].Length();
00783           bump.m_sx[1] = -1.0/m_domain[0].Length();
00784           bump.m_type[0] = 1;
00785         }
00786         if ( !m_bZeroSideDerivative[0] && !m_bZeroSideDerivative[2] )
00787         {
00788           bump.m_y0 = m_domain[1][0];
00789           bump.m_sy[0] = 1.0/m_domain[1].Length();
00790           bump.m_sy[1] = 1.0/m_domain[1].Length();
00791           bump.m_type[1] = 1;
00792         }
00793       }
00794     }
00795 
00796 
00797     ON_Matrix M(count,count);
00798     double* B = (double*)onmalloc(2*count*sizeof(*B));
00799     double* X = B + count;
00800     int j;
00801     for ( i = 0; i < count; i++ ) 
00802     {
00803       ON_2dPoint p = m_bumps[i].m_point;
00804       B[i] = m_offset_value[i].m_distance;
00805       for ( j = 0; j < count; j++ )
00806       {
00807         M[i][j] = m_bumps[j].ValueAt(p.x,p.y);
00808       }
00809     }
00810     int rank = M.RowReduce(ON_ZERO_TOLERANCE,B);
00811     if ( count == rank )
00812     {
00813       if ( M.BackSolve(ON_ZERO_TOLERANCE,count,B,X) )
00814       {
00815         m_bValid = true;
00816       }
00817     }
00818 
00819     if ( !m_bValid )
00820     {
00821 #if 0 //defined(TL2_MATRIX_INC_)
00822       // Have access to SVD - try it
00823       for ( i = 0; i < count; i++ ) 
00824       {
00825         ON_2dPoint p = m_bumps[i].m_point;
00826         B[i] = m_offset_value[i].m_distance;
00827         for ( j = 0; j < count; j++ )
00828         {
00829           M[i][j] = m_bumps[j].ValueAt(p.x,p.y);
00830         }
00831       }
00832       ON_Matrix U, V;
00833       double* diagonal = (double*)onmalloc(2*count*sizeof(*diagonal));
00834       double* inverted_diagonal = diagonal + count;
00835       if ( TL2_MatrixSVD(M,U,diagonal,V,30) )
00836       {
00837         rank = TL2_MatrixSVDInvertDiagonal(count,diagonal,inverted_diagonal);
00838         if ( rank > 0 )
00839         {
00840           if ( TL2_MatrixSVDSolve(U,inverted_diagonal,V,1,B,1,X) )
00841           {
00842             m_bValid = true;
00843           }
00844         }
00845       }
00846 #endif
00847     }
00848 
00849     if ( m_bValid )
00850     {
00851       for ( i = 0; i < count; i++ )
00852       {
00853         m_bumps[i].m_a = X[i];
00854       }
00855     }
00856 
00857     onfree(B);
00858   }
00859   return m_bValid;
00860 }
00861 
00862 ON_BumpFunction::ON_BumpFunction()
00863 {
00864   m_point.x = ON_UNSET_VALUE;
00865   m_point.y = ON_UNSET_VALUE;
00866   m_type[0] = 0;
00867   m_type[1] = 0;
00868   m_x0 = 0.0;
00869   m_y0 = 0.0;
00870   m_sx[0] = 0.0;
00871   m_sx[1] = 0.0;
00872   m_sy[0] = 0.0;
00873   m_sy[1] = 0.0;
00874   m_a = 0.0;
00875 }
00876 
00877 bool ON_BumpFunction::operator==(const ON_BumpFunction& other) const
00878 {
00879   return ( m_point.x == other.m_point.x && m_point.y == other.m_point.y );
00880 }
00881 
00882 bool ON_BumpFunction::operator<(const ON_BumpFunction& other) const
00883 {
00884   return ( (m_point.x < other.m_point.x) || (m_point.x == other.m_point.x && m_point.y < other.m_point.y) );
00885 }
00886 
00887 bool ON_BumpFunction::operator>(const ON_BumpFunction& other) const
00888 {
00889   return ( (m_point.x > other.m_point.x) || (m_point.x == other.m_point.x && m_point.y > other.m_point.y) );
00890 }
00891 
00892 double ON_BumpFunction::ValueAt(
00893   double s,
00894   double t
00895   ) const
00896 {
00897   double v;
00898   Evaluate(s,t,0,&v);
00899   return v;
00900 }
00901 


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