The Parametric Pseudo-Manifold (PPS) Library 1.0
|
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.