Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / utilities / svd_eigen_HH.hpp
1 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16 // Lesser General Public License for more details.
17
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
21
22
23 //Based on the svd of the KDL-0.2 library by Erwin Aertbelien
24 #ifndef SVD_EIGEN_HH_HPP
25 #define SVD_EIGEN_HH_HPP
26
27
28 #include <Eigen/Array>
29 #include <algorithm>
30
31 namespace KDL
32 {
33     template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
34         double at,bt,ct;
35         at = fabs(a);
36         bt = fabs(b);
37         if (at > bt ) {
38             ct=bt/at;
39             return Scalar(at*sqrt(1.0+ct*ct));
40         } else {
41             if (bt==0)
42                 return Scalar(0.0);
43             else {
44                 ct=at/bt;
45                 return Scalar(bt*sqrt(1.0+ct*ct));
46             }
47         }
48     }
49
50
51     template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
52         return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
53     }
54
55     /**
56      * svd calculation of boost ublas matrices
57      *
58      * @param A matrix<double>(mxn)
59      * @param U matrix<double>(mxn)
60      * @param S vector<double> n
61      * @param V matrix<double>(nxn)
62      * @param tmp vector<double> n
63      * @param maxiter defaults to 150
64      *
65      * @return -2 if maxiter exceeded, 0 otherwise
66      */
67         template<typename MatrixA, typename MatrixUV, typename VectorS> 
68         int svd_eigen_HH(
69                 const Eigen::MatrixBase<MatrixA>&               A,
70                 Eigen::MatrixBase<MatrixUV>&                    U,
71                 Eigen::MatrixBase<VectorS>&                             S,
72                 Eigen::MatrixBase<MatrixUV>&                    V,
73                 Eigen::MatrixBase<VectorS>&                             tmp,
74                 int maxiter=150)
75         {
76         //get the rows/columns of the matrix
77         const int rows = A.rows();
78         const int cols = A.cols();
79         
80         U = A;
81         
82         int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
83         int ppi(0);
84         bool flag;
85         e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
86         
87         g=scale=anorm=e_scalar(0.0);
88         
89         /* Householder reduction to bidiagonal form. */
90         for (i=0;i<cols;i++) {
91             ppi=i+1;
92             tmp(i)=scale*g;
93             g=s=scale=e_scalar(0.0); 
94             if (i<rows) {
95                 // compute the sum of the i-th column, starting from the i-th row
96                 for (k=i;k<rows;k++) scale += fabs(U(k,i));
97                 if (scale!=0) {
98                     // multiply the i-th column by 1.0/scale, start from the i-th element
99                     // sum of squares of column i, start from the i-th element
100                     for (k=i;k<rows;k++) {
101                         U(k,i) /= scale;
102                         s += U(k,i)*U(k,i);
103                     }
104                     f=U(i,i);  // f is the diag elem
105                     g = -SIGN(e_scalar(sqrt(s)),f);
106                     h=f*g-s;
107                     U(i,i)=f-g;
108                     for (j=ppi;j<cols;j++) {
109                         // dot product of columns i and j, starting from the i-th row
110                         for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
111                         f=s/h;
112                         // copy the scaled i-th column into the j-th column
113                         for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
114                     }
115                     for (k=i;k<rows;k++) U(k,i) *= scale;
116                 }
117             }
118             // save singular value
119             S(i)=scale*g;
120             g=s=scale=e_scalar(0.0);
121             if ((i <rows) && (i+1 != cols)) {
122                 // sum of row i, start from columns i+1
123                 for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
124                 if (scale!=0) {
125                     for (k=ppi;k<cols;k++) {
126                         U(i,k) /= scale;
127                         s += U(i,k)*U(i,k);
128                     }
129                     f=U(i,ppi);
130                     g = -SIGN(e_scalar(sqrt(s)),f);
131                     h=f*g-s;
132                     U(i,ppi)=f-g;
133                     for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
134                     for (j=ppi;j<rows;j++) {
135                         for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
136                         for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
137                     }
138                     for (k=ppi;k<cols;k++) U(i,k) *= scale;
139                 }
140             }
141             maxarg1=anorm;
142             maxarg2=(fabs(S(i))+fabs(tmp(i)));
143             anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;              
144         }
145         /* Accumulation of right-hand transformations. */
146         for (i=cols-1;i>=0;i--) {
147             if (i<cols-1) {
148                 if (g) {
149                     for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
150                     for (j=ppi;j<cols;j++) {
151                         for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
152                         for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
153                     }
154                 }
155                 for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
156             }
157             V(i,i)=1.0;
158             g=tmp(i);
159             ppi=i;
160         }
161         /* Accumulation of left-hand transformations. */
162         for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
163             ppi=i+1;
164             g=S(i);
165             for (j=ppi;j<cols;j++) U(i,j)=0.0;
166             if (g) {
167                 g=e_scalar(1.0)/g;
168                 for (j=ppi;j<cols;j++) {
169                     for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
170                     f=(s/U(i,i))*g;
171                     for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
172                 }
173                 for (j=i;j<rows;j++) U(j,i) *= g;
174             } else {
175                 for (j=i;j<rows;j++) U(j,i)=0.0;
176             }
177             ++U(i,i);
178         }
179         
180         /* Diagonalization of the bidiagonal form. */
181         for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
182             for (its=1;its<=maxiter;its++) {  /* Loop over allowed iterations. */
183                 flag=true;
184                 for (ppi=k;ppi>=0;ppi--) {  /* Test for splitting. */
185                     nm=ppi-1;             /* Note that tmp(1) is always zero. */
186                     if ((fabs(tmp(ppi))+anorm) == anorm) {
187                         flag=false;
188                         break;
189                     }
190                     if ((fabs(S(nm)+anorm) == anorm)) break;
191                 }
192                 if (flag) {
193                     c=e_scalar(0.0);           /* Cancellation of tmp(l), if l>1: */
194                     s=e_scalar(1.);
195                     for (i=ppi;i<=k;i++) {
196                         f=s*tmp(i);
197                         tmp(i)=c*tmp(i);
198                         if ((fabs(f)+anorm) == anorm) break;
199                         g=S(i);
200                         h=PYTHAG(f,g);
201                         S(i)=h;
202                         h=e_scalar(1.0)/h;
203                         c=g*h;
204                         s=(-f*h);
205                         for (j=0;j<rows;j++) {
206                             y=U(j,nm);
207                             z=U(j,i);
208                             U(j,nm)=y*c+z*s;
209                             U(j,i)=z*c-y*s;
210                         }
211                     }
212                 }
213                 z=S(k);
214                 
215                 if (ppi == k) {       /* Convergence. */
216                     if (z < e_scalar(0.0)) {   /* Singular value is made nonnegative. */
217                         S(k) = -z;
218                         for (j=0;j<cols;j++) V(j,k)=-V(j,k);
219                     }
220                     break;
221                 }
222                 
223                 x=S(ppi);            /* Shift from bottom 2-by-2 minor: */
224                 nm=k-1;
225                 y=S(nm);
226                 g=tmp(nm);
227                 h=tmp(k);
228                 f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
229                 
230                 g=PYTHAG(f,e_scalar(1.0));
231                 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
232                 
233                 /* Next QR transformation: */
234                 c=s=1.0;
235                 for (j=ppi;j<=nm;j++) {
236                     i=j+1;
237                     g=tmp(i);
238                     y=S(i);
239                     h=s*g;
240                     g=c*g;
241                     z=PYTHAG(f,h);
242                     tmp(j)=z;
243                     c=f/z;
244                     s=h/z;
245                     f=x*c+g*s;
246                     g=g*c-x*s;
247                     h=y*s;
248                     y=y*c;
249                     for (jj=0;jj<cols;jj++) {
250                         x=V(jj,j);
251                         z=V(jj,i);
252                         V(jj,j)=x*c+z*s;
253                         V(jj,i)=z*c-x*s;
254                     }
255                     z=PYTHAG(f,h);
256                     S(j)=z;
257                     if (z) {
258                         z=e_scalar(1.0)/z;
259                         c=f*z;
260                         s=h*z;
261                     }
262                     f=(c*g)+(s*y);
263                     x=(c*y)-(s*g);
264                     for (jj=0;jj<rows;jj++) {
265                         y=U(jj,j);
266                         z=U(jj,i);
267                         U(jj,j)=y*c+z*s;
268                         U(jj,i)=z*c-y*s;
269                     }
270                 }
271                 tmp(ppi)=0.0;
272                 tmp(k)=f;
273                 S(k)=x;
274             }
275         }
276
277         //Sort eigen values:
278         for (i=0; i<cols; i++){
279             
280             double S_max = S(i);
281             int i_max = i;
282             for (j=i+1; j<cols; j++){
283                 double Sj = S(j);
284                 if (Sj > S_max){
285                     S_max = Sj;
286                     i_max = j;
287                 }
288             }
289             if (i_max != i){
290                 /* swap eigenvalues */
291                 e_scalar tmp = S(i);
292                 S(i)=S(i_max);
293                 S(i_max)=tmp;
294                 
295                 /* swap eigenvectors */
296                 U.col(i).swap(U.col(i_max));
297                 V.col(i).swap(V.col(i_max));
298             }
299         }
300         
301         
302         if (its == maxiter) 
303             return (-2);
304         else 
305             return (0);
306     }
307
308 }
309 #endif