You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
369 lines
8.0 KiB
369 lines
8.0 KiB
/*===========================================================================*\
|
|
* *
|
|
* IsoEx *
|
|
* Copyright (C) 2002 by Computer Graphics Group, RWTH Aachen *
|
|
* www.rwth-graphics.de *
|
|
* *
|
|
*---------------------------------------------------------------------------*
|
|
* *
|
|
* License *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or modify it *
|
|
* under the terms of the GNU Library General Public License as published *
|
|
* by the Free Software Foundation, version 2. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, but *
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
|
|
* Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; if not, write to the Free Software *
|
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. *
|
|
* *
|
|
\*===========================================================================*/
|
|
|
|
//=============================================================================
|
|
//
|
|
// SVD decomposition and back-substitution
|
|
//
|
|
//=============================================================================
|
|
|
|
#define SVD_C
|
|
|
|
//== INCLUDES =================================================================
|
|
|
|
#include "svd.hh"
|
|
#include <math.h>
|
|
#include <iostream>
|
|
|
|
|
|
//== NAMESPACES ===============================================================
|
|
namespace IsoEx {
|
|
namespace Math {
|
|
//=============================================================================
|
|
|
|
|
|
inline double dsqr(double a)
|
|
{
|
|
double dsqrarg(a);
|
|
return (dsqrarg == 0.0 ? 0.0 : dsqrarg*dsqrarg);
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
|
|
inline double dmax(double a, double b)
|
|
{
|
|
double dmaxarg1(a), dmaxarg2(b);
|
|
return (dmaxarg1 > dmaxarg2 ? dmaxarg1 : dmaxarg2);
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
|
|
inline int imin(int a, int b)
|
|
{
|
|
int iminarg1(a), iminarg2(b);
|
|
return (iminarg1 < iminarg2 ? iminarg1 : iminarg2);
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
|
|
inline double sign(double a, double b)
|
|
{
|
|
return (b >= 0.0 ? fabs(a) : -fabs(a));
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
|
|
inline double dpythag(double a, double b)
|
|
{
|
|
double absa(fabs(a)), absb(fabs(b));
|
|
if (absa > absb) return absa*sqrt(1.0+dsqr(absb/absa));
|
|
else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+dsqr(absa/absb)));
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
|
|
template <typename MAT_MxN,
|
|
typename VEC_N,
|
|
typename MAT_NxN>
|
|
bool svd_decomp( MAT_MxN& A, VEC_N& S, MAT_NxN& V )
|
|
{
|
|
int m(A.rows()), n(A.cols());
|
|
int flag, i, its, j, jj, k, l(0), nm(0);
|
|
double anorm(0.0), c, f, g(0.0), h, s, scale(0.0), x, y, z;
|
|
VEC_N rv1(n);
|
|
bool convergence(true);
|
|
|
|
|
|
|
|
for (i=0;i<n;i++)
|
|
{
|
|
l=i+1;
|
|
rv1(i)=scale*g;
|
|
g=s=scale=0.0;
|
|
if (i < m)
|
|
{
|
|
for (k=i;k<m;k++) scale += fabs(A(k,i));
|
|
if (scale)
|
|
{
|
|
for (k=i;k<m;k++)
|
|
{
|
|
A(k,i) /= scale;
|
|
s += A(k,i)*A(k,i);
|
|
}
|
|
f=A(i,i);
|
|
g = -sign(sqrt(s),f);
|
|
h=f*g-s;
|
|
A(i,i)=f-g;
|
|
for (j=l;j<n;j++)
|
|
{
|
|
for (s=0.0,k=i;k<m;k++) s += A(k,i)*A(k,j);
|
|
f=s/h;
|
|
for (k=i;k<m;k++) A(k,j) += f*A(k,i);
|
|
}
|
|
for (k=i;k<m;k++) A(k,i) *= scale;
|
|
}
|
|
}
|
|
S(i)=scale *g;
|
|
g=s=scale=0.0;
|
|
if (i < m && i != (n - 1))
|
|
{
|
|
for (k=l;k<n;k++) scale += fabs(A(i,k));
|
|
if (scale)
|
|
{
|
|
for (k=l;k<n;k++)
|
|
{
|
|
A(i,k) /= scale;
|
|
s += A(i,k)*A(i,k);
|
|
}
|
|
f=A(i,l);
|
|
g = -sign(sqrt(s),f);
|
|
h=f*g-s;
|
|
A(i,l)=f-g;
|
|
for (k=l;k<n;k++) rv1(k)=A(i,k)/h;
|
|
for (j=l;j<m;j++)
|
|
{
|
|
for (s=0.0,k=l;k<n;k++) s += A(j,k)*A(i,k);
|
|
for (k=l;k<n;k++) A(j,k) += s*rv1(k);
|
|
}
|
|
for (k=l;k<n;k++) A(i,k) *= scale;
|
|
}
|
|
}
|
|
anorm=dmax(anorm,(fabs(S(i))+fabs(rv1(i))));
|
|
}
|
|
|
|
for (i = (n-1); i >= 0;i--)
|
|
{
|
|
if (i < (n-1))
|
|
{
|
|
if (g)
|
|
{
|
|
for (j=l;j<n;j++) V(j,i)=(A(i,j)/A(i,l))/g;
|
|
for (j=l;j<n;j++)
|
|
{
|
|
for (s=0.0,k=l;k<n;k++) s += A(i,k)*V(k,j);
|
|
for (k=l;k<n;k++) V(k,j) += s*V(k,i);
|
|
}
|
|
}
|
|
for (j=l;j<n;j++) V(i,j)=V(j,i)=0.0;
|
|
}
|
|
V(i,i)=1.0;
|
|
g=rv1(i);
|
|
l=i;
|
|
}
|
|
|
|
|
|
for (i=imin(m,n)-1;i>=0;i--)
|
|
{
|
|
l=i+1;
|
|
g=S(i);
|
|
for (j=l;j<n;j++) A(i,j)=0.0;
|
|
if (g)
|
|
{
|
|
g=1.0/g;
|
|
for (j=l;j<n;j++)
|
|
{
|
|
for (s=0.0,k=l;k<m;k++) s += A(k,i)*A(k,j);
|
|
f=(s/A(i,i))*g;
|
|
for (k=i;k<m;k++) A(k,j) += f*A(k,i);
|
|
}
|
|
for (j=i;j<m;j++) A(j,i) *= g;
|
|
}
|
|
else for (j=i;j<m;j++) A(j,i)=0.0;
|
|
++A(i,i);
|
|
}
|
|
|
|
|
|
for (k = (n-1); k>=0; k--)
|
|
{
|
|
for (its=1;its<=100;its++)
|
|
{
|
|
flag=1;
|
|
for (l=k;l>=0;l--)
|
|
{
|
|
nm=l-1;
|
|
if ((double)(fabs(rv1(l))+anorm) == anorm)
|
|
{
|
|
flag=0;
|
|
break;
|
|
}
|
|
if ((double)(fabs(S(nm))+anorm) == anorm) break;
|
|
}
|
|
if (flag)
|
|
{
|
|
c=0.0;
|
|
s=1.0;
|
|
for (i=l;i<=k;i++)
|
|
{
|
|
f=s*rv1(i);
|
|
rv1(i)=c*rv1(i);
|
|
if ((double)(fabs(f)+anorm) == anorm) break;
|
|
g=S(i);
|
|
h=dpythag(f,g);
|
|
S(i)=h;
|
|
h=1.0/h;
|
|
c=g*h;
|
|
s = -f*h;
|
|
for (j=0;j<m;j++)
|
|
{
|
|
y=A(j,nm);
|
|
z=A(j,i);
|
|
A(j,nm)=y*c+z*s;
|
|
A(j,i)=z*c-y*s;
|
|
}
|
|
}
|
|
}
|
|
z=S(k);
|
|
if (l == k)
|
|
{
|
|
if (z < 0.0)
|
|
{
|
|
S(k) = -z;
|
|
for (j=0;j<n;j++) V(j,k) = -V(j,k);
|
|
}
|
|
break;
|
|
}
|
|
|
|
|
|
if (its == 30)
|
|
{
|
|
std::cerr << "SVD FAILURE (no convergence after 30 iters)\n";
|
|
convergence = false;
|
|
}
|
|
|
|
x=S(l);
|
|
nm=k-1;
|
|
y=S(nm);
|
|
g=rv1(nm);
|
|
h=rv1(k);
|
|
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
|
|
g=dpythag(f,1.0);
|
|
f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x;
|
|
c=s=1.0;
|
|
|
|
for (j=l;j<=nm;j++)
|
|
{
|
|
i=j+1;
|
|
g=rv1(i);
|
|
y=S(i);
|
|
h=s*g;
|
|
g=c*g;
|
|
z=dpythag(f,h);
|
|
rv1(j)=z;
|
|
c=f/z;
|
|
s=h/z;
|
|
f=x*c+g*s;
|
|
g = g*c-x*s;
|
|
h=y*s;
|
|
y *= c;
|
|
for (jj=0;jj<n;jj++)
|
|
{
|
|
x=V(jj,j);
|
|
z=V(jj,i);
|
|
V(jj,j)=x*c+z*s;
|
|
V(jj,i)=z*c-x*s;
|
|
}
|
|
z=dpythag(f,h);
|
|
S(j)=z;
|
|
if (z)
|
|
{
|
|
z=1.0/z;
|
|
c=f*z;
|
|
s=h*z;
|
|
}
|
|
f=c*g+s*y;
|
|
x=c*y-s*g;
|
|
for (jj=0;jj<m;jj++)
|
|
{
|
|
y=A(jj,j);
|
|
z=A(jj,i);
|
|
A(jj,j)=y*c+z*s;
|
|
A(jj,i)=z*c-y*s;
|
|
}
|
|
}
|
|
rv1(l)=0.0;
|
|
rv1(k)=f;
|
|
S(k)=x;
|
|
}
|
|
}
|
|
|
|
return convergence;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
|
|
template <typename MAT_MxN,
|
|
typename VEC_N,
|
|
typename MAT_NxN,
|
|
typename VEC_M>
|
|
void
|
|
svd_backsub( const MAT_MxN& A, const VEC_M& S, const MAT_NxN& V,
|
|
const VEC_M& b, VEC_N& x )
|
|
{
|
|
int m(A.rows()), n(V.cols());
|
|
int jj, j, i;
|
|
double s;
|
|
VEC_N tmp(n);
|
|
|
|
|
|
// Calculate U^T * B
|
|
for ( j=0; j<n; j++ )
|
|
{
|
|
s = 0.0;
|
|
if (S(j)) // non-zero result only if S_j is nonzero
|
|
{
|
|
for ( i=0; i<m; i++ ) s += A(i,j)*b(i);
|
|
s /= S(j);
|
|
}
|
|
tmp(j) = s;
|
|
}
|
|
|
|
|
|
// Matrix multiply by V to get answer
|
|
for ( j=0; j<n; j++ )
|
|
{
|
|
s = 0.0;
|
|
for ( jj=0; jj<n; jj++ ) s += V(j,jj)*tmp(jj);
|
|
x(j) = s;
|
|
}
|
|
}
|
|
|
|
|
|
//=============================================================================
|
|
} // namespace Math
|
|
} // namespace IsoEx
|
|
//=============================================================================
|
|
|
|
|