diff options
Diffstat (limited to 'intern/iksolver/intern/TNT/svd.h')
-rw-r--r-- | intern/iksolver/intern/TNT/svd.h | 435 |
1 files changed, 0 insertions, 435 deletions
diff --git a/intern/iksolver/intern/TNT/svd.h b/intern/iksolver/intern/TNT/svd.h deleted file mode 100644 index cfff73caa3f..00000000000 --- a/intern/iksolver/intern/TNT/svd.h +++ /dev/null @@ -1,435 +0,0 @@ -/** - */ - -#ifndef SVD_H - -#define SVD_H - -// Compute the Single Value Decomposition of an arbitrary matrix A -// That is compute the 3 matrices U,W,V with U column orthogonal (m,n) -// ,W a diagonal matrix and V an orthogonal square matrix s.t. -// A = U.W.Vt. From this decomposition it is trivial to compute the -// inverse of A as Ainv = V.Winv.tranpose(U). -// -// s = diagonal elements of W -// work1 = workspace, length must be A.num_rows -// work2 = workspace, length must be A.num_cols - -#include "tntmath.h" - -#define SVD_MAX_ITER 200 - -namespace TNT -{ - -template <class MaTRiX, class VecToR > -void SVD(MaTRiX &A, MaTRiX &U, VecToR &s, MaTRiX &V, VecToR &work1, VecToR &work2, int maxiter=SVD_MAX_ITER) { - - int m = A.num_rows(); - int n = A.num_cols(); - int nu = min(m,n); - - VecToR& work = work1; - VecToR& e = work2; - - U = 0; - s = 0; - - int i=0, j=0, k=0; - - // Reduce A to bidiagonal form, storing the diagonal elements - // in s and the super-diagonal elements in e. - - int nct = min(m-1,n); - int nrt = max(0,min(n-2,m)); - for (k = 0; k < max(nct,nrt); k++) { - if (k < nct) { - - // Compute the transformation for the k-th column and - // place the k-th diagonal in s[k]. - // Compute 2-norm of k-th column without under/overflow. - s[k] = 0; - for (i = k; i < m; i++) { - s[k] = hypot(s[k],A[i][k]); - } - if (s[k] != 0.0) { - if (A[k][k] < 0.0) { - s[k] = -s[k]; - } - for (i = k; i < m; i++) { - A[i][k] /= s[k]; - } - A[k][k] += 1.0; - } - s[k] = -s[k]; - } - for (j = k+1; j < n; j++) { - if ((k < nct) && (s[k] != 0.0)) { - - // Apply the transformation. - - typename MaTRiX::value_type t = 0; - for (i = k; i < m; i++) { - t += A[i][k]*A[i][j]; - } - t = -t/A[k][k]; - for (i = k; i < m; i++) { - A[i][j] += t*A[i][k]; - } - } - - // Place the k-th row of A into e for the - // subsequent calculation of the row transformation. - - e[j] = A[k][j]; - } - if (k < nct) { - - // Place the transformation in U for subsequent back - // multiplication. - - for (i = k; i < m; i++) - U[i][k] = A[i][k]; - } - if (k < nrt) { - - // Compute the k-th row transformation and place the - // k-th super-diagonal in e[k]. - // Compute 2-norm without under/overflow. - e[k] = 0; - for (i = k+1; i < n; i++) { - e[k] = hypot(e[k],e[i]); - } - if (e[k] != 0.0) { - if (e[k+1] < 0.0) { - e[k] = -e[k]; - } - for (i = k+1; i < n; i++) { - e[i] /= e[k]; - } - e[k+1] += 1.0; - } - e[k] = -e[k]; - if ((k+1 < m) & (e[k] != 0.0)) { - - // Apply the transformation. - - for (i = k+1; i < m; i++) { - work[i] = 0.0; - } - for (j = k+1; j < n; j++) { - for (i = k+1; i < m; i++) { - work[i] += e[j]*A[i][j]; - } - } - for (j = k+1; j < n; j++) { - typename MaTRiX::value_type t = -e[j]/e[k+1]; - for (i = k+1; i < m; i++) { - A[i][j] += t*work[i]; - } - } - } - - // Place the transformation in V for subsequent - // back multiplication. - - for (i = k+1; i < n; i++) - V[i][k] = e[i]; - } - } - - // Set up the final bidiagonal matrix or order p. - - int p = min(n,m+1); - if (nct < n) { - s[nct] = A[nct][nct]; - } - if (m < p) { - s[p-1] = 0.0; - } - if (nrt+1 < p) { - e[nrt] = A[nrt][p-1]; - } - e[p-1] = 0.0; - - // If required, generate U. - - for (j = nct; j < nu; j++) { - for (i = 0; i < m; i++) { - U[i][j] = 0.0; - } - U[j][j] = 1.0; - } - for (k = nct-1; k >= 0; k--) { - if (s[k] != 0.0) { - for (j = k+1; j < nu; j++) { - typename MaTRiX::value_type t = 0; - for (i = k; i < m; i++) { - t += U[i][k]*U[i][j]; - } - t = -t/U[k][k]; - for (i = k; i < m; i++) { - U[i][j] += t*U[i][k]; - } - } - for (i = k; i < m; i++ ) { - U[i][k] = -U[i][k]; - } - U[k][k] = 1.0 + U[k][k]; - for (i = 0; i < k-1; i++) { - U[i][k] = 0.0; - } - } else { - for (i = 0; i < m; i++) { - U[i][k] = 0.0; - } - U[k][k] = 1.0; - } - } - - // If required, generate V. - - for (k = n-1; k >= 0; k--) { - if ((k < nrt) & (e[k] != 0.0)) { - for (j = k+1; j < nu; j++) { - typename MaTRiX::value_type t = 0; - for (i = k+1; i < n; i++) { - t += V[i][k]*V[i][j]; - } - t = -t/V[k+1][k]; - for (i = k+1; i < n; i++) { - V[i][j] += t*V[i][k]; - } - } - } - for (i = 0; i < n; i++) { - V[i][k] = 0.0; - } - V[k][k] = 1.0; - } - - // Main iteration loop for the singular values. - - int pp = p-1; - int iter = 0; - typename MaTRiX::value_type eps = pow(2.0,-52.0); - while (p > 0) { - int kase=0; - k=0; - - // Test for maximum iterations to avoid infinite loop - if(maxiter == 0) - break; - maxiter--; - - // This section of the program inspects for - // negligible elements in the s and e arrays. On - // completion the variables kase and k are set as follows. - - // kase = 1 if s(p) and e[k-1] are negligible and k<p - // kase = 2 if s(k) is negligible and k<p - // kase = 3 if e[k-1] is negligible, k<p, and - // s(k), ..., s(p) are not negligible (qr step). - // kase = 4 if e(p-1) is negligible (convergence). - - for (k = p-2; k >= -1; k--) { - if (k == -1) { - break; - } - if (TNT::abs(e[k]) <= eps*(TNT::abs(s[k]) + TNT::abs(s[k+1]))) { - e[k] = 0.0; - break; - } - } - if (k == p-2) { - kase = 4; - } else { - int ks; - for (ks = p-1; ks >= k; ks--) { - if (ks == k) { - break; - } - typename MaTRiX::value_type t = (ks != p ? TNT::abs(e[ks]) : 0.) + - (ks != k+1 ? TNT::abs(e[ks-1]) : 0.); - if (TNT::abs(s[ks]) <= eps*t) { - s[ks] = 0.0; - break; - } - } - if (ks == k) { - kase = 3; - } else if (ks == p-1) { - kase = 1; - } else { - kase = 2; - k = ks; - } - } - k++; - - // Perform the task indicated by kase. - - switch (kase) { - - // Deflate negligible s(p). - - case 1: { - typename MaTRiX::value_type f = e[p-2]; - e[p-2] = 0.0; - for (j = p-2; j >= k; j--) { - typename MaTRiX::value_type t = hypot(s[j],f); - typename MaTRiX::value_type cs = s[j]/t; - typename MaTRiX::value_type sn = f/t; - s[j] = t; - if (j != k) { - f = -sn*e[j-1]; - e[j-1] = cs*e[j-1]; - } - - for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][p-1]; - V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1]; - V[i][j] = t; - } - } - } - break; - - // Split at negligible s(k). - - case 2: { - typename MaTRiX::value_type f = e[k-1]; - e[k-1] = 0.0; - for (j = k; j < p; j++) { - typename MaTRiX::value_type t = hypot(s[j],f); - typename MaTRiX::value_type cs = s[j]/t; - typename MaTRiX::value_type sn = f/t; - s[j] = t; - f = -sn*e[j]; - e[j] = cs*e[j]; - - for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][k-1]; - U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; - U[i][j] = t; - } - } - } - break; - - // Perform one qr step. - - case 3: { - - // Calculate the shift. - - typename MaTRiX::value_type scale = max(max(max(max( - TNT::abs(s[p-1]),TNT::abs(s[p-2])),TNT::abs(e[p-2])), - TNT::abs(s[k])),TNT::abs(e[k])); - typename MaTRiX::value_type sp = s[p-1]/scale; - typename MaTRiX::value_type spm1 = s[p-2]/scale; - typename MaTRiX::value_type epm1 = e[p-2]/scale; - typename MaTRiX::value_type sk = s[k]/scale; - typename MaTRiX::value_type ek = e[k]/scale; - typename MaTRiX::value_type b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0; - typename MaTRiX::value_type c = (sp*epm1)*(sp*epm1); - typename MaTRiX::value_type shift = 0.0; - if ((b != 0.0) || (c != 0.0)) { - shift = sqrt(b*b + c); - if (b < 0.0) { - shift = -shift; - } - shift = c/(b + shift); - } - typename MaTRiX::value_type f = (sk + sp)*(sk - sp) + shift; - typename MaTRiX::value_type g = sk*ek; - - // Chase zeros. - - for (j = k; j < p-1; j++) { - typename MaTRiX::value_type t = hypot(f,g); - /* division by zero checks added to avoid NaN (brecht) */ - typename MaTRiX::value_type cs = (t == 0.0f)? 0.0f: f/t; - typename MaTRiX::value_type sn = (t == 0.0f)? 0.0f: g/t; - if (j != k) { - e[j-1] = t; - } - f = cs*s[j] + sn*e[j]; - e[j] = cs*e[j] - sn*s[j]; - g = sn*s[j+1]; - s[j+1] = cs*s[j+1]; - - for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][j+1]; - V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1]; - V[i][j] = t; - } - - t = hypot(f,g); - /* division by zero checks added to avoid NaN (brecht) */ - cs = (t == 0.0f)? 0.0f: f/t; - sn = (t == 0.0f)? 0.0f: g/t; - s[j] = t; - f = cs*e[j] + sn*s[j+1]; - s[j+1] = -sn*e[j] + cs*s[j+1]; - g = sn*e[j+1]; - e[j+1] = cs*e[j+1]; - if (j < m-1) { - for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][j+1]; - U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1]; - U[i][j] = t; - } - } - } - e[p-2] = f; - iter = iter + 1; - } - break; - - // Convergence. - - case 4: { - - // Make the singular values positive. - - if (s[k] <= 0.0) { - s[k] = (s[k] < 0.0 ? -s[k] : 0.0); - - for (i = 0; i <= pp; i++) - V[i][k] = -V[i][k]; - } - - // Order the singular values. - - while (k < pp) { - if (s[k] >= s[k+1]) { - break; - } - typename MaTRiX::value_type t = s[k]; - s[k] = s[k+1]; - s[k+1] = t; - if (k < n-1) { - for (i = 0; i < n; i++) { - t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t; - } - } - if (k < m-1) { - for (i = 0; i < m; i++) { - t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t; - } - } - k++; - } - iter = 0; - p--; - } - break; - } - } -} - -} - -#endif - |