26 int svd_eigen_HH(
const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,
int maxiter,
double epsilon)
29 const int rows = A.rows();
30 const int cols = A.cols();
33 U.topLeftCorner(rows,cols)=A;
35 int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
37 bool flag,maxarg1,maxarg2;
38 double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
41 for (i=0;i<cols;i++) {
47 for (k=i;k<rows;k++) scale += fabs(U(k,i));
48 if (fabs(scale)>epsilon) {
51 for (k=i;k<rows;k++) {
56 if (!(s>=0))
return -3;
60 for (j=ppi;j<cols;j++) {
62 for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
63 if (!(h!=0))
return -4;
66 for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
68 for (k=i;k<rows;k++) U(k,i) *= scale;
74 if ((i <rows) && (i+1 != cols)) {
76 for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
77 if (fabs(scale)>epsilon) {
78 for (k=ppi;k<cols;k++) {
83 if (!(s>=0))
return -5;
87 if (!(h!=0))
return -6;
88 for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
89 for (j=ppi;j<rows;j++) {
90 for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
91 for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
93 for (k=ppi;k<cols;k++) U(i,k) *= scale;
97 maxarg2=(fabs(S(i))+fabs(tmp(i)));
98 anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
101 for (i=cols-1;i>=0;i--) {
104 if (!(U(i,ppi)!=0))
return -7;
105 for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
106 for (j=ppi;j<cols;j++) {
107 for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
108 for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
111 for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
118 for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
121 for (j=ppi;j<cols;j++) U(i,j)=0.0;
122 if (fabs(g)>epsilon) {
124 for (j=ppi;j<cols;j++) {
125 for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
126 if (!(U(i,i)!=0))
return -8;
128 for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
130 for (j=i;j<rows;j++) U(j,i) *= g;
132 for (j=i;j<rows;j++) U(j,i)=0.0;
138 for (k=cols-1;k>=0;k--) {
139 for (its=1;its<=maxiter;its++) {
141 for (ppi=k;ppi>=0;ppi--) {
143 if ((fabs(tmp(ppi))+anorm) == anorm) {
147 if ((fabs(S(nm)+anorm) == anorm))
break;
152 for (i=ppi;i<=k;i++) {
155 if ((fabs(f)+anorm) == anorm)
break;
159 if (!(h!=0))
return -9;
163 for (j=0;j<rows;j++) {
176 for (j=0;j<cols;j++) V(j,k)=-V(j,k);
186 if (!(h!=0&&y!=0))
return -10;
187 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
190 if (!(x!=0))
return -11;
191 if (!((f+
SIGN(g,f))!=0))
return -12;
192 f=((x-z)*(x+z)+h*((y/(f+
SIGN(g,f)))-h))/x;
196 for (j=ppi;j<=nm;j++) {
204 if (!(z!=0))
return -13;
211 for (jj=0;jj<cols;jj++) {
219 if (fabs(z)>epsilon) {
226 for (jj=0;jj<rows;jj++) {
240 for (i=0; i<cols; i++){
244 for (j=i+1; j<cols; j++){
258 U.col(i).swap(U.col(i_max));
259 V.col(i).swap(V.col(i_max));
double SIGN(double a, double b)
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double PYTHAG(double a, double b)