00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "stdafx.h"
00022
00023 #include <iostream>
00024
00025 #include "gul_types.h"
00026 #include "gul_vector.h"
00027 #include "gunu_basics.h"
00028 #include "gunu_interpolate.h"
00029
00030 namespace gunu {
00031
00032 using gul::rtr;
00033 using gul::Ptr;
00034 using gul::point;
00035 using gul::hpoint;
00036 using gul::Max;
00037 using gul::length;
00038 using gul::cross_product;
00039 using gul::set;
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 template< class T >
00050 GULAPI void CubicCurveInterpolation( int n, Ptr<point<T> > Q, Ptr<T> U,
00051 Ptr<point<T> > P )
00052 {
00053 Ptr<point<T> > R;
00054 Ptr<T> dd,abc;
00055 T deni;
00056 int i;
00057
00058 R.reserve_pool(n+1);
00059 dd.reserve_pool(n+1);
00060 abc.reserve_pool(4);
00061
00062 for( i = 3; i < n; i++ )
00063 R[i] = Q[i-1];
00064
00065 CalcBasisFunctions( U[4], 4, 3, U, abc );
00066
00067 deni = (T)1/abc[1];
00068 P[2] = deni*(Q[1]-abc[0]*P[1]);
00069
00070 for( i = 3; i < n; i++ )
00071 {
00072 dd[i] = deni*abc[2];
00073
00074 CalcBasisFunctions( U[i+2], i+2, 3, U, abc );
00075
00076 deni = (T)1/(abc[1]-abc[0]*dd[i]);
00077 P[i] = deni*(R[i]-abc[0]*P[i-1]);
00078 }
00079
00080 dd[n] = deni*abc[2];
00081
00082 CalcBasisFunctions( U[n+2], n+2, 3, U, abc );
00083
00084 deni = (T)1/(abc[1]-abc[0]*dd[n]);
00085 P[n] = deni*(Q[n-1]-abc[2]*P[n+1]-abc[0]*P[n-1]);
00086
00087 for( i = n-1; i >= 2; i-- )
00088 {
00089 P[i] = P[i]-dd[i+1]*P[i+1];
00090 }
00091 }
00092 template
00093 GULAPI void CubicCurveInterpolation( int n, Ptr<point<float> > Q, Ptr<float> U,
00094 Ptr<point<float> > P );
00095 template
00096 GULAPI void CubicCurveInterpolation( int n, Ptr<point<double> > Q, Ptr<double> U,
00097 Ptr<point<double> > P );
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108 template< class K >
00109 GULAPI void CubicLocalCurveInterpolation(
00110 int n, Ptr< point<K> > Q, bool cornerflag,
00111 Ptr<K> U, Ptr< point<K> > P )
00112 {
00113 Ptr< point<K> > q,T;
00114 point<K> v1;
00115 K a,b,c,alpha,l1,l2,u,ui;
00116 int upos, cpts_pos,k;
00117 bool no_alpha;
00118
00119 q.reserve_pool(n+4);
00120 T.reserve_pool(n+1);
00121
00122
00123
00124
00125
00126
00127
00128 for( k = 1; k <= n; k++ )
00129 q[k+1] = Q[k] - Q[k-1];
00130
00131 q[1] = (K)2 * q[2] - q[3];
00132 q[0] = (K)2 * q[1] - q[2];
00133
00134 q[n+2] = (K)2 * q[n+1] - q[n];
00135 q[n+3] = (K)2 * q[n+2] - q[n+1];
00136
00137 for( k = 0; k <= n; k++ )
00138 {
00139
00140
00141 l1 = length(cross_product(q[k], q[k+1]));
00142 l2 = length(cross_product(q[k+2], q[k+3]));
00143
00144 no_alpha = false;
00145 if( l1 + l2 != 0.0 )
00146 alpha = l1 / (l1 + l2);
00147 else
00148 if( !cornerflag )
00149 alpha = 0.5;
00150 else
00151 no_alpha = true;
00152
00153 if( no_alpha == false )
00154 {
00155
00156 T[k] = ((K)1-alpha)*q[k+1] + alpha*q[k+2];
00157 l1 = length(T[k]);
00158 if( l1 != 0.0 )
00159 T[k] = ((K)1/l1) * T[k];
00160 else
00161 set( T[k], (K)0 );
00162 }
00163 else
00164 set( T[k], (K)0 );
00165 }
00166
00167
00168
00169 P[0] = Q[0];
00170 cpts_pos = 1;
00171 U[0] = 0;
00172 U[1] = 0;
00173 U[2] = 0;
00174 U[3] = 0;
00175 upos = 4;
00176
00177 for( k = 0; k < n; k++ )
00178 {
00179 v1 = T[k] + T[k+1];
00180 a = ((K)16) - (v1 * v1);
00181
00182 l1 = q[k+2] * v1;
00183 b = ((K)12) * l1;
00184
00185 c = ((K)-36) * (q[k+2] * q[k+2]);
00186
00187 alpha =
00188 ( -(b/(((K)2)*a)) + rtr<K>::sqrt(((b*b)/(((K)4)*a*a)) - (c/a)) ) / ((K)3);
00189
00190
00191 P[cpts_pos++] = Q[k] + (alpha * T[k]);
00192 P[cpts_pos++] = Q[k+1] - (alpha * T[k+1]);
00193
00194
00195 v1 = P[cpts_pos-2] - Q[k];
00196 l1 = length(v1);
00197 u = U[upos-1] + ((K)3) * l1;
00198 U[upos++] = u;
00199 U[upos++] = u;
00200 }
00201
00202 P[cpts_pos] = Q[n];
00203 upos -= 2;
00204 ui = ((K)1) / U[upos];
00205 for( k = 4; k < upos; k++ )
00206 U[k] *= ui;
00207
00208 U[upos++] = (K)1;
00209 U[upos++] = (K)1;
00210 U[upos++] = (K)1;
00211 U[upos] = (K)1;
00212 }
00213 template
00214 GULAPI void CubicLocalCurveInterpolation(
00215 int n, Ptr< point<float> > Q, bool cornerflag,
00216 Ptr<float> U, Ptr< point<float> > P );
00217 template
00218 GULAPI void CubicLocalCurveInterpolation(
00219 int n, Ptr< point<double> > Q, bool cornerflag,
00220 Ptr<double> U, Ptr< point<double> > P );
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232 template< class T, class HP >
00233 GULAPI void CubicLocalSurfaceInterpolation(
00234 int n, int m, Ptr< Ptr< point<T> > > Q, bool cornerflag,
00235 Ptr<T> U, Ptr<T> V, Ptr< Ptr< HP > > controlPoints )
00236 {
00237 T gamma, alpha, a, l1, l2, total, d;
00238 int rpos, cpos, k, l, i;
00239 bool no_alpha;
00240 Ptr< Ptr< Ptr< Ptr< point<T> > > > > P;
00241 Ptr<T> r, s, alpha_k, beta_l, delta_uk, delta_vl, ub, vb;
00242 Ptr< Ptr< point<T> > > D_uv_, d_vu_, d_uv_, T_u_, T_v_;
00243 Ptr< point<T> > q, D_v_, d_v_, D_u_, d_u_;
00244 point<T> v1;
00245
00246
00247
00248 P.reserve_pool(m+1);
00249 T_u_.reserve_pool(m+1);
00250 T_v_.reserve_pool(m+1);
00251 d_vu_.reserve_pool(m+1);
00252 d_uv_.reserve_pool(m+1);
00253 D_uv_.reserve_pool(m+1);
00254
00255 for( l = 0; l <= m; l++ )
00256 {
00257 P[l].reserve_pool(n+1);
00258 T_u_[l].reserve_pool(n+1);
00259 T_v_[l].reserve_pool(n+1);
00260 d_vu_[l].reserve_pool(n+1);
00261 d_uv_[l].reserve_pool(n+1);
00262 D_uv_[l].reserve_pool(n+1);
00263
00264 for( k = 0; k <= n; k++ )
00265 {
00266 P[l][k].reserve_pool(3);
00267
00268 for( i = 0; i < 3; i++ )
00269 P[l][k][i].reserve_pool(3);
00270 }
00271 }
00272 ub.reserve_pool(n+1);
00273 vb.reserve_pool(m+1);
00274 k = Max(m,n);
00275 q.reserve_pool(k+4);
00276 r.reserve_pool(m+1);
00277 s.reserve_pool(n+1);
00278 delta_vl.reserve_pool(m+1);
00279 delta_uk.reserve_pool(n+1);
00280 beta_l.reserve_pool(m+1);
00281 alpha_k.reserve_pool(n+1);
00282 D_v_.reserve_pool(n+1);
00283 d_v_.reserve_pool(n+1);
00284 D_u_.reserve_pool(m+1);
00285 d_u_.reserve_pool(m+1);
00286
00287
00288
00289 total = 0.0;
00290
00291 for( k = 0; k <= n; k++ )
00292 ub[k] = 0.0;
00293
00294 for( l = 0; l <= m; l++ )
00295 {
00296 for( k = 1; k <= n; k++ )
00297 q[k+1] = Q[l][k] - Q[l][k-1];
00298
00299 q[1] = ((T)2) * q[2] - q[3];
00300 q[0] = ((T)2) * q[1] - q[2];
00301 q[n+2] = ((T)2) * q[n+1] - q[n];
00302 q[n+3] = ((T)2) * q[n+2] - q[n+1];
00303
00304 for( k = 0; k <= n; k++ )
00305 {
00306 l1 = length(cross_product(q[k], q[k+1]));
00307 l2 = length(cross_product(q[k+2], q[k+3]));
00308
00309 no_alpha = false;
00310 if( l1 + l2 != 0.0 )
00311 alpha = l1 / (l1 + l2);
00312 else
00313 if( !cornerflag )
00314 alpha = (T)0.5;
00315 else
00316 no_alpha = true;
00317
00318 if( no_alpha == false )
00319 {
00320 T_u_[l][k] = ((T)1-alpha)*q[k+1] + alpha*q[k+2];
00321
00322 l1 = length(T_u_[l][k]);
00323 if( l1 != 0.0 )
00324 T_u_[l][k] = ((T)1/l1) * T_u_[l][k];
00325 else
00326 set( T_u_[l][k], (T)0 );
00327 }
00328 else
00329 set( T_u_[l][k], (T)0 );
00330 }
00331
00332
00333 r[l] = (T)0;
00334
00335 for( k = 1; k <= n; k++ )
00336 {
00337 d = length(q[k+1]);
00338 ub[k] += d;
00339 r[l] += d;
00340 }
00341 total += r[l];
00342 }
00343 for( k = 1; k < n; k++ )
00344 ub[k] = ub[k-1] + (ub[k]/total);
00345
00346 ub[n] = 1.0;
00347
00348
00349
00350 total = 0.0;
00351
00352 for( l = 0; l <= m; l++ )
00353 vb[l] = 0.0;
00354
00355 for( k = 0; k <= n; k++ )
00356 {
00357 for( l = 1; l <= m; l++ )
00358 q[l+1] = Q[l][k] - Q[l-1][k];
00359
00360 q[1] = ((T)2) * q[2] - q[3];
00361 q[0] = ((T)2) * q[1] - q[2];
00362 q[m+2] = ((T)2) * q[m+1] - q[m];
00363 q[m+3] = ((T)2) * q[m+2] - q[m+1];
00364
00365 for( l = 0; l <= m; l++ )
00366 {
00367 l1 = length(cross_product(q[l], q[l+1]));
00368 l2 = length(cross_product(q[l+2], q[l+3]));
00369
00370 no_alpha = false;
00371 if( l1 + l2 != (T)0 )
00372 alpha = l1 / (l1 + l2);
00373 else
00374 if( !cornerflag )
00375 alpha = (T)0.5;
00376 else
00377 no_alpha = true;
00378
00379 if( no_alpha == false )
00380 {
00381 T_v_[l][k] = ((T)1-alpha) * q[l+1] + alpha*q[l+2];
00382 l1 = length( T_v_[l][k] );
00383 if( l1 != (T)0 )
00384 T_v_[l][k] = ((T)1/l1) * T_v_[l][k];
00385 else
00386 set( T_v_[l][k], (T)0 );
00387 }
00388 else
00389 set( T_v_[l][k], (T)0 );
00390 }
00391
00392
00393 s[k] = 0.0;
00394
00395 for( l = 1; l <= m; l++ )
00396 {
00397 d = length( q[l+1] );
00398 vb[l] += d;
00399 s[k] += d;
00400 }
00401 total += s[k];
00402 }
00403 for( l = 1; l < m; l++ )
00404 vb[l] = vb[l-1] + (vb[l]/total);
00405
00406 vb[m] = (T)1;
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425 for( l = 0; l <= m; l++ )
00426 {
00427 for( k = 0; k < n; k++ )
00428 {
00429 P[l][k][0][0] = Q[l][k];
00430 a = (r[l] * (ub[k+1] - ub[k])) / (T)3;
00431 P[l][k][0][1] = Q[l][k] + a * T_u_[l][k];
00432 P[l][k][0][2] = Q[l][k+1] - a * T_u_[l][k+1];
00433 }
00434 P[l][n][0][0] = Q[l][n];
00435 }
00436
00437 for( k = 0; k <= n; k++ )
00438 {
00439 for( l = 0; l < m; l++ )
00440 {
00441 a = (s[k] * (vb[l+1] - vb[l])) / (T)3;
00442 P[l][k][1][0] = Q[l][k] + a * T_v_[l][k];
00443 P[l][k][2][0] = Q[l+1][k] - a * T_v_[l+1][k];
00444 }
00445 }
00446
00447
00448
00449
00450
00451 for( k = 1; k <= n; k++ )
00452 delta_uk[k] = ub[k] - ub[k-1];
00453 for( k = 1; k < n; k++ )
00454 alpha_k[k] = delta_uk[k] / (delta_uk[k] + delta_uk[k+1]);
00455 alpha_k[0] = 0.5;
00456 alpha_k[n] = 0.5;
00457
00458 for( l = 1; l <= m; l++ )
00459 delta_vl[l] = vb[l] - vb[l-1];
00460 for( l = 1; l < m; l++ )
00461 beta_l[l] = delta_vl[l] / (delta_vl[l] + delta_vl[l+1]);
00462 beta_l[0] = 0.5;
00463 beta_l[m] = 0.5;
00464
00465
00466
00467 for( l = 0; l <= m; l++ )
00468 {
00469 for( k = 0; k <= n; k++ )
00470 D_v_[k] = s[k] * T_v_[l][k];
00471
00472 for( k = 1; k <= n; k++ )
00473 {
00474
00475 d_v_[k] = D_v_[k] - D_v_[k-1];
00476
00477 if( delta_uk[k] != (T)0 )
00478 d_v_[k] = ((T)1 / delta_uk[k]) * d_v_[k];
00479 else
00480 set( d_v_[k], (T)0 );
00481 }
00482 for( k = 1; k < n; k++ )
00483 {
00484 d_vu_[l][k] = ((T)1 - alpha_k[k]) * d_v_[k] + alpha * d_v_[k+1];
00485 }
00486
00487
00488 d_vu_[l][0] = (T)2 * d_v_[1] - d_vu_[l][1];
00489 d_vu_[l][n] = (T)2 * d_v_[n] - d_vu_[l][n-1];
00490 }
00491
00492
00493 for( k = 0; k <= n; k++ )
00494 {
00495 for( l = 0; l <= m; l++ )
00496 D_u_[l] = r[l] * T_u_[l][k];
00497
00498 for( l = 1; l <= m; l++ )
00499 {
00500
00501 d_u_[l] = D_u_[l] - D_u_[l-1];
00502 if( delta_vl[l] != (T)0 )
00503 d_u_[l] = ((T)1 / delta_vl[l]) * d_u_[l];
00504 else
00505 set( d_u_[l], (T)0 );
00506 }
00507 for( l = 1; l < m; l++ )
00508 {
00509 d_uv_[l][k] = ((T)1 - beta_l[l]) * d_u_[l] + beta_l[l] * d_u_[l+1];
00510 }
00511
00512
00513 d_uv_[0][k] = (T)2 * d_u_[1] - d_uv_[1][k];
00514 d_uv_[m][k] = (T)2 * d_u_[m] - d_uv_[m-1][k];
00515 }
00516
00517
00518 for( l = 0; l <= m; l++ )
00519 for( k = 0; k <= n; k++ )
00520 {
00521 v1 = alpha_k[k] * d_uv_[l][k] + beta_l[l] * d_vu_[l][k];
00522 if( alpha_k[k] + beta_l[l] != (T)0 )
00523 D_uv_[l][k] = ((T)1/(alpha_k[k] + beta_l[l])) * v1;
00524 else
00525 set( D_uv_[l][k], (T)0 );
00526 }
00527
00528
00529
00530
00531
00532 for( l = 0; l < m; l++ )
00533 for( k = 0; k < n; k++ )
00534 {
00535 gamma = (delta_uk[k+1] * delta_vl[l+1]) / (T)9;
00536
00537 P[l][k][1][1] = gamma * D_uv_[l][k] +
00538 P[l][k][1][0] + P[l][k][0][1] - P[l][k][0][0];
00539 P[l][k][1][2] = -gamma * D_uv_[l][k+1] +
00540 P[l][k+1][1][0] - P[l][k+1][0][0] + P[l][k][0][2];
00541 P[l][k][2][1] = -gamma * D_uv_[l+1][k] +
00542 P[l+1][k][0][1] - P[l+1][k][0][0] + P[l][k][2][0];
00543 P[l][k][2][2] = gamma * D_uv_[l+1][k+1] +
00544 P[l+1][k][0][2] + P[l][k+1][2][0] - P[l+1][k+1][0][0];
00545 }
00546
00547
00548 U[0] = 0.0;
00549 U[1] = 0.0;
00550 U[2] = 0.0;
00551 U[3] = 0.0;
00552 cpos = 4;
00553 for( k = 1; k < n; k++ )
00554 {
00555 U[cpos++] = ub[k];
00556 U[cpos++] = ub[k];
00557 }
00558 U[cpos++] = 1.0;
00559 U[cpos++] = 1.0;
00560 U[cpos++] = 1.0;
00561 U[cpos] = 1.0;
00562
00563 V[0] = 0.0;
00564 V[1] = 0.0;
00565 V[2] = 0.0;
00566 V[3] = 0.0;
00567 cpos = 4;
00568 for( l = 1; l < m; l++ )
00569 {
00570 V[cpos++] = vb[l];
00571 V[cpos++] = vb[l];
00572 }
00573 V[cpos++] = 1.0;
00574 V[cpos++] = 1.0;
00575 V[cpos++] = 1.0;
00576 V[cpos] = 1.0;
00577
00578
00579
00580 rpos = 0;
00581 cpos = 0;
00582
00583
00584
00585 controlPoints[rpos][cpos++] = P[0][0][0][0];
00586 for( k = 0; k < n; k++ )
00587 {
00588 controlPoints[rpos][cpos++] = P[0][k][0][1];
00589 controlPoints[rpos][cpos++] = P[0][k][0][2];
00590 }
00591 controlPoints[rpos][cpos++] = P[0][n][0][0];
00592
00593
00594
00595 for( l = 0; l < m; l++ )
00596 {
00597 rpos++;
00598 cpos = 0;
00599 controlPoints[rpos][cpos++] = P[l][0][1][0];
00600 for( k = 0; k < n; k++ )
00601 {
00602 controlPoints[rpos][cpos++] = P[l][k][1][1];
00603 controlPoints[rpos][cpos++] = P[l][k][1][2];
00604 }
00605 controlPoints[rpos][cpos++] = P[l][n][1][0];
00606
00607 rpos++;
00608 cpos = 0;
00609 controlPoints[rpos][cpos++] = P[l][0][2][0];
00610 for( k = 0; k < n; k++ )
00611 {
00612 controlPoints[rpos][cpos++] = P[l][k][2][1];
00613 controlPoints[rpos][cpos++] = P[l][k][2][2];
00614 }
00615 controlPoints[rpos][cpos++] = P[l][n][2][0];
00616 }
00617
00618
00619 rpos++;
00620 cpos = 0;
00621 controlPoints[rpos][cpos++] = P[m][0][0][0];
00622 for( k = 0; k < n; k++ )
00623 {
00624 controlPoints[rpos][cpos++] = P[m][k][0][1];
00625 controlPoints[rpos][cpos++] = P[m][k][0][2];
00626 }
00627 controlPoints[rpos][cpos++] = P[m][n][0][0];
00628
00629
00630 }
00631 template GULAPI void CubicLocalSurfaceInterpolation(
00632 int n, int m, Ptr< Ptr< point<float> > > Q, bool cornerflag,
00633 Ptr<float> U, Ptr<float> V, Ptr< Ptr< point<float> > > controlPoints );
00634 template GULAPI void CubicLocalSurfaceInterpolation(
00635 int n, int m, Ptr< Ptr< point<double> > > Q, bool cornerflag,
00636 Ptr<double> U, Ptr<double> V, Ptr< Ptr< point<double> > > controlPoints );
00637 template GULAPI void CubicLocalSurfaceInterpolation(
00638 int n, int m, Ptr< Ptr< point<float> > > Q, bool cornerflag,
00639 Ptr<float> U, Ptr<float> V, Ptr< Ptr< hpoint<float> > > controlPoints );
00640 template GULAPI void CubicLocalSurfaceInterpolation(
00641 int n, int m, Ptr< Ptr< point<double> > > Q, bool cornerflag,
00642 Ptr<double> U, Ptr<double> V, Ptr< Ptr< hpoint<double> > > controlPoints );
00643 }
00644
00645