Initial revision
[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
269
270 // A is replaced by the column orthogonal matrix U 
271
272
273 template <class MaTRiX, class VecToR >
274 void SVD_a( MaTRiX &a, VecToR &w,  MaTRiX &v) {
275
276         int n = a.num_cols();
277         int m = a.num_rows();
278
279         int flag,i,its,j,jj,k,l,nm;
280         typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
281
282         VecToR work_space;
283         work_space.newsize(n);
284
285         g = scale = anorm = 0.0;
286         
287         for (i=1;i <=n;i++) {
288                 l = i+1;
289                 work_space(i) = scale*g;
290                 g = s=scale=0.0;
291
292                 if (i <= m) {
293                         for(k=i; k<=m; k++) scale += abs(a(k,i));
294
295                         if (scale) {
296                                 for (k = i; k <=m ; k++) {
297                                         a(k,i) /= scale;
298                                         s += a(k,i)*a(k,i);
299                                 }
300                                 f = a(i,i);
301                                 g = -sign(sqrt(s),f);
302                                 h = f*g -s;
303                                 a(i,i) = f-g;
304         
305                                 for (j = l; j <=n; j++) {
306                                         for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j);
307                                         f = s/h;
308                                         for (k = i; k <= m; k++) a(k,j) += f*a(k,i);
309                                 }
310                                 for (k = i; k <=m;k++) a(k,i) *= scale;
311                         }
312                 }
313
314                 w(i) = scale*g;
315                 g = s = scale = 0.0;
316
317                 if (i <=m && i != n) {
318                         for (k = l; k <=n;k++) scale += abs(a(i,k));
319                         if (scale) {
320                                 for(k = l;k <=n;k++) {
321                                         a(i,k) /= scale;
322                                         s += a(i,k) * a(i,k);
323                                 }
324
325                                 f = a(i,l);
326                                 g = -sign(sqrt(s),f);
327                                 h= f*g -s;
328                                 a(i,l) = f-g;
329                                 for(k=l;k<=n;k++) work_space(k) = a(i,k)/h;
330                                 for (j=l;j<=m;j++) {
331                                         for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k);
332                                         for(k=l;k<=n;k++) a(j,k) += s*work_space(k);
333                                 }
334                                 for(k=l;k<=n;k++) a(i,k)*=scale;
335                         }
336                 }
337                 anorm = max(anorm,(abs(w(i)) + abs(work_space(i))));
338         }
339         for (i=n;i>=1;i--) {
340                 if (i <n) {
341                         if (g) {
342                                 for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g;
343                                 for(j=l;j<=n;j++) {
344                                         for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j);
345                                         for(k=l; k<=n;k++) v(k,j) +=s*v(k,i);
346                                 }
347                         }
348                         for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0;
349                 }
350                 v(i,i) = 1.0;
351                 g = work_space(i);
352                 l = i;
353         }
354
355         for (i = min(m,n);i>=1;i--) {
356                 l = i+1;
357                 g = w(i);
358                 for (j=l;j <=n;j++) a(i,j) = 0.0;
359                 if (g) {
360                         g = 1.0/g;
361                         for (j=l;j<=n;j++) {
362                                 for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j);
363                                 f = (s/a(i,i))*g;
364                                 for (k=i;k<=m;k++) a(k,j) += f*a(k,i);  
365                         }
366                         for (j=i;j<=m;j++) a(j,i)*=g;
367                 } else {
368                         for (j=i;j<=m;j++) a(j,i) = 0.0;
369                 }
370                 ++a(i,i);
371         }
372
373         for (k=n;k>=1;k--) {
374                 for (its=1;its<=30;its++) {
375                         flag=1;
376                         for(l=k;l>=1;l--) {
377                                 nm = l-1;
378                                 if (abs(work_space(l)) + anorm == anorm) {
379                                         flag = 0;
380                                         break;
381                                 }
382                                 if (abs(w(nm)) + anorm == anorm) break;
383                         }
384                         if (flag) {
385                                 c = 0.0;
386                                 s = 1.0;
387                                 for (i=l;i<=k;i++) {
388                                         f = s*work_space(i);
389                                         work_space(i) = c*work_space(i);
390                                         if (abs(f) +anorm == anorm) break;
391                                         g = w(i);
392                                         h  = pythag(f,g);
393                                         w(i) = h;
394                                         h = 1.0/h;
395                                         c = g*h;
396                                         s = -f*h;
397                                         for (j=1;j<=m;j++) {
398                                                 y= a(j,nm);
399                                                 z=a(j,i);
400                                                 a(j,nm) = y*c + z*s;
401                                                 a(j,i) = z*c - y*s;
402                                         }
403                                 }
404                         }
405                         z=w(k);
406                         if (l==k) {
407                                 if (z <0.0) {
408                                         w(k) = -z;
409                                         for (j=1;j<=n;j++) v(j,k) = -v(j,k);
410                                 }
411                                 break;
412                         }
413
414                         if (its == 30) assert(false);
415
416                         x=w(l);
417                         nm=k-1;
418                         y=w(nm);
419                         g=work_space(nm);
420                         h=work_space(k);
421                         
422                         f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
423                         g = pythag(f,1.0);
424                         f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x;
425                         c=s=1.0;
426
427                         for (j=l;j<=nm;j++) {
428                                 i=j+1;
429                                 g = work_space(i);
430                                 y=w(i);
431                                 h=s*g;
432                                 g=c*g;
433                                 z=pythag(f,h);
434                                 work_space(j) = z;
435                                 c=f/z;
436                                 s=h/z;
437                                 f=x*c + g*s;
438                                 g= g*c - x*s;
439                                 h=y*s;
440                                 y*=c;
441                                 for(jj=1;jj<=n;jj++) {
442                                         x=v(jj,j);
443                                         z=v(jj,i);
444                                         v(jj,j) = x*c + z*s;
445                                         v(jj,i) = z*c- x*s;
446                                 }
447                                 z=pythag(f,h);
448                                 w(j)=z;
449                                 if(z) {
450                                         z = 1.0/z;
451                                         c=f*z;
452                                         s=h*z;
453                                 }
454                                 f=c*g + s*y;
455                                 x= c*y-s*g;
456                         
457                                 for(jj=1;jj<=m;jj++) {
458                                         y=a(jj,j);
459                                         z=a(jj,i);
460                                         a(jj,j) = y*c+z*s;
461                                         a(jj,i) = z*c - y*s;
462                                 }
463                         }
464
465                         work_space(l) = 0.0;
466                         work_space(k) = f;
467                         w(k) = x;
468                 }
469         }
470 }
471 }
472 #endif
473
474
475
476
477
478
479
480
481
482
483
484                                 
485
486         
487
488
489
490
491
492
493
494
495
496
497
498
499                                         
500
501
502
503
504
505
506         
507