Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members  

gunu_knot_removal.cpp

Go to the documentation of this file.
00001 /* LIBGUL - Geometry Utility Library
00002  * Copyright (C) 1998-1999 Norbert Irmer
00003  *
00004  * This library is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU Library General Public
00006  * License as published by the Free Software Foundation; either
00007  * version 2 of the License, or (at your option) any later version.
00008  *
00009  * This library is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * Library General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Library General Public
00015  * License along with this library; if not, write to the
00016  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
00017  * Boston, MA 02111-1307, USA.
00018  */
00019 
00020 #include "stdafx.h"
00021 
00022 #include <iostream>
00023 
00024 #include "gul_types.h"
00025 #include "gul_float.h"
00026 #include "gul_vector.h"
00027 #include "guma_sorting.h"
00028 #include "gunu_basics.h"
00029 #include "gunu_parametrize.h"
00030 #include "gunu_global_approximate.h"
00031 #include "gunu_derivatives.h"
00032 
00033 #include "gunu_knot_removal.h"
00034 
00035 namespace gunu {
00036 
00037 using gul::Ptr;
00038 using gul::hpoint2;
00039 using gul::point2;
00040 using gul::hpoint;
00041 using gul::point;
00042 using gul::rel_equal;
00043 using gul::abs_equal;
00044 using gul::euclidian_distance;
00045 using gul::length;
00046 using gul::rtr;
00047 using guma::_BinarySearch;
00048 
00049 /*------------------------------------------------------------------------
00050   Tries to remove the knot U[r] of multiplicity 's' (U[r]!=U[r+1]) from
00051   'U' 'num' times. It returns how often the knot actually was removed,
00052   the tolerance 'tol' is used to decide if a knot is removable.
00053   The new knot vector and control points overwrite the old ones.
00054 ------------------------------------------------------------------------*/
00055 
00056 template< class T, class HP >
00057 int RemoveCurveKnot( int num, int r, int s, T tol, 
00058                      int n, int p, Ptr<T>& U, Ptr<HP>& Pw )
00059 
00060 {
00061   T u,alfi,alfj;
00062   int m,ord,fout,last,first,t,off,i,j,ii,jj,k;
00063   bool remflag;
00064   Ptr<HP> temp;
00065 
00066   temp.reserve_pool(2*p+1);
00067   u = U[r];
00068   
00069   m = n+p+1;
00070   ord = p+1;
00071   
00072   fout = (2*r - s - p)/2;
00073   last = r-s;
00074   first = r-p;
00075   
00076   for( t = 0; t < num; t++ )
00077   {
00078     off = first-1;
00079     temp[0] = Pw[off];
00080     temp[last+1-off] = Pw[last+1];
00081 
00082     i = first;
00083     j = last;
00084     ii = 1;
00085     jj = last - off;
00086     remflag = false;
00087     
00088     while( j-i > t )
00089     {
00090       alfi = (u-U[i])/(U[i+ord+t]-U[i]);
00091       alfj = (u-U[j-t])/(U[j+ord]-U[j-t]);
00092       temp[ii] = ((T)1/alfi)*(Pw[i]-((T)1-alfi)*temp[ii-1]);
00093       temp[jj] = ((T)1/((T)1-alfj))*(Pw[j]-alfj*temp[jj+1]);
00094       
00095       i = i+1;
00096       j = j-1;
00097       ii = ii+1;
00098       jj = jj-1;
00099     }
00100 
00101     if( j-i < t )
00102     {
00103       if( abs_equal(temp[ii-1], temp[jj+1], tol) )
00104         remflag = true;
00105     }
00106     else
00107     {
00108       alfi = (u-U[i])/(U[i+ord+t]-U[i]);
00109       if( abs_equal( Pw[i], alfi*temp[ii+t+1] + ((T)1-alfi)*temp[ii-1], tol) )
00110         remflag = true;
00111     }
00112     
00113     if( !remflag )
00114       break;
00115     
00116     i = first;
00117     j = last;
00118     
00119     while( j-i > t )
00120     {
00121       Pw[i] = temp[i-off];
00122       Pw[j] = temp[j-off];
00123 
00124       i = i+1;
00125       j = j-1;
00126     }
00127       
00128     first = first-1;
00129     last = last+1;    
00130   }
00131   
00132   if( t == 0 )
00133     return 0;
00134   
00135   for( k = r+1; k <= m; k++ )
00136     U[k-t] = U[k];
00137     
00138   i = j = fout;
00139   
00140   for( k = 1; k < t; k++ )
00141   {
00142     if( k%2 )
00143       i = i+1;
00144     else
00145       j = j-1;
00146   }
00147   
00148   for( k = i+1; k <= n; k++, j++ )
00149   {
00150     Pw[j] = Pw[k];
00151   }  
00152     
00153   return t;
00154 }
00155 
00156 // template instantiation
00157 template int RemoveCurveKnot( 
00158                 int num, int r, int s, float tol, 
00159                 int n, int p, Ptr<float>& U, Ptr< hpoint<float> >& Pw );
00160 template int RemoveCurveKnot( 
00161                 int num, int r, int s, float tol, 
00162                 int n, int p, Ptr<float>& U, Ptr< hpoint2<float> >& Pw );
00163 
00164 template int RemoveCurveKnot( 
00165                 int num, int r, int s, double tol, 
00166                 int n, int p, Ptr<double>& U, Ptr< point<double> >& Pw );
00167 template int RemoveCurveKnot( 
00168                 int num, int r, int s, double tol, 
00169                 int n, int p, Ptr<double>& U, Ptr< point2<double> >& Pw );
00170 
00171 
00172 /*------------------------------------------------------------------------
00173   Removes the knot U[r] of multiplicity 's' (U[r]!=U[r+1]) from
00174   'U' 'num' times (without tolerance check). 
00175   The new knot vector and control points overwrite the old ones.
00176 ------------------------------------------------------------------------*/
00177 
00178 template< class T, class HP >
00179 void RemoveCurveKnot( int num, int r, int s, 
00180                      int n, int p, Ptr<T>& U, Ptr<HP>& Pw )
00181 
00182 {
00183   T u,alfi,alfj;
00184   int m,ord,fout,last,first,t,off,i,j,ii,jj,k;
00185   Ptr<HP> temp;
00186 
00187   if( num == 0 )  // nothing to do
00188     return;
00189 
00190   temp.reserve_pool(2*p+1);
00191   u = U[r];
00192   
00193   m = n+p+1;
00194   ord = p+1;
00195   
00196   fout = (2*r - s - p)/2;
00197   last = r-s;
00198   first = r-p;
00199   
00200   for( t = 0; t < num; t++ )
00201   {
00202     off = first-1;
00203     temp[0] = Pw[off];
00204     temp[last+1-off] = Pw[last+1];
00205 
00206     i = first;
00207     j = last;
00208     ii = 1;
00209     jj = last - off;
00210       
00211     while( j-i > t )
00212     {
00213       alfi = (u-U[i])/(U[i+ord+t]-U[i]);
00214       alfj = (u-U[j-t])/(U[j+ord]-U[j-t]);
00215       temp[ii] = ((T)1/alfi)*(Pw[i]-((T)1-alfi)*temp[ii-1]);
00216       temp[jj] = ((T)1/((T)1-alfj))*(Pw[j]-alfj*temp[jj+1]);
00217       
00218       i = i+1;
00219       j = j-1;
00220       ii = ii+1;
00221       jj = jj-1;
00222     }
00223 
00224     i = first;
00225     j = last;
00226     
00227     while( j-i > t )
00228     {
00229       Pw[i] = temp[i-off];
00230       Pw[j] = temp[j-off];
00231 
00232       i = i+1;
00233       j = j-1;
00234     }
00235       
00236     first = first-1;
00237     last = last+1;    
00238   }
00239     
00240   for( k = r+1; k <= m; k++ )
00241     U[k-t] = U[k];
00242     
00243   i = j = fout;
00244   
00245   for( k = 1; k < t; k++ )
00246   {
00247     if( k%2 )
00248       i = i+1;
00249     else
00250       j = j-1;
00251   }
00252   
00253   for( k = i+1; k <= n; k++, j++ )
00254   {
00255     Pw[j] = Pw[k];
00256   }  
00257     
00258   return;
00259 }
00260 
00261 // template instantiation
00262 template void RemoveCurveKnot( 
00263                 int num, int r, int s,
00264                 int n, int p, Ptr<float>& U, Ptr< hpoint<float> >& Pw );
00265 template void RemoveCurveKnot( 
00266                 int num, int r, int s,
00267                 int n, int p, Ptr<float>& U, Ptr< hpoint2<float> >& Pw );
00268 
00269 template void RemoveCurveKnot( 
00270                 int num, int r, int s,
00271                 int n, int p, Ptr<double>& U, Ptr< point<double> >& Pw );
00272 template void RemoveCurveKnot( 
00273                 int num, int r, int s,
00274                 int n, int p, Ptr<double>& U, Ptr< point2<double> >& Pw );
00275 
00276 
00277 /*-----------------------------------------------------------------------------
00278   Calculates a knot removal error factor
00279 -----------------------------------------------------------------------------*/
00280 template< class T, class HP >
00281 T RemovalError( int r, int s, int n, int p, const Ptr<T>& U, const Ptr<HP>& Pw )
00282 
00283 {
00284   T u,alfi,alfj,Br;
00285   int ord,last,first,off,i,j,ii,jj;
00286   Ptr<HP> temp;
00287 
00288   temp.reserve_pool(2*p+1);
00289   u = U[r];
00290   
00291   ord = p+1;
00292   last = r-s;
00293   first = r-p;
00294 
00295   off = first-1;
00296 
00297   temp[0] = Pw[off];
00298   temp[last+1-off] = Pw[last+1];
00299 
00300   i = first;
00301   j = last;
00302   ii = 1;
00303   jj = last - off;
00304   
00305   while( j-i > 0 )
00306   {
00307     alfi = (u-U[i])/(U[i+ord]-U[i]);
00308     alfj = (u-U[j])/(U[j+ord]-U[j]);
00309 
00310     temp[ii] = ((T)1/alfi)*(Pw[i]-((T)1-alfi)*temp[ii-1]);
00311     temp[jj] = ((T)1/((T)1-alfj))*(Pw[j]-alfj*temp[jj+1]);
00312       
00313     i = i+1;
00314     j = j-1;
00315     ii = ii+1;
00316     jj = jj-1;
00317   }
00318 
00319   if( j-i < 0 )
00320   {
00321     Br = euclidian_distance( temp[ii-1], temp[jj+1] );
00322   }
00323   else
00324   {
00325     alfi = (u-U[i])/(U[i+ord]-U[i]);
00326     Br = euclidian_distance( Pw[i], alfi*temp[ii+1] + ((T)1-alfi)*temp[ii-1] );
00327   }
00328     
00329   return Br; 
00330 }
00331 
00332 // template instantiation
00333 template float RemovalError( 
00334       int r, int s, int n, int p, const Ptr<float>& U, 
00335           const Ptr< point<float> >& Pw );
00336 
00337 /*----------------------------------------------------------------------
00338  Calculates the number of distinct knots of a knot vector, and their 
00339  multiplicities
00340 ----------------------------------------------------------------------*/
00341 
00342 template< class T >
00343 int GetMultiplicities( int n, int p, const Ptr<T>& U, Ptr<int>& S, Ptr<int>& R )
00344 {
00345   int i,k,s;
00346 
00347   k = 0;
00348   i = p+1;
00349 
00350   while( i <= n )  
00351   {
00352     s = 1;
00353 
00354     while( U[i+s] == U[i] )
00355       s++;
00356 
00357     S[k] = s;
00358     R[k] = i + s - 1;
00359 
00360     i += s;
00361     k++;
00362   }
00363 
00364   return k;
00365 }
00366 
00367 // template instantiation
00368 template int GetMultiplicities( 
00369              int n, int p, const Ptr<float>& U, Ptr<int>& S, Ptr<int>& R );
00370 
00371 
00372 
00373 /*-------------------------------------------------------------------------
00374   calc range of parameter indices which fall into the domain of each basis
00375   function
00376 --------------------------------------------------------------------------*/
00377 
00378 template< class T >
00379 GULAPI void CalcParameterRanges(
00380        int           n,
00381        int           p,
00382        const Ptr<T>& U,
00383        int           nP,
00384        const Ptr<T>& P,
00385        Ptr<int>&     R,
00386        Ptr<int>&     S )
00387 
00388 {
00389   int   i,ilo,ihi;
00390   bool  finished_early;
00391 
00392   ilo = 0;
00393   ihi = 0;
00394   finished_early = false;
00395 
00396   for( i = 0; i <= n; i++ )
00397   {
00398     while( P[ilo] < U[i] ) // search first param. >= ulo
00399     {
00400       ilo++;
00401 
00402       // if all remaining parameter ranges are empty
00403       if( ilo >= nP )
00404       {
00405         finished_early = true;
00406         break;
00407       }
00408     }
00409 
00410     if( finished_early )
00411       break;
00412 
00413     R[i] = ilo;
00414 
00415     while( U[i+p+1] >= P[ihi] ) // search last param. <= uhi
00416     {
00417       ihi++;
00418       if( ihi == nP )
00419         break;
00420     }
00421     ihi--;                  
00422 
00423     // if parameter range is empty, start next search with 0
00424     // again
00425     if( ihi == -1 )
00426     {
00427       ihi = 0;
00428       S[i] = 0;
00429     }
00430     else
00431     {
00432       S[i] = ihi-ilo+1;
00433 
00434       // if not the last interval, skip last point, if it is on
00435       // the interval end
00436 
00437       if( (i+p+1 <= n) && (P[ihi] == U[i+p+1]) ) 
00438       {  
00439         S[i]--;
00440         ihi--;
00441         if( ihi < 0 )
00442           ihi = 0;
00443       }
00444     }
00445   }
00446 
00447   if( finished_early )
00448   {
00449     for( ; i <= n; i++ )
00450     {
00451       R[i] = nP;
00452       S[i] = 0;
00453     }
00454   }
00455 }
00456 // template instantiation
00457 template GULAPI void CalcParameterRanges(
00458        int               n,
00459        int               p,
00460        const Ptr<float>& U,
00461        int               nP,
00462        const Ptr<float>& P,
00463        Ptr<int>&         R,
00464        Ptr<int>&         S );
00465 
00466 
00467 /*-------------------------------------------------------------------------
00468   calculates the maximum number of indices which fall into one kotspan
00469   (this function is a hack, like the function above :)
00470 --------------------------------------------------------------------------*/
00471 
00472 template< class T >
00473 GULAPI int CalcMaxPointsPerSpan(
00474        int           n,
00475        int           p,
00476        const Ptr<T>& U,
00477        int           nP,
00478        const Ptr<T>& P )
00479 {
00480   int i1,i2,i;
00481   T u1,u2;
00482   int maxpps;
00483 
00484   i = 1;
00485   i1 = 0;
00486   i2 = 0;
00487   maxpps = 0;
00488 
00489   for(;;)
00490   {
00491     // search next knotspan [u1,u2]
00492     while( (i <= n+p+1) && (U[i] == U[i-1]) ) i++;
00493     if( i == n+p+2 ) break; // not found
00494     u1 = U[i-1];
00495     u2 = U[i];
00496     i++;
00497 
00498     // search first parameter >= u1
00499     while( (i1 < nP) && (P[i1] < u1) ) i1++;
00500     if( (i1 == nP) || (P[i1] < u1) ) // not found
00501       break;
00502   
00503     // search last parameter <= u2
00504     while( (i2 < nP) && (P[i2] <= u2) ) i2++;
00505     i2--;
00506     if( (i2 < 0) || (P[i2] > u2) ) // not found
00507       continue;
00508  
00509     if( i2-i1+1 > maxpps )
00510       maxpps = i2-i1+1;
00511 
00512     i1 = i2;
00513   }
00514 
00515   return maxpps;
00516 }
00517 
00518 /*----------------------------------------------------------------------
00519   Removes (roughly) as many knots as possible from a curve. it returns the
00520   index of the last inner knot of the new knot vector (new 'n'). U,Pw,
00521   and QE are input _and_ output arrays.
00522 -----------------------------------------------------------------------*/  
00523 template< class T, class EP >
00524 GULAPI int RemoveCurveKnots(
00525         int             n,
00526         int             p,
00527         Ptr<T>&         U,
00528         Ptr<EP>&        Pw,  
00529         int             nQ,
00530         const Ptr<T>&   QU,
00531         T               tol,
00532         Ptr<T>&         QE )
00533 {
00534   Ptr<int> R,S,QR,QS;
00535   Ptr<T>   B,newerr;
00536   int      i,mini,nK,r,s,k,ilo,ihi,dummi;
00537   T        minB,N,alfa;
00538   bool     removable;
00539 
00540   S.reserve_pool(n-p);
00541   R.reserve_pool(n-p);
00542   B.reserve_pool(n-p);
00543   QR.reserve_pool(n+1);
00544   QS.reserve_pool(n+1);
00545   newerr.reserve_pool(nQ);
00546 
00547   nK = GetMultiplicities(n,p,U,S,R);
00548   if( nK == 0 ) return n;  // nothing to remove
00549 
00550   // calc removal error factors for each distinct interior knot
00551 
00552   minB = B[0] = RemovalError(R[0],S[0],n,p,U,Pw);
00553   mini = 0;
00554 
00555   for( i = 1; i < nK; i++ )
00556   {
00557     B[i] = RemovalError(R[i],S[i],n,p,U,Pw);
00558     if( B[i] <= minB )
00559     {
00560       minB = B[i];
00561       mini = i;
00562     }
00563   }
00564 
00565   CalcParameterRanges(n,p,U,nQ,QU,QR,QS);
00566 
00567   for(;;)
00568   {
00569     if( minB == rtr<T>::maximum() ) break;
00570 
00571     r = R[mini];
00572     s = S[mini];
00573 
00574     removable = true;
00575 
00576     if( ((p+s)%2) == 0 )
00577     {
00578       k = (p+s)/2; 
00579 
00580       for( i = QR[r-k]; i < QR[r-k]+QS[r-k]; i++ ) 
00581       {
00582         N = CalcBasisFunction(p,r-k,QU[i],n,U);
00583         newerr[i] = N*minB + QE[i]; 
00584         if( newerr[i] > tol )
00585         {
00586           removable = false;
00587           B[mini] = rtr<T>::maximum();
00588           break;
00589         }
00590       }
00591       if( removable )
00592       {
00593         for( i = QR[r-k]; i < QR[r-k]+QS[r-k]; i++ ) 
00594           QE[i] = newerr[i];
00595       }
00596     }
00597     else
00598     {
00599       k = (p+s+1)/2;
00600       alfa = (U[r]-U[r-k+1])/(U[r-k+p+2]-U[r-k+1]);
00601       minB *= (T)1 - alfa;
00602 
00603       for( i = QR[r-k+1]; i < QR[r-k+1]+QS[r-k+1]; i++ ) 
00604       {
00605         N = CalcBasisFunction(p,r-k+1,QU[i],n,U);
00606         newerr[i] = N*minB + QE[i]; 
00607         if( newerr[i] > tol )
00608         {
00609           removable = false;
00610           B[mini] = rtr<T>::maximum();
00611           break;
00612         }
00613       }
00614       if( removable )
00615       {
00616         for( i = QR[r-k+1]; i < QR[r-k+1]+QS[r-k+1]; i++ ) 
00617           QE[i] = newerr[i];
00618       }
00619     }
00620   
00621     if( removable )
00622     {
00623       RemoveCurveKnot(1,r,s,n,p,U,Pw);
00624 
00625       // U now contains the new knot vector
00626       n--;
00627 
00628       // remove entries for removed knot/basis function
00629 
00630       if( s == 1 )
00631       {
00632         nK--;
00633         if( nK == 0 ) break;  // no more internal knots 
00634       
00635         for( i = mini; i < nK; i++ )
00636         {
00637           B[i] = B[i+1];
00638           R[i] = R[i+1]-1;
00639           S[i] = S[i+1];
00640         }
00641       }
00642       else
00643       {
00644         S[mini]--;
00645 
00646         for( i = mini; i < nK; i++ )
00647           R[i]--;
00648       } 
00649 
00650       // calculate changed 'Br' values:
00651       
00652       // for these inner knots new span index = old span index:
00653 
00654       i = mini-1;
00655       while( (i >= 0) && (R[i] >= r-p) )
00656       {
00657         B[i] = RemovalError(R[i],S[i],n,p,U,Pw);
00658         i--;
00659       }
00660 
00661       // for these inner knots new span index = old span index - 1:
00662 
00663       i = mini;
00664       while( (i < nK) && (R[i] <= r+p-s) )
00665       {
00666         B[i] = RemovalError(R[i],S[i],n,p,U,Pw);
00667         i++;
00668       }
00669 
00670       // update parameter index ranges
00671       for( i = r-p-1; i <= r-s; i++ )
00672       {
00673         if( QS[i] == 0 ) // if parameter range was empty for this basis func.
00674         {
00675           _BinarySearch(U[i],nQ,QU,&dummi,&ilo);
00676           QR[i] = ihi = ilo;
00677         }
00678         else
00679           ilo = ihi = QR[i];
00680 
00681         if( ilo < 0 ) continue; // still no points in this domain
00682   
00683         while( QU[ihi] <= U[i+p+1] )
00684         {
00685           ihi++;
00686           if( ihi >= nQ ) break;
00687         }
00688         ihi--;
00689 
00690         // if not the last interval, skip last point if it is on
00691         // the interval end
00692         if( (i+p+1 <= n)  && (QU[ihi] == U[i+p+1]) )
00693           ihi--;        
00694 
00695         QS[i] = ihi-ilo+1;
00696       }
00697 
00698       // remove parameter range entries for removed basis function
00699 
00700       for( i = r; i <= n; i++ )
00701       {
00702         QR[i] = QR[i+1];
00703         QS[i] = QS[i+1];
00704       }
00705     }      
00706 
00707     // search knot with smallest 'Br'
00708     mini = 0;
00709     minB = B[0];
00710     for( i = 1; i < nK; i++ )
00711     {
00712       if( B[i] <= minB )
00713       {
00714         mini = i;
00715         minB = B[i];
00716       }
00717     }
00718 
00719   }
00720 
00721   return n;
00722 }
00723 
00724 // template instantiation
00725 template GULAPI int RemoveCurveKnots(
00726         int                         n,
00727         int                         p,
00728         Ptr<float>&                 U,
00729         Ptr< point<float> >&        Pw,  
00730         int                         nQ,
00731         const Ptr<float>&           QU,
00732         float                       tol,
00733         Ptr<float>&                 QE );
00734 
00735 
00736 
00737 /*-------------------------------------------------------------------
00738   newton iteration
00739 --------------------------------------------------------------------*/
00740 
00741 template< class T, class EP >
00742 bool PTCCheck( const EP& P, const Ptr<EP>& D, T eps, T *err )
00743 {
00744   EP C,V,A;
00745   T num,den;
00746 
00747   C = D[0];
00748   V = D[1];  
00749   A = D[2];  
00750   
00751   // check point coincidence
00752   if( rel_equal(C,P,eps) )
00753     return true;
00754 
00755   // check zero cosine
00756   num = V*(C-P);
00757   den = rtr<T>::sqrt(V*V) * rtr<T>::sqrt((C-P)*(C-P));
00758   if( rel_equal(num+den,den,eps) )
00759     return true;
00760 
00761   *err = rtr<T>::fabs(num/den);
00762 
00763   return false;
00764 }
00765 
00766 template< class T, class HP, class EP >
00767 bool ProjectToCurve( const EP& P, T u, T eps, int n, int p, 
00768                      const Ptr<T>& U, const Ptr<HP>& Pw,
00769                      T *ret_u, Ptr<EP>& D )
00770 {
00771   bool found;
00772   EP C,V,A;
00773   T u1,err,err1,a,b;
00774   int t,s,sav,i,j;
00775 
00776   CurveDerivatives(u,n,p,U,Pw,2,D);
00777   found = PTCCheck( P, D, eps, &err );
00778   if( found ) 
00779   {
00780     *ret_u = u;
00781     return true;  
00782   }
00783 
00784   // initialize parameter range interval [a,b]
00785   // (C2 continuity required for changing knot spans)
00786 
00787   t = sav = FindSpan(u,n,p,U);
00788   for(;;)  // find left boundary
00789   {
00790     s = 1;
00791     while( (s <= p) && (U[t-s] == U[t]) ) s++;
00792     if( p-s < 2 )
00793     {
00794       a = U[t];
00795       break;
00796     }
00797     t = t-s;
00798   }
00799 
00800   t = sav + 1;
00801   for(;;)  // find right boundary
00802   {
00803     s = 1;
00804     while( (s <= p) && (U[t+s] == U[t]) ) s++;
00805     if( p-s < 2 )
00806     {
00807       b = U[t];
00808       break;
00809     }
00810     t = t+s;
00811   }
00812 
00813   // main loop
00814   for( j = 0; j < 100; j++ )  // max iterations = 100 ?
00815   {
00816     // calc next value for u
00817     C = D[0];
00818     V = D[1];  
00819     A = D[2];  
00820     u1 = u - (V*(C-P))/(A*(C-P) + V*V);
00821 
00822     // clip with search interval
00823     if( u1 < a ) u1 = a;
00824     if( u1 >= b ) u1 = b - b*rtr<T>::epsilon();
00825 
00826     // check that parameter changes enough
00827     if( rel_equal(u,u1,eps) )
00828     {
00829       *ret_u = u;
00830       return true;
00831     }
00832 
00833     // check step width (error should decrease when doing a newton step)
00834     for( i = 0; i < 10; i++ )
00835     {
00836       CurveDerivatives(u1,n,p,U,Pw,2,D);
00837       found = PTCCheck( P, D, eps, &err1 );
00838       if( found ) 
00839       {
00840         *ret_u = u1;
00841         return true;  
00842       }
00843 
00844       if( err1 < err )
00845         break;
00846 
00847       u1 = u + (T)0.5*(u1-u);
00848     }
00849     // step width adjustment not successful
00850     if( err1 > err )
00851       return false;
00852 
00853     u = u1;
00854     err = err1;
00855   }
00856 
00857   return false;
00858 }
00859 
00860 /*-----------------------------------------------------------------------
00861   increase multiplicity of all knots in a knot vector by one,
00862   the new knot vector must be reserved by the caller, it returns
00863   the index of the last knot in the new knot vector
00864 ------------------------------------------------------------------------*/
00865 template< class T >
00866 inline int IncMultiplicities( int n, int p, const Ptr<T>& U, Ptr<T>& Uh )
00867 {
00868   int i,j;
00869 
00870   Uh[0] = U[0];
00871   j = 1; 
00872 
00873   for( i = 1; i <= n+p+1; i++ )
00874   {
00875     if( U[i] != U[i-1] )
00876     {
00877       Uh[j] = U[i-1];
00878       j++;
00879     }
00880     Uh[j] = U[i];
00881     j++;
00882   }
00883 
00884   Uh[j] = U[n+p+1];
00885 
00886   return j;
00887 }
00888 
00889 /*-----------------------------------------------------------------------
00890   global curve approximation with error bound
00891 ------------------------------------------------------------------------*/
00892 template< class T, class EP >
00893 GULAPI bool GlobalCurveApproximationE( 
00894         int            nQ, 
00895         const Ptr<EP>& Q,
00896         T              tol,   // a bound for the absolute error of the curve
00897         T              eps,   // a bound for the relative error (needed internally
00898                               // for measuring point coincidence and zero cosine)
00899         int            degree,
00900         int           *ret_n,
00901         Ptr<T>        *ret_U,
00902         Ptr<EP>       *ret_Pw,
00903         Ptr<T>        *ret_QE,  // error at each data point
00904         Ptr<T>        *ret_QU ) // parameter value of each data point 
00905 {
00906   Ptr<T> d,QU,QE,U;
00907   Ptr<EP> Pw;
00908   T sum,u;
00909   int i,n,nn,p;
00910   int j,nh,ph,maxpps,deg;
00911   Ptr<T> Uh;
00912   Ptr<EP> Ph,Pwh;
00913   bool result;
00914   Ptr<EP> D;
00915 
00916   d.reserve_pool(nQ);
00917   QU.reserve_pool(nQ);
00918   QE.reserve_pool(nQ);
00919   Pw.reserve_pool(nQ);
00920   U.reserve_pool(nQ+2);
00921   D.reserve_pool(3);
00922 
00923   sum = ChordLength( nQ, Q, d );
00924   if( sum == (T)0 ) return false;
00925   ChordLengthParameters( nQ, sum, d, QU );
00926 
00927   // build a linear curve interpolating the data points
00928   U[0] = (T)0;
00929   U[1] = (T)0;
00930   for( i = 1; i < nQ-1; i++ )
00931     U[i+1] = QU[i];
00932   U[nQ] = (T)1;
00933   U[nQ+1] = (T)1;
00934 
00935   for( i = 0; i < nQ; i++ )
00936     Pw[i] = Q[i];
00937 
00938   // initialize errors to zero
00939   for( i = 0; i < nQ; i++ )
00940     QE[i] = (T)0;
00941 
00942   n = nQ-1;
00943   p = 1;
00944 
00945   for( deg = 1; deg <= degree; deg++ )
00946   { 
00947     nn = RemoveCurveKnots( n, p, U, Pw, nQ, QU, tol, QE );
00948 
00949     // build a knot vector with degree p + 1 (increases multiplicities by 1)
00950     if( p < degree )
00951     {
00952       n = nn;
00953 
00954       Uh.reserve_pool(2*(n+p+2));
00955       j = IncMultiplicities(n,p,U,Uh);
00956 
00957       ph = p+1;
00958       nh = j-ph-1;
00959       Pwh.reserve_pool(nh+1);  
00960     }
00961     // when the final degree has been reached, do a final fit with a curve of
00962     // the same degree, if the knot removal was successful
00963     else
00964     {
00965       if( nn == n )
00966         break; 
00967 
00968       n = nn;
00969 
00970       Uh = U;
00971       ph = p;
00972       nh = n;
00973       Pwh.reserve_pool(nh+1);  
00974     }
00975     maxpps = CalcMaxPointsPerSpan(nh,ph,Uh,nQ,QU);
00976     result = DoGlobalCurveApproximation(nQ,Q,QU,nh,ph,Uh,maxpps,Pwh);
00977     if( !result ) return false;
00978 
00979     for( i = 0; i < nQ; i++ )
00980     {
00981       result = ProjectToCurve<T,EP,EP>( Q[i], QU[i], eps, 
00982                                         nh, ph, Uh, Pwh, &u, D );
00983       if( !result ) return false;
00984 
00985       if( (i > 0) && (u <= QU[i-1]) ) // for the time being
00986         return false;
00987   
00988       QU[i] = u;   
00989       QE[i] = length(D[0]-Q[i]);
00990     }
00991 
00992     Pw = Pwh;
00993     U = Uh;
00994     n = nh;
00995     p = ph;
00996   }
00997  
00998   *ret_n = n;
00999   (*ret_U) = U;
01000   (*ret_Pw) = Pw;
01001   (*ret_QE) = QE;
01002   (*ret_QU) = QU;
01003 
01004   return true;
01005 }
01006 
01007 // template instantiation
01008 template GULAPI bool GlobalCurveApproximationE< float, point<float> >( 
01009         int                 nQ, 
01010         const               Ptr<point<float> >& Q,
01011         float               tol,   // a bound for the absolute error of the curve
01012         float               eps,   // a bound for the relative error (needed internally
01013                                    // for measuring point coincidence and zero cosine)
01014         int                 degree,
01015         int                *ret_n,
01016         Ptr<float>         *ret_U,
01017         Ptr<point<float> > *ret_Pw, 
01018         Ptr<float>         *ret_QE,   // error at each data point
01019         Ptr<float>         *ret_QU ); // parameter value of each data point
01020 
01021 }

Generated on Mon Jan 21 04:17:39 2002 for GUL 0.6 - Geometry Utility Library by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001