9028b79130d620bdc33e805d196752648edc5729
[blender.git] / intern / iksolver / intern / TNT / svd.h
1 /**
2  * $Id$
3  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version. The Blender
9  * Foundation also sells licenses for use in proprietary software under
10  * the Blender License.  See http://www.blender.org/BL/ for information
11  * about this.
12  *
13  * This program 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
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  *
22  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23  * All rights reserved.
24  *
25  * The Original Code is: all of this file.
26  *
27  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #ifndef SVD_H
33
34 #define SVD_H
35
36 // Compute the Single Value Decomposition of an arbitrary matrix A
37 // That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 
38 // ,W a diagonal matrix and V an orthogonal square matrix s.t. 
39 // A = U.W.Vt. From this decomposition it is trivial to compute the 
40 // inverse of A as Ainv = V.Winv.tranpose(U).
41 // work_space is a temporary vector used by this class to compute
42 // intermediate values during the computation of the SVD. This should
43 // be of length a.num_cols. This is not checked
44
45 #include "tntmath.h"
46
47 namespace TNT
48 {
49
50
51 template <class MaTRiX, class VecToR >
52 void SVD(MaTRiX &a, VecToR &w,  MaTRiX &v, VecToR &work_space) {
53
54                 int n = a.num_cols();
55         int m = a.num_rows();
56
57         int flag,i,its,j,jj,k,l(0),nm(0);
58         typename MaTRiX::value_type c,f,h,x,y,z;
59         typename MaTRiX::value_type anorm(0),g(0),scale(0);
60     typename MaTRiX::value_type s(0);
61
62     work_space.newsize(n);
63
64         for (i=1;i<=n;i++) {
65                 l=i+1;
66                 work_space(i)=scale*g;
67
68                 g = (typename MaTRiX::value_type)0;
69
70                 s = (typename  MaTRiX::value_type)0;
71         scale = (typename  MaTRiX::value_type)0;
72
73                 if (i <= m) {
74                         for (k=i;k<=m;k++) scale += TNT::abs(a(k,i));
75                         if (scale > (typename  MaTRiX::value_type)0) {
76                                 for (k=i;k<=m;k++) {
77                                         a(k,i) /= scale;
78                                         s += a(k,i)*a(k,i);
79                                 }
80                                 f=a(i,i);
81                                 g = -TNT::sign(sqrt(s),f);
82                                 h=f*g-s;
83                                 a(i,i)=f-g;
84                                 if (i != n) {
85                                         for (j=l;j<=n;j++) {
86                         s = (typename  MaTRiX::value_type)0;
87                                                 for (k=i;k<=m;k++) s += a(k,i)*a(k,j);
88                                                 f=s/h;
89                                                 for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
90                                         }
91                                 }
92                                 for (k=i;k<=m;k++) a(k,i) *= scale;
93                         }
94                 }
95                 w(i)=scale*g;
96         g = (typename  MaTRiX::value_type)0;
97         s = (typename  MaTRiX::value_type)0;
98         scale = (typename  MaTRiX::value_type)0;
99                 if (i <= m && i != n) {
100                         for (k=l;k<=n;k++) scale += TNT::abs(a(i,k));
101                         if (scale > (typename  MaTRiX::value_type)0) {
102                                 for (k=l;k<=n;k++) {
103                                         a(i,k) /= scale;
104                                         s += a(i,k)*a(i,k);
105                                 }
106                                 f=a(i,l);
107                                 g = -TNT::sign(sqrt(s),f);
108                                 h=f*g-s;
109                                 a(i,l)=f-g;
110                                 for (k=l;k<=n;k++) work_space(k)=a(i,k)/h;
111                                 if (i != m) {
112                                         for (j=l;j<=m;j++) {
113                         s = (typename  MaTRiX::value_type)0;
114                                                 for (k=l;k<=n;k++) s += a(j,k)*a(i,k);
115                                                 for (k=l;k<=n;k++) a(j,k) += s*work_space(k);
116                                         }
117                                 }
118                                 for (k=l;k<=n;k++) a(i,k) *= scale;
119                         }
120                 }
121                 anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
122         }
123         for (i=n;i>=1;i--) {
124                 if (i < n) {
125                         if (g != (typename  MaTRiX::value_type)0) {
126                                 for (j=l;j<=n;j++)
127                                         v(j,i)=(a(i,j)/a(i,l))/g;
128                                 for (j=l;j<=n;j++) {
129                     s = (typename  MaTRiX::value_type)0;
130                                         for (k=l;k<=n;k++) s += a(i,k)*v(k,j);
131                                         for (k=l;k<=n;k++) v(k,j) += s*v(k,i);
132                                 }
133                         }
134                         for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename  MaTRiX::value_type)0;
135                 }
136                 v(i,i)= (typename  MaTRiX::value_type)1;
137                 g=work_space(i);
138                 l=i;
139         }
140         for (i=n;i>=1;i--) {
141                 l=i+1;
142                 g=w(i);
143                 if (i < n) {
144                         for (j=l;j<=n;j++) a(i,j)= (typename  MaTRiX::value_type)0;
145                 }
146                 if (g !=  (typename  MaTRiX::value_type)0) {
147                         g= ((typename  MaTRiX::value_type)1)/g;
148                         if (i != n) {
149                                 for (j=l;j<=n;j++) {
150                     s =  (typename  MaTRiX::value_type)0;
151                                         for (k=l;k<=m;k++) s += a(k,i)*a(k,j);
152                                         f=(s/a(i,i))*g;
153                                         for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
154                                 }
155                         }
156                         for (j=i;j<=m;j++) a(j,i) *= g;
157                 } else {
158                         for (j=i;j<=m;j++) a(j,i)= (typename  MaTRiX::value_type)0;
159                 }
160                 ++a(i,i);
161         }
162         for (k=n;k>=1;k--) {
163                 for (its=1;its<=30;its++) {
164                         flag=1;
165                         for (l=k;l>=1;l--) {
166                                 nm=l-1;
167                                 if (TNT::abs(work_space(l))+anorm == anorm) {
168                                         flag=0;
169                                         break;
170                                 }
171                                 if (TNT::abs(w(nm))+anorm == anorm) break;
172                         }
173                         if (flag) {
174                                 c= (typename  MaTRiX::value_type)0;
175                                 s= (typename  MaTRiX::value_type)1;
176                                 for (i=l;i<=k;i++) {
177                                         f=s*work_space(i);
178                                         if (TNT::abs(f)+anorm != anorm) {
179                                                 g=w(i);
180                                                 h= (typename  MaTRiX::value_type)TNT::pythag(float(f),float(g));
181                                                 w(i)=h;
182                                                 h= ((typename  MaTRiX::value_type)1)/h;
183                                                 c=g*h;
184                                                 s=(-f*h);
185                                                 for (j=1;j<=m;j++) {
186                                                         y=a(j,nm);
187                                                         z=a(j,i);
188                                                         a(j,nm)=y*c+z*s;
189                                                         a(j,i)=z*c-y*s;
190                                                 }
191                                         }
192                                 }
193                         }
194                         z=w(k);
195                         if (l == k) {
196                                 if (z <  (typename  MaTRiX::value_type)0) {
197                                         w(k) = -z;
198                                         for (j=1;j<=n;j++) v(j,k)=(-v(j,k));
199                                 }
200                                 break;
201                         }
202
203
204 #if 1
205                         if (its == 30)
206                         {
207                                 TNTException an_exception;
208                                 an_exception.i = 0;
209                                 throw an_exception;
210
211                                 return ;
212                                 assert(false);
213                         }
214 #endif
215                         x=w(l);
216                         nm=k-1;
217                         y=w(nm);
218                         g=work_space(nm);
219                         h=work_space(k);
220                         f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename  MaTRiX::value_type)2)*h*y);
221                         g=(typename  MaTRiX::value_type)TNT::pythag(float(f), float(1));
222                         f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x;
223                         c =  (typename  MaTRiX::value_type)1;
224                         s =  (typename  MaTRiX::value_type)1;
225                         for (j=l;j<=nm;j++) {
226                                 i=j+1;
227                                 g=work_space(i);
228                                 y=w(i);
229                                 h=s*g;
230                                 g=c*g;
231                                 z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
232                                 work_space(j)=z;
233                                 c=f/z;
234                                 s=h/z;
235                                 f=x*c+g*s;
236                                 g=g*c-x*s;
237                                 h=y*s;
238                                 y=y*c;
239                                 for (jj=1;jj<=n;jj++) {
240                                         x=v(jj,j);
241                                         z=v(jj,i);
242                                         v(jj,j)=x*c+z*s;
243                                         v(jj,i)=z*c-x*s;
244                                 }
245                                 z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
246                                 w(j)=z;
247                                 if (z !=  (typename  MaTRiX::value_type)0) {
248                                         z= ((typename  MaTRiX::value_type)1)/z;
249                                         c=f*z;
250                                         s=h*z;
251                                 }
252                                 f=(c*g)+(s*y);
253                                 x=(c*y)-(s*g);
254                                 for (jj=1;jj<=m;jj++) {
255                                         y=a(jj,j);
256                                         z=a(jj,i);
257                                         a(jj,j)=y*c+z*s;
258                                         a(jj,i)=z*c-y*s;
259                                 }
260                         }
261                         work_space(l)= (typename  MaTRiX::value_type)0;
262                         work_space(k)=f;
263                         w(k)=x;
264                 }
265         }
266 }
267
268 // A is replaced by the column orthogonal matrix U 
269
270 template <class MaTRiX, class VecToR >
271 void SVD_a( MaTRiX &a, VecToR &w,  MaTRiX &v) {
272
273         int n = a.num_cols();
274         int m = a.num_rows();
275
276         int flag,i,its,j,jj,k,l,nm;
277         typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
278
279         VecToR work_space;
280         work_space.newsize(n);
281
282         g = scale = anorm = 0.0;
283         
284         for (i=1;i <=n;i++) {
285                 l = i+1;
286                 work_space(i) = scale*g;
287                 g = s=scale=0.0;
288
289                 if (i <= m) {
290                         for(k=i; k<=m; k++) scale += abs(a(k,i));
291
292                         if (scale) {
293                                 for (k = i; k <=m ; k++) {
294                                         a(k,i) /= scale;
295                                         s += a(k,i)*a(k,i);
296                                 }
297                                 f = a(i,i);
298                                 g = -sign(sqrt(s),f);
299                                 h = f*g -s;
300                                 a(i,i) = f-g;
301         
302                                 for (j = l; j <=n; j++) {
303                                         for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j);
304                                         f = s/h;
305                                         for (k = i; k <= m; k++) a(k,j) += f*a(k,i);
306                                 }
307                                 for (k = i; k <=m;k++) a(k,i) *= scale;
308                         }
309                 }
310
311                 w(i) = scale*g;
312                 g = s = scale = 0.0;
313
314                 if (i <=m && i != n) {
315                         for (k = l; k <=n;k++) scale += abs(a(i,k));
316                         if (scale) {
317                                 for(k = l;k <=n;k++) {
318                                         a(i,k) /= scale;
319                                         s += a(i,k) * a(i,k);
320                                 }
321
322                                 f = a(i,l);
323                                 g = -sign(sqrt(s),f);
324                                 h= f*g -s;
325                                 a(i,l) = f-g;
326                                 for(k=l;k<=n;k++) work_space(k) = a(i,k)/h;
327                                 for (j=l;j<=m;j++) {
328                                         for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k);
329                                         for(k=l;k<=n;k++) a(j,k) += s*work_space(k);
330                                 }
331                                 for(k=l;k<=n;k++) a(i,k)*=scale;
332                         }
333                 }
334                 anorm = max(anorm,(abs(w(i)) + abs(work_space(i))));
335         }
336         for (i=n;i>=1;i--) {
337                 if (i <n) {
338                         if (g) {
339                                 for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g;
340                                 for(j=l;j<=n;j++) {
341                                         for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j);
342                                         for(k=l; k<=n;k++) v(k,j) +=s*v(k,i);
343                                 }
344                         }
345                         for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0;
346                 }
347                 v(i,i) = 1.0;
348                 g = work_space(i);
349                 l = i;
350         }
351
352         for (i = min(m,n);i>=1;i--) {
353                 l = i+1;
354                 g = w(i);
355                 for (j=l;j <=n;j++) a(i,j) = 0.0;
356                 if (g) {
357                         g = 1.0/g;
358                         for (j=l;j<=n;j++) {
359                                 for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j);
360                                 f = (s/a(i,i))*g;
361                                 for (k=i;k<=m;k++) a(k,j) += f*a(k,i);  
362                         }
363                         for (j=i;j<=m;j++) a(j,i)*=g;
364                 } else {
365                         for (j=i;j<=m;j++) a(j,i) = 0.0;
366                 }
367                 ++a(i,i);
368         }
369
370         for (k=n;k>=1;k--) {
371                 for (its=1;its<=30;its++) {
372                         flag=1;
373                         for(l=k;l>=1;l--) {
374                                 nm = l-1;
375                                 if (abs(work_space(l)) + anorm == anorm) {
376                                         flag = 0;
377                                         break;
378                                 }
379                                 if (abs(w(nm)) + anorm == anorm) break;
380                         }
381                         if (flag) {
382                                 c = 0.0;
383                                 s = 1.0;
384                                 for (i=l;i<=k;i++) {
385                                         f = s*work_space(i);
386                                         work_space(i) = c*work_space(i);
387                                         if (abs(f) +anorm == anorm) break;
388                                         g = w(i);
389                                         h  = pythag(f,g);
390                                         w(i) = h;
391                                         h = 1.0/h;
392                                         c = g*h;
393                                         s = -f*h;
394                                         for (j=1;j<=m;j++) {
395                                                 y= a(j,nm);
396                                                 z=a(j,i);
397                                                 a(j,nm) = y*c + z*s;
398                                                 a(j,i) = z*c - y*s;
399                                         }
400                                 }
401                         }
402                         z=w(k);
403                         if (l==k) {
404                                 if (z <0.0) {
405                                         w(k) = -z;
406                                         for (j=1;j<=n;j++) v(j,k) = -v(j,k);
407                                 }
408                                 break;
409                         }
410
411                         if (its == 30) assert(false);
412
413                         x=w(l);
414                         nm=k-1;
415                         y=w(nm);
416                         g=work_space(nm);
417                         h=work_space(k);
418                         
419                         f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
420                         g = pythag(f,1.0);
421                         f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x;
422                         c=s=1.0;
423
424                         for (j=l;j<=nm;j++) {
425                                 i=j+1;
426                                 g = work_space(i);
427                                 y=w(i);
428                                 h=s*g;
429                                 g=c*g;
430                                 z=pythag(f,h);
431                                 work_space(j) = z;
432                                 c=f/z;
433                                 s=h/z;
434                                 f=x*c + g*s;
435                                 g= g*c - x*s;
436                                 h=y*s;
437                                 y*=c;
438                                 for(jj=1;jj<=n;jj++) {
439                                         x=v(jj,j);
440                                         z=v(jj,i);
441                                         v(jj,j) = x*c + z*s;
442                                         v(jj,i) = z*c- x*s;
443                                 }
444                                 z=pythag(f,h);
445                                 w(j)=z;
446                                 if(z) {
447                                         z = 1.0/z;
448                                         c=f*z;
449                                         s=h*z;
450                                 }
451                                 f=c*g + s*y;
452                                 x= c*y-s*g;
453                         
454                                 for(jj=1;jj<=m;jj++) {
455                                         y=a(jj,j);
456                                         z=a(jj,i);
457                                         a(jj,j) = y*c+z*s;
458                                         a(jj,i) = z*c - y*s;
459                                 }
460                         }
461
462                         work_space(l) = 0.0;
463                         work_space(k) = f;
464                         w(k) = x;
465                 }
466         }
467 }
468
469 }
470 #endif
471