The Parametric Pseudo-Manifold (PPS) Library 1.0
bezier.cpp
Go to the documentation of this file.
00001 
00025 #include "bezier.h"      // Bezier
00026 
00027 #include "ludcmp.h"      // LUdcmp
00028 
00029 #include <cassert>       // assert
00030 #include <cmath>         // fabs
00031 
00032 
00047 namespace pps {
00048 
00049 
00078   Bezier::Bezier(
00079                  double* param_pts ,
00080                  double* image_pts ,
00081                  unsigned np ,
00082                  unsigned m ,
00083                  unsigned n ,
00084                  double rx ,
00085                  double ry ,
00086                  double sx ,
00087                  double sy
00088                 ) 
00089   {
00093     assert( m > 0 ) ;
00094     assert( n > 0 ) ;
00095     assert( rx < sx ) ;
00096     assert( ry < sy ) ;
00097     assert( ( 2 * ( n + 1 ) * ( m + 1 ) ) <= np ) ;
00098 
00103     set_bidegree_1st_index( m ) ;
00104     set_bidegree_2nd_index( n ) ;
00105 
00106     set_aff_1st_point_x_coord( rx ) ;
00107     set_aff_1st_point_y_coord( ry ) ;
00108 
00109     set_aff_2nd_point_x_coord( sx ) ;
00110     set_aff_2nd_point_y_coord( sy ) ;
00111 
00115     unsigned ncp = ( get_bidegree_1st_index() + 1 ) * 
00116                    ( get_bidegree_2nd_index() + 1 ) ;
00117 
00118     _ctrl_pts = new double*[ ncp ] ;
00119 
00120     for ( unsigned i = 0 ; i < ncp ; i++ ) {
00121       _ctrl_pts[ i ] = new double[ 3 ] ;
00122     }
00123 
00130     double** A = new double*[ np ] ;
00131 
00132     for ( unsigned i = 0 ; i < np ; i++ ) {
00133       A[ i ] = new double[ ncp ] ;
00134     }
00135 
00136     comp_bpoly_matrix( A , param_pts , np ) ;
00137 
00142     double** AtA = new double*[ ncp ] ;
00143 
00144     for ( unsigned i = 0 ; i < ncp ; i++ ) {
00145       AtA[ i ] = new double[ ncp ] ;
00146     }
00147 
00148     comp_matrix_ata( A , np , ncp , AtA ) ;
00149 
00154     double** AtB = new double*[ ncp ] ;
00155 
00156     for ( unsigned i = 0 ; i < ncp ; i++ ) {
00157       AtB[ i ] = new double[ 3 ] ;
00158     }
00159 
00160     comp_matrix_atb( A , image_pts , np , ncp , AtB ) ;
00161 
00171     LUdcmp LU( AtA , ncp );
00172 
00179     double* Y = new double[ ncp ] ;
00180 
00181     for ( unsigned i = 0 ; i < 3 ; i++ ) {
00182       for ( unsigned j = 0 ; j < ncp ; j++ ) {
00183         Y[ j ] = AtB[ j ][ i ] ;
00184       }
00185 
00189       LU.solve( Y , ncp ) ; 
00190 
00195       for ( unsigned j = 0 ; j < ncp ; j++ ) {
00196         _ctrl_pts[ j ][ i ] = Y[ j ] ;
00197       }
00198     }
00199 
00203     if (A != 0) {
00204       for ( unsigned i = 0 ; i < np ; i++ ) {
00205         if ( A[ i ] != 0 ) {
00206           delete[] A[ i ] ;
00207         }
00208       }
00209 
00210       delete A;
00211     }
00212 
00213     if ( AtA != 0 ) {
00214       for ( unsigned i = 0 ; i < ncp ; i++ ) {
00215         if ( AtA[ i ] != 0 ) {
00216           delete[] AtA[ i ] ;
00217         }
00218       }
00219 
00220       delete AtA ;
00221     }
00222 
00223     if ( AtB != 0 ) {
00224       for ( unsigned i = 0 ; i < ncp ; i++ ) {
00225         if ( AtB[ i ] != 0 ) {
00226           delete[] AtB[ i ] ;
00227         }
00228       }
00229 
00230       delete AtB ;
00231     }
00232 
00233     delete[] Y ;
00234 
00235     return ;
00236   }
00237 
00238 
00247   Bezier::Bezier( const Bezier& bz ) 
00248   {
00253     set_bidegree_1st_index( bz.get_bidegree_1st_index() ) ;
00254     set_bidegree_2nd_index( bz.get_bidegree_2nd_index() ) ;
00255 
00256     set_aff_1st_point_x_coord( bz.get_aff_1st_point_x_coord() ) ;
00257     set_aff_1st_point_y_coord( bz.get_aff_1st_point_y_coord() ) ;
00258 
00259     set_aff_2nd_point_x_coord( bz.get_aff_2nd_point_x_coord() ) ;
00260     set_aff_2nd_point_y_coord( bz.get_aff_2nd_point_y_coord() ) ;
00261 
00265     unsigned ncp = ( get_bidegree_1st_index() + 1 ) * 
00266                    ( get_bidegree_2nd_index() + 1 ) ;
00267 
00268     _ctrl_pts = new double*[ 
00269                             ( get_bidegree_1st_index() + 1 ) * 
00270                             ( get_bidegree_2nd_index() + 1 ) 
00271                            ] ;
00272 
00273     for ( unsigned i = 0 ; i < ncp ; i++ ) {
00274       _ctrl_pts[ i ] = new double[ 3 ] ;
00275 
00276       for ( unsigned j = 0 ; j < 3 ; j++ ) {
00277         _ctrl_pts[ i ][ j ] = bz._ctrl_pts[ i ][ j ] ;
00278       }
00279     }
00280 
00281     return ;
00282   }
00283 
00284 
00290   Bezier::~Bezier()
00291   {
00292     if ( _ctrl_pts != 0 ) {
00293 
00294       unsigned ncp = ( get_bidegree_1st_index() + 1 ) * 
00295                      ( get_bidegree_2nd_index() + 1 ) ;
00296 
00297       for ( unsigned i = 0 ; i < ncp ; i++ ) {
00298         if ( _ctrl_pts[ i ] != 0 ) {
00299           delete[] _ctrl_pts[ i ] ;
00300         }
00301       }
00302 
00303       delete[] _ctrl_pts ;
00304     }
00305 
00306     return ;
00307   }
00308 
00309 
00322   void
00323   Bezier::b(
00324             unsigned i ,
00325             unsigned j ,
00326             double& x ,
00327             double& y ,
00328             double& z
00329            ) 
00330     const
00331   {
00332     assert( 
00333            ( i <= get_bidegree_1st_index() ) && 
00334            ( j <= get_bidegree_2nd_index() ) 
00335           ) ;
00336 
00337     unsigned l = index( j , i ) ;
00338 
00339     x = _ctrl_pts[ l ][ 0 ] ;
00340     y = _ctrl_pts[ l ][ 1 ] ;
00341     z = _ctrl_pts[ l ][ 2 ] ;
00342 
00343     return ;
00344   }
00345 
00346 
00358   void
00359   Bezier::point(
00360                 double u , 
00361                 double v ,
00362                 double& x ,
00363                 double& y ,
00364                 double& z 
00365                ) 
00366     const
00367   {
00371     double rx = get_aff_1st_point_x_coord() ;
00372     double ry = get_aff_1st_point_y_coord() ;
00373     double sx = get_aff_2nd_point_x_coord() ;
00374     double sy = get_aff_2nd_point_y_coord() ;
00375     
00380     double uu = ( u - rx ) / ( sx - rx ) ;
00381     double vv = ( v - ry ) / ( sy - ry ) ;
00382 
00383     if ( fabs( uu ) <= 1e-15 ) {
00384       uu = 0 ;
00385     }
00386     else if ( fabs( 1 - uu ) <= 1e-15 ) {
00387       uu = 1 ;
00388     }
00389 
00390     if ( fabs( vv ) <= 1e-15 ) {
00391       vv = 0 ;
00392     }
00393     else if ( fabs( 1 - vv ) <= 1e-15 ) {
00394       vv = 1 ;
00395     }
00396 
00397     unsigned m = get_bidegree_1st_index() ;
00398     unsigned n = get_bidegree_2nd_index() ;
00399 
00400     std::vector< double > bu( m + 1 ) ;
00401     all_bernstein( m , uu , bu );
00402 
00406     std::vector< double > bv( n + 1 ) ;
00407     all_bernstein( n , vv , bv );
00408 
00412     x = 0 ;
00413     y = 0 ;
00414     z = 0 ;
00415     for ( unsigned j = 0 ; j <= n ; j++ ) {
00416       for ( unsigned i = 0 ; i <= m ; i++ ) {
00417         double buv = bu[ i ] * bv[ j ] ;
00418         double xaux ;
00419         double yaux ;
00420         double zaux ;
00421         b( i , j , xaux , yaux , zaux ) ;
00422 
00423         x += buv * xaux ;
00424         y += buv * yaux ;
00425         z += buv * zaux ;
00426       }
00427     }
00428 
00429     return ;
00430   }
00431 
00432 
00447   double
00448   Bezier::bernstein(
00449                     unsigned n ,
00450                     unsigned i ,
00451                     double u
00452                    )
00453     const 
00454   {
00455     assert( i <= n ) ;
00456 
00457     std::vector< double > temp( n + 1 ) ;
00458 
00459     for ( unsigned j = 0 ; j <= n ; j++ ) {
00460       temp[ j ] = 0 ;
00461     }
00462 
00463     temp[ n - i ] = 1 ;
00464 
00465     double u1 = 1 - u ;
00466 
00467     for ( unsigned k = 1 ; k <= n ; k++ ) {
00468       for ( unsigned j = n ; j >= k ; j-- ) {
00469         temp[ j ] = ( u1 * temp[ j ] ) + ( u * temp[ j - 1 ] ) ;
00470       }
00471     }
00472 
00473     return temp[ n ] ;
00474   }
00475 
00476 
00488   void
00489   Bezier::all_bernstein(
00490                         unsigned n ,
00491                         double u ,
00492                         std::vector< double >& b
00493                        )
00494     const
00495   {
00496     b[ 0 ] = 1 ;
00497     double u1 = 1 - u ;
00498 
00499     for ( unsigned j = 1 ; j <= n ; j++ ) {
00500       double saved = 0 ;
00501       for ( unsigned k = 0 ; k < j ;  k++ ) {
00502         double temp = b[ k ] ;
00503         b[ k ] = saved + ( u1 * temp ) ;
00504         saved = u * temp ;
00505       }
00506 
00507       b[ j ] = saved ;
00508     }
00509 
00510     return ;
00511   }
00512 
00528   void
00529   Bezier::comp_bpoly_matrix(
00530                             double**& a ,
00531                             double* param_pts , 
00532                             unsigned np 
00533                            ) 
00534       const
00535   {
00540     double rx = get_aff_1st_point_x_coord() ;
00541     double ry = get_aff_1st_point_y_coord() ;
00542     double sx = get_aff_2nd_point_x_coord() ;
00543     double sy = get_aff_2nd_point_y_coord() ;
00544 
00545     for ( unsigned i = 0 ; i < np ; i++ ) {
00546       double x = ( param_pts[ ( i << 1 )     ] - rx ) / 
00547         ( sx - rx ) ;
00548 
00549       double y = ( param_pts[ ( i << 1 ) + 1 ] - ry ) / 
00550         ( sy - ry ) ;
00551 
00552       unsigned m = get_bidegree_1st_index() ;
00553       unsigned n = get_bidegree_2nd_index() ;
00554 
00555       std::vector< double > bu( m + 1 ) ;
00556       std::vector< double > bv( n + 1 ) ;
00557 
00558       all_bernstein( m , x , bu ) ;
00559       all_bernstein( n , y , bv ) ;
00560 
00561       for ( unsigned k = 0 ; k <= n ; ++k ) {
00562         for ( unsigned j = 0 ; j <= m ; ++j ) {
00563           unsigned l = index( k , j ) ;
00564           a[ i ][ l ] = bu[ j ] * bv[ k ] ;
00565         }
00566       }
00567     }
00568 
00569     return ;
00570   }
00571 
00572 
00585   void
00586   Bezier::comp_matrix_ata(
00587                           double** a ,
00588                           unsigned n ,
00589                           unsigned p ,
00590                           double**& ata
00591                          )
00592     const
00593   { 
00594     for ( unsigned i = 0 ; i < p ; i++ ) {
00595       for ( unsigned k = 0 ; k < p ; k++ ) {
00596         ata[ i ][ k ] = 0 ;
00597         for ( unsigned j = 0 ; j < n ; j++ ) {
00598           ata[ i ][ k ] += a[ j ][ i ] * a[ j ][ k ] ;
00599         }
00600       }
00601     }
00602 
00603     return ;
00604   }
00605 
00606 
00621   void
00622   Bezier::comp_matrix_atb(
00623                           double** a ,
00624                           double* b ,
00625                           unsigned n ,
00626                           unsigned p ,
00627                           double**& atb
00628                          )
00629     const
00630   {
00635     for ( unsigned i = 0 ; i < p ; i++ ) {
00636       atb[ i ] = new double[ 3 ] ;
00637       for ( unsigned k = 0 ; k < 3 ; k++ ) {
00638         atb[ i ][ k ] = 0 ;
00639         for ( unsigned j = 0 ; j < n ; j++ ) {
00640           atb[ i ][ k ] += a[ j ][ i ] * b[ ( 3 * j ) + k ] ;
00641         }
00642       }
00643     }
00644 
00645     return ;
00646   }
00647 
00648 }
00649  //end of group class.