00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "stdafx.h"
00026
00027 #include <iostream>
00028
00029 #include "gul_types.h"
00030 #include "gul_float.h"
00031 #include "gul_vector.h"
00032 #include "guma_lineq.h"
00033
00034 namespace guma {
00035
00036 using gul::Ptr;
00037 using gul::rtr;
00038 using gul::SingularMatrixError;
00039 using gul::InternalError;
00040 using gul::Swap;
00041 using gul::Square;
00042 using gul::Sign;
00043 using gul::Max;
00044 using gul::Min;
00045 using gul::point;
00046 using gul::rel_equal;
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 template< class T >
00059 GULAPI bool GaussJordan( int nRowsCols, int nRightSides,
00060 Ptr< Ptr<T> >& A, Ptr< Ptr<T> > &b )
00061 {
00062 Ptr<int> indexc,indexr;
00063 Ptr<T> ipivot;
00064 int icol,irow, n,i,j,k,l,ll,m;
00065 T big,pivotinv,dummy;
00066
00067 n = nRowsCols - 1;
00068 m = nRightSides - 1;
00069
00070 indexc.reserve_pool(n+1);
00071 indexr.reserve_pool(n+1);
00072 ipivot.reserve_pool(n+1);
00073
00074 for( j = 0; j <= n; j++ )
00075 ipivot[j] = 0;
00076
00077 for( i = 0; i <= n; i++ )
00078 {
00079 big = 0.0;
00080
00081 for( j = 0; j <= n; j++ )
00082 {
00083 if( ipivot[j] != 1 )
00084 {
00085 for( k = 0; k <= n; k++ )
00086 {
00087 if( ipivot[k] == 0 )
00088 {
00089 if( rtr<T>::fabs( A[j][k] ) >= big )
00090 {
00091 big = rtr<T>::fabs( A[j][k] );
00092 irow = j;
00093 icol = k;
00094 }
00095 }
00096 else if( ipivot[k] > 1 )
00097 return false;
00098 }
00099 }
00100 }
00101 ipivot[icol]++;
00102
00103 if( irow != icol )
00104 {
00105 for( l = 0; l <= n; l++ )
00106 Swap( A[irow][l], A[icol][l] );
00107 for( l = 0; l <= m; l++ )
00108 Swap( b[irow][l], b[icol][l] );
00109 }
00110 indexr[i] = irow;
00111 indexc[i] = icol;
00112
00113 if( A[icol][icol] == 0.0 )
00114 return false;
00115
00116 pivotinv = (T)1 / A[icol][icol];
00117 A[icol][icol] = 1.0;
00118 for( l = 0; l <= n; l++ )
00119 A[icol][l] *= pivotinv;
00120 for( l = 0; l <= m; l++ )
00121 b[icol][l] *= pivotinv;
00122
00123 for( ll = 0; ll <= n; ll++ )
00124 {
00125 if( ll != icol )
00126 {
00127 dummy = A[ll][icol];
00128 A[ll][icol] = 0.0;
00129 for( l = 0; l <= n; l++ )
00130 A[ll][l] -= A[icol][l] * dummy;
00131 for( l = 0; l <= m; l++ )
00132 b[ll][l] -= b[icol][l] * dummy;
00133 }
00134 }
00135 }
00136 for( l = n; l >= 0; l-- )
00137 {
00138 if( indexr[l] != indexc[l] )
00139 {
00140 for( k = 0; k <= n; k++ )
00141 Swap( A[k][indexr[l]], A[k][indexc[l]] );
00142 }
00143 }
00144 return true;
00145 }
00146 template GULAPI bool GaussJordan( int nRowsCols, int m,
00147 Ptr< Ptr<float> >& A, Ptr< Ptr<float> > &b );
00148 template GULAPI bool GaussJordan( int nRowsCols, int m,
00149 Ptr< Ptr<double> >& A, Ptr< Ptr<double> > &b );
00150
00151
00152
00153
00154 template< class T >
00155 GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<T> >& A,
00156 int *perm_sign, Ptr<int>& index )
00157 {
00158 Ptr<T> vv;
00159 T big,sum,dummy,temp;
00160 int i,j,k,imax,n,d;
00161
00162 n = nRowsCols - 1;
00163 vv.reserve_pool(n+1);
00164 d = 1;
00165
00166 for( i = 0; i <= n; i++ )
00167 {
00168 big = 0.0;
00169 for( j = 0; j <= n; j++ )
00170 {
00171 if( (temp = rtr<T>::fabs(A[i][j])) > big )
00172 big = temp;
00173 }
00174 if( big == 0.0 )
00175 return false;
00176
00177 vv[i] = (T)1 / big;
00178 }
00179 for( j = 0; j <= n; j++ )
00180 {
00181 for( i = 0; i < j; i++ )
00182 {
00183 sum = A[i][j];
00184 for( k = 0; k < i; k++ )
00185 sum -= A[i][k] * A[k][j];
00186 A[i][j] = sum;
00187 }
00188 big = 0.0;
00189
00190 for( i = j; i <= n; i++ )
00191 {
00192 sum = A[i][j];
00193 for( k = 0; k < j; k++ )
00194 sum -= A[i][k] * A[k][j];
00195 A[i][j] = sum;
00196
00197 if( (dummy = vv[i] * rtr<T>::fabs(sum)) >= big )
00198 {
00199 big = dummy;
00200 imax = i;
00201 }
00202 }
00203 if( j != imax )
00204 {
00205 for( k = 0; k <= n; k++ )
00206 Swap( A[imax][k], A[j][k] );
00207
00208 d = -d;
00209 vv[imax] = vv[j];
00210 }
00211 index[j] = imax;
00212
00213 if( A[j][j] == 0.0 )
00214 A[j][j] = rtr<T>::tiny();
00215
00216 if( j != n )
00217 {
00218 dummy = (T)1 / A[j][j];
00219 for( i = j+1; i <= n; i++ )
00220 A[i][j] *= dummy;
00221 }
00222 }
00223
00224 *perm_sign = d;
00225 return true;
00226 }
00227 template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<float> >& A,
00228 int *perm_sign, Ptr<int>& index );
00229 template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<double> >& A,
00230 int *perm_sign, Ptr<int>& index );
00231
00232
00233
00234
00235
00236 template< class T >
00237 GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index,
00238 Ptr< Ptr<T> >& A, Ptr<T>& b )
00239 {
00240 int ii,i,j,ip,n;
00241 T sum;
00242
00243 n = nRowsCols-1;
00244
00245 ii = -1;
00246
00247 for( i = 0; i <= n; i++ )
00248 {
00249 ip = index[i];
00250 sum = b[ip];
00251 b[ip] = b[i];
00252
00253 if( ii >= 0 )
00254 {
00255 for( j = ii; j <= i-1; j++ )
00256 sum -= A[i][j] * b[j];
00257 }
00258 else
00259 if( sum != 0.0 )
00260 ii = i;
00261
00262 b[i] = sum;
00263 }
00264 for( i = n; i >= 0; i-- )
00265 {
00266 sum = b[i];
00267
00268 for( j = i+1; j <= n; j++ )
00269 sum -= A[i][j] * b[j];
00270
00271 b[i] = sum / A[i][i];
00272 }
00273 }
00274 template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index,
00275 Ptr< Ptr<float> >& A, Ptr<float>& b );
00276 template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index,
00277 Ptr< Ptr<double> >& A, Ptr<double>& b );
00278
00279
00280
00281
00282
00283
00284
00285 template< class T >
00286 GULAPI void BandMVProduct( int n, int m1, int m2,
00287 Ptr< Ptr<T> > a, Ptr<T> x, Ptr<T> b )
00288 {
00289 int i,k,j,m;
00290
00291 for( i = 0; i < n; i++ )
00292 {
00293 k = i - m1;
00294
00295 m = Min(m1+m2+1,n-k);
00296
00297 b[i] = 0;
00298
00299 for( j = Max(0,-k); j < m; j++ )
00300 b[i] += a[i][j] * x[j+k];
00301 }
00302 }
00303
00304 template GULAPI void BandMVProduct(
00305 int n, int m1, int m2, Ptr< Ptr<float> > a,
00306 Ptr<float> x, Ptr<float> b );
00307 template GULAPI void BandMVProduct(
00308 int n, int m1, int m2, Ptr< Ptr<double> > a,
00309 Ptr<double> x, Ptr<double> b );
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319 template< class T >
00320 GULAPI void BandDecomposition(
00321 int n, int m1, int m2, Ptr< Ptr<T> >& A,
00322 Ptr< Ptr<T> >& L, int *perm_sign, Ptr<int>& index )
00323 {
00324 int i,j,k,l,mm,d;
00325 T dum;
00326
00327 mm = m1 + m2 + 1;
00328 l = m1;
00329 for( i = 0; i < m1; i++ )
00330 {
00331 for( j = m1-i; j < mm; j++ )
00332 A[i][j-l] = A[i][j];
00333 l--;
00334 for( j = mm-l-1; j < mm; j++ )
00335 A[i][j] = 0.0;
00336 }
00337 d = 1;
00338 l = m1;
00339
00340 for( k = 0; k < n; k++ )
00341 {
00342 dum = A[k][0];
00343 i = k;
00344
00345 if( l < n ) l++;
00346 for( j = k+1; j < l; j++ )
00347 {
00348 if( rtr<T>::fabs(A[j][0]) > rtr<T>::fabs(dum) )
00349 {
00350 dum = A[j][0];
00351 i = j;
00352 }
00353 }
00354 index[k] = i;
00355
00356 if( dum == 0.0 )
00357 A[k][0] = rtr<T>::tiny();
00358
00359 if( i != k )
00360 {
00361 d = -d;
00362 for( j = 0; j < mm; j++ )
00363 Swap( A[k][j], A[i][j] );
00364 }
00365
00366 for( i = k+1; i < l; i++ )
00367 {
00368 dum = A[i][0]/A[k][0];
00369 L[k][i-k-1] = dum;
00370
00371 for( j = 1; j < mm; j++ )
00372 A[i][j-1] = A[i][j] - dum*A[k][j];
00373
00374 A[i][mm-1] = 0.0;
00375 }
00376 }
00377 *perm_sign = d;
00378 }
00379
00380 template GULAPI void BandDecomposition(
00381 int n, int m1, int m2, Ptr< Ptr<float> >& A, Ptr< Ptr<float> >& L,
00382 int *perm_sign, Ptr<int>& index );
00383 template GULAPI void BandDecomposition(
00384 int n, int m1, int m2, Ptr< Ptr<double> >& A, Ptr< Ptr<double> >& L,
00385 int *perm_sign, Ptr<int>& index );
00386
00387
00388
00389
00390
00391
00392 template< class T >
00393 GULAPI void BandBackSubstitution(
00394 int n, int m1, int m2, const Ptr< Ptr<T> >& A,
00395 const Ptr< Ptr<T> >& L, const Ptr<int>& index, Ptr<T>& b )
00396 {
00397 int i,k,l,mm;
00398 T dum;
00399
00400 mm = m1 + m2 + 1;
00401 l = m1;
00402
00403 for( k = 0; k < n; k++ )
00404 {
00405 i = index[k];
00406 if( i != k ) Swap( b[k], b[i] );
00407 if( l < n ) l++;
00408 for( i = k+1; i < l; i++ )
00409 b[i] -= L[k][i-k-1]*b[k];
00410 }
00411 l = 1;
00412 for( i = n-1; i >= 0; i-- )
00413 {
00414 dum = b[i];
00415 for( k = 1; k < l; k++ )
00416 dum -= A[i][k]*b[k+i];
00417 b[i] = dum/A[i][0];
00418 if( l < mm ) l++;
00419 }
00420 }
00421
00422 template GULAPI void BandBackSubstitution(
00423 int n, int m1, int m2, const Ptr< Ptr<float> >& A,
00424 const Ptr< Ptr<float> >& L, const Ptr<int>& index, Ptr<float>& b );
00425 template GULAPI void BandBackSubstitution(
00426 int n, int m1, int m2, const Ptr< Ptr<double> >& A,
00427 const Ptr< Ptr<double> >& L, const Ptr<int>& index, Ptr<double>& b );
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444 template< class T >
00445 GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<T> >& a,
00446 Ptr<T>& w, Ptr< Ptr<T> >& v )
00447 {
00448 int flag,i,its,j,jj,k,l,nm;
00449 T anorm,c,f,g,h,s,scale,x,y,z;
00450 Ptr<T> rv1;
00451
00452 rv1.reserve_pool(n+1);
00453
00454
00455
00456 g = scale = anorm = 0.0;
00457
00458 for( i = 1; i <= n; i++ )
00459 {
00460 l = i+1;
00461 rv1[i-1] = scale * g;
00462 g = s = scale = 0.0;
00463
00464 if( i <= m )
00465 {
00466 for( k = i; k <=m; k++ )
00467 scale += rtr<T>::fabs( a[k-1][i-1] );
00468 if( scale != 0.0 )
00469 {
00470 for( k = i; k <= m; k++ )
00471 {
00472 a[k-1][i-1] /= scale;
00473 s += a[k-1][i-1] * a[k-1][i-1];
00474 }
00475 f = a[i-1][i-1];
00476 g = -Sign( rtr<T>::sqrt(s), f );
00477 h = f*g - s;
00478 a[i-1][i-1] = f-g;
00479
00480 for( j=l; j<=n; j++ )
00481 {
00482 for( s=0.0,k=i; k<=m; k++ )
00483 s += a[k-1][i-1] * a[k-1][j-1];
00484 f = s / h;
00485 for( k = i; k <= m; k++ )
00486 a[k-1][j-1] += f * a[k-1][i-1];
00487 }
00488 for( k = i; k <=m; k++ )
00489 a[k-1][i-1] *= scale;
00490 }
00491 }
00492 w[i-1] = scale * g;
00493 g = s = scale = 0.0;
00494
00495 if( (i <= m) && (i != n) )
00496 {
00497 for( k = l; k <= n; k++ )
00498 scale += rtr<T>::fabs( a[i-1][k-1] );
00499
00500 if( scale != 0.0 )
00501 {
00502 for( k = l; k <= n; k++ )
00503 {
00504 a[i-1][k-1] /= scale;
00505 s += a[i-1][k-1] * a[i-1][k-1];
00506 }
00507
00508 f = a[i-1][l-1];
00509 g = -Sign( rtr<T>::sqrt(s), f );
00510 h = f*g - s;
00511 a[i-1][l-1] = f-g;
00512
00513 for( k = l; k <= n; k++ )
00514 rv1[k-1] = a[i-1][k-1] / h;
00515
00516 for( j = l; j <= m; j++ )
00517 {
00518 for( s = 0.0,k=l; k <= n; k++ )
00519 s += a[j-1][k-1] * a[i-1][k-1];
00520 for( k = l; k <= n; k++ )
00521 a[j-1][k-1] += s * rv1[k-1];
00522 }
00523
00524 for( k = l; k <= n; k++ )
00525 a[i-1][k-1] *= scale;
00526 }
00527 }
00528 anorm = Max( anorm, (rtr<T>::fabs(w[i-1]) + rtr<T>::fabs(rv1[i-1])) );
00529 }
00530
00531
00532 for( i = n; i >= 1; i-- )
00533 {
00534 if( i < n )
00535 {
00536 if( g != 0.0 )
00537 {
00538 for( j = l; j <= n; j++ )
00539 v[j-1][i-1] = (a[i-1][j-1] / a[i-1][l-1]) / g;
00540 for( j = l; j <= n; j++ )
00541 {
00542 for( s=0.0,k=l; k<=n; k++ )
00543 s += a[i-1][k-1] * v[k-1][j-1];
00544 for( k = l; k <= n; k++ )
00545 v[k-1][j-1] += s * v[k-1][i-1];
00546 }
00547 }
00548 for( j = l; j <= n; j++ )
00549 v[i-1][j-1] = v[j-1][i-1] = 0.0;
00550 }
00551 v[i-1][i-1] = 1.0;
00552 g = rv1[i-1];
00553 l = i;
00554 }
00555
00556
00557 for( i = Min(m,n); i >=1; i-- )
00558 {
00559 l = i+1;
00560 g = w[i-1];
00561
00562 for( j = l; j <= n; j++ )
00563 a[i-1][j-1] = 0.0;
00564
00565 if( g != 0.0 )
00566 {
00567 g = (T)1 / g;
00568
00569 for( j = l; j <= n; j++ )
00570 {
00571 for( s=0.0,k=l; k <= m; k++ )
00572 s += a[k-1][i-1] * a[k-1][j-1];
00573
00574 f = (s/a[i-1][i-1]) * g;
00575
00576 for( k = i; k <= m; k++ )
00577 a[k-1][j-1] += f * a[k-1][i-1];
00578 }
00579
00580 for( j = i; j <= m; j++ )
00581 a[j-1][i-1] *= g;
00582 }
00583 else
00584 for( j = i; j <= m; j++ )
00585 a[j-1][i-1] = 0.0;
00586
00587 a[i-1][i-1] += 1.0;
00588 }
00589
00590
00591 for( k = n; k >= 1; k-- )
00592 {
00593 for( its = 1; its <= 30; its++ )
00594 {
00595 flag = 1;
00596
00597 for( l = k; l >= 1; l-- )
00598 {
00599 nm = l-1;
00600 if( rtr<T>::fabs(rv1[l-1]) + anorm == anorm )
00601 {
00602 flag = 0;
00603 break;
00604 }
00605 if( rtr<T>::fabs(w[nm-1]) + anorm == anorm )
00606 break;
00607 }
00608 if( flag != 0 )
00609 {
00610 c = 0.0;
00611 s = 1.0;
00612 for( i = l; i <= k; i++ )
00613 {
00614 f = s * rv1[i-1];
00615 rv1[i-1] = c * rv1[i-1];
00616
00617 if( rtr<T>::fabs(f) + anorm == anorm )
00618 break;
00619
00620 g = w[i-1];
00621 h = Pythagoras( f, g );
00622 w[i-1] = h;
00623 h = (T)1 / h;
00624 c = g * h;
00625 s = -f * h;
00626
00627 for( j = 1; j <= m; j++ )
00628 {
00629 y = a[j-1][nm-1];
00630 z = a[j-1][i-1];
00631 a[j-1][nm-1] = y * c + z * s;
00632 a[j-1][i-1] = z * c - y * s;
00633 }
00634 }
00635 }
00636 z = w[k-1];
00637 if( l == k )
00638 {
00639 if( z < 0.0 )
00640 {
00641 w[k-1] = -z;
00642 for( j = 1; j <= n; j++ )
00643 v[j-1][k-1] = -v[j-1][k-1];
00644 }
00645 break;
00646 }
00647 if( its == 30 )
00648 throw InternalError();
00649
00650 x = w[l-1];
00651 nm = k-1;
00652 y = w[nm-1];
00653 g = rv1[nm-1];
00654 h = rv1[k-1];
00655 f = ((y-z)*(y+z)+(g-h)*(g+h))/((T)2*h*y);
00656 g = Pythagoras( f, (T)1 );
00657 f = ((x-z)*(x+z) + h*((y/(f+Sign(g,f)))-h)) / x;
00658 c = s = 1.0;
00659
00660 for( j = l; j <= nm; j++ )
00661 {
00662 i = j+1;
00663 g = rv1[i-1];
00664 y = w[i-1];
00665 h = s * g;
00666 g = c * g;
00667 z = Pythagoras( f, h );
00668 rv1[j-1] = z;
00669 c = f/z;
00670 s = h/z;
00671 f = x*c + g*s;
00672 g = g*c - x*s;
00673 h = y*s;
00674 y *= c;
00675
00676 for( jj = 1; jj <= n; jj++ )
00677 {
00678 x = v[jj-1][j-1];
00679 z = v[jj-1][i-1];
00680 v[jj-1][j-1] = x*c + z*s;
00681 v[jj-1][i-1] = z*c - x*s;
00682 }
00683 z = Pythagoras( f, h );
00684 w[j-1] = z;
00685 if( z != 0.0 )
00686 {
00687 z = (T)1 / z;
00688 c = f*z;
00689 s = h*z;
00690 }
00691 f = c*g + s*y;
00692 x = c*y - s*g;
00693
00694 for( jj = 1; jj <= m; jj++ )
00695 {
00696 y = a[jj-1][j-1];
00697 z = a[jj-1][i-1];
00698 a[jj-1][j-1] = y*c + z*s;
00699 a[jj-1][i-1] = z*c - y*s;
00700 }
00701 }
00702 rv1[l-1] = 0.0;
00703 rv1[k-1] = f;
00704 w[k-1] = x;
00705 }
00706 }
00707 }
00708 template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<float> >& a,
00709 Ptr<float>& w, Ptr< Ptr<float> >& v );
00710 template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<double> >& a,
00711 Ptr<double>& w, Ptr< Ptr<double> >& v );
00712
00713
00714
00715
00716
00717 template< class T >
00718 GULAPI void SVBackSubstitution( int m, int n,
00719 Ptr< Ptr<T> >& u, Ptr<T>& w,
00720 Ptr< Ptr<T> >& v, Ptr<T>& b,
00721 Ptr<T>& x )
00722 {
00723 int jj,j,i;
00724 T s;
00725 Ptr<T> tmp;
00726
00727 tmp.reserve_pool(n);
00728
00729 for( j = 1; j <= n; j++ )
00730 {
00731 s = 0.0;
00732 if( w[j-1] != 0.0 )
00733 {
00734 for( i = 1; i <= m; i++ )
00735 s += u[i-1][j-1] * b[i-1];
00736 s /= w[j-1];
00737 }
00738 tmp[j-1] = s;
00739 }
00740 for( j = 1; j <= n; j++ )
00741 {
00742 s = 0.0;
00743 for ( jj = 1; jj <= n; jj++ )
00744 s += v[j-1][jj-1] * tmp[jj-1];
00745 x[j-1] = s;
00746 }
00747 return;
00748 }
00749 template GULAPI void SVBackSubstitution( int m, int n,
00750 Ptr< Ptr<float> >& u, Ptr<float>& w,
00751 Ptr< Ptr<float> >& v, Ptr<float>& b,
00752 Ptr<float>& x );
00753 template GULAPI void SVBackSubstitution( int m, int n,
00754 Ptr< Ptr<double> >& u, Ptr<double>& w,
00755 Ptr< Ptr<double> >& v, Ptr<double>& b,
00756 Ptr<double>& x );
00757
00758
00759
00760
00761
00762
00763 template< class T >
00764 GULAPI void SVBackSubstitution( int m, int n,
00765 Ptr< Ptr<T> >& u, Ptr<T>& w,
00766 Ptr< Ptr<T> >& v, Ptr<T>& b )
00767 {
00768 int jj,j,i;
00769 T s;
00770 Ptr<T> tmp;
00771
00772 tmp.reserve_pool(n);
00773
00774 for( j = 1; j <= n; j++ )
00775 {
00776 s = 0.0;
00777 if( w[j-1] != 0.0 )
00778 {
00779 for( i = 1; i <= m; i++ )
00780 s += u[i-1][j-1] * b[i-1];
00781 s /= w[j-1];
00782 }
00783 tmp[j-1] = s;
00784 }
00785 for( j = 1; j <= n; j++ )
00786 {
00787 s = 0.0;
00788 for ( jj = 1; jj <= n; jj++ )
00789 s += v[j-1][jj-1] * tmp[jj-1];
00790 b[j-1] = s;
00791 }
00792 return;
00793 }
00794 template GULAPI void SVBackSubstitution( int m, int n,
00795 Ptr< Ptr<float> >& u, Ptr<float>& w,
00796 Ptr< Ptr<float> >& v, Ptr<float>& b );
00797 template GULAPI void SVBackSubstitution( int m, int n,
00798 Ptr< Ptr<double> >& u, Ptr<double>& w,
00799 Ptr< Ptr<double> >& v, Ptr<double>& b );
00800
00801
00802
00803
00804
00805
00806
00807
00808 template< class T >
00809 GULAPI bool SVZero( int n, Ptr<T>& w )
00810 {
00811 T wmax,wmin;
00812 int j;
00813 bool well_conditioned;
00814 T limit = (T)n * rtr<T>::epsilon();
00815
00816 wmax = wmin = rtr<T>::fabs(w[0]);
00817 for( j = 2; j <= n; j++ )
00818 {
00819 if( rtr<T>::fabs(w[j-1]) > wmax ) wmax = rtr<T>::fabs(w[j-1]);
00820 else if( rtr<T>::fabs(w[j-1]) < wmin ) wmin = rtr<T>::fabs(w[j-1]);
00821 }
00822
00823 if( (wmax == 0.0) ||
00824 (wmin / wmax < limit) )
00825 well_conditioned = false;
00826 else
00827 well_conditioned = true;
00828
00829
00830
00831
00832
00833 wmin = wmax * limit;
00834
00835 for( j = 1; j <= n; j++ )
00836 if( wmin > rtr<T>::fabs(w[j-1]) ) w[j-1] = 0.0;
00837
00838 return well_conditioned;
00839 }
00840 template GULAPI bool SVZero( int n, Ptr<float>& w );
00841 template GULAPI bool SVZero( int n, Ptr<double>& w );
00842
00843
00844
00845
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
00857 template< class T >
00858 int SVDIntersectLines( point<T> P1, point<T> T1,
00859 point<T> P2, point<T> T2,
00860 T *alpha1, T *alpha2, point<T> *P )
00861 {
00862 Ptr< Ptr<T> > u,v;
00863 Ptr<T> w, x, b;
00864 point<T> L1,L2;
00865 int code,i;
00866
00867 u.reserve_pool(3);
00868 for( i = 0; i < 3; i++ )
00869 u[i].reserve_pool(2);
00870 v.reserve_pool(2);
00871 for( i = 0; i < 2; i++ )
00872 v[i].reserve_pool(2);
00873 w.reserve_pool(2);
00874 x.reserve_pool(2);
00875 b.reserve_pool(3);
00876
00877 code = 0;
00878
00879 u[0][0] = T1.x; u[0][1] = -T2.x;
00880 u[1][0] = T1.y; u[1][1] = -T2.y;
00881 u[2][0] = T1.z; u[2][1] = -T2.z;
00882
00883 SVDecomposition( 3, 2, u, w, v );
00884
00885 if( SVZero( 2, w ) )
00886 code = 1;
00887
00888 b[0] = P2.x - P1.x;
00889 b[1] = P2.y - P1.y;
00890 b[2] = P2.z - P1.z;
00891
00892 SVBackSubstitution( 3, 2, u, w, v, b, x );
00893
00894 L1.x = P1.x + x[0] * T1.x;
00895 L1.y = P1.y + x[0] * T1.y;
00896 L1.z = P1.z + x[0] * T1.z;
00897
00898 L2.x = P2.x + x[1] * T2.x;
00899 L2.y = P2.y + x[1] * T2.y;
00900 L2.z = P2.z + x[1] * T2.z;
00901
00902
00903 if( !code && !rel_equal(L1, L2, rtr<T>::zero_tol() ) )
00904 code = 2;
00905
00906 P->x = (L1.x + L2.x) / (T)2;
00907 P->y = (L1.y + L2.y) / (T)2;
00908 P->z = (L1.z + L2.z) / (T)2;
00909
00910 *alpha1 = x[0];
00911 *alpha2 = x[1];
00912
00913 return code;
00914 }
00915 template int SVDIntersectLines( point<float> P1, point<float> T1,
00916 point<float> P2, point<float> T2,
00917 float *alpha1, float *alpha2, point<float> *P );
00918 template int SVDIntersectLines( point<double> P1, point<double> T1,
00919 point<double> P2, point<double> T2,
00920 double *alpha1, double *alpha2, point<double> *P );
00921
00922
00923
00924
00925
00926
00927
00928 #if 0
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972 void FindPlane( int nP, const Ptr< point<T> >& P )
00973 {
00974 A.reserve_pool(4); for( i=0; i<4; i++ ) A[i].reserve_pool(4);
00975 V.reserve_pool(4); for( i=0; i<4; i++ ) V[i].reserve_pool(4);
00976 w.reserve_pool(4);
00977
00978 for( i = 0; i < 4; i++ )
00979 for( j = 0; j < 4; j++ )
00980 A[i][j] = (T)0;
00981
00982 for( i = 0; i < nP; i++ )
00983 {
00984 x = P[i].x; y = P[i].y; z = P[i].y;
00985
00986 A[0][0] += x*x; A[0][1] += x*y; A[0][2] += x*z; A[0][3] += x;
00987 A[1][0] += y*x; A[1][1] += y*y; A[1][2] += y*z; A[1][3] += y;
00988 A[2][0] += z*x; A[2][1] += z*y; A[2][2] += z*z; A[2][3] += z;
00989 A[3][0] += x; A[3][1] += y; A[3][2] += z; A[3][3] += (T)1;
00990 }
00991
00992
00993
00994 }
00995 #endif
00996
00997 }
00998
00999
01000
01001