Motion Planning Kernel: math/svd.cpp Source File

00001 /************************************************************************/
00002 /* dsvdcmp.C -- contains routines for doing SVD and finding 2-norm      */
00003 /*      condition numbers for matrices.                                 */
00004 /*                                                                      */
00005 /*      Author: Code originally from Numerical Recipes in C, 2nd ed.    */
00006 /*              modifications by A. Thall.                              */
00007 /*      Date:   Mods done 2. Feb 97.                                    */
00008 /*                                                                      */
00009 /*      Notes:  1.  NumRec routines are faux FORTRAN; in original code, */
00010 /*              all matrices and vectors were 1-indexed.  This has been */
00011 /*              remedied here.                                          */
00012 /*              2.  This code is straight C, compiled under C++ to      */
00013 /*              avoid having to do an 'extern "C"' declaration, but no  */
00014 /*              big deal either way.                                    */ 
00015 /*                                                                      */
00016 /************************************************************************/
00017 #include <math.h>
00018 #include <stdio.h>
00019 #include <stdlib.h>
00020 
00021 void dsvdcmp(double **a, int m, int n, double *w, double **v);
00022 
00023 int svd(int m,int n, double **a,double *q,double **u,double **v)
00024 {
00025         int i,j;
00026 
00027         for (i=0; i<m; i++)
00028                 for (j=0; j<n; j++)
00029                 {
00030                         u[i][j] = a[i][j];
00031                 }
00032 
00033         dsvdcmp(u, m, n, q, v);
00034         return 0;
00035 }
00036 
00037 /************************************************************************/
00038 /* Some macros for comparisons, vector allocation, etc.                 */
00039 /************************************************************************/
00040 static double dsqrarg;
00041 #define DSQR(a) ((dsqrarg=(a)) == 0.0 ? 0.0 : dsqrarg*dsqrarg)
00042 static double dmaxarg1,dmaxarg2;
00043 #define DMAX(a,b) (dmaxarg1=(a),dmaxarg2=(b),(dmaxarg1) > (dmaxarg2) ?\
00044         (dmaxarg1) : (dmaxarg2))
00045 static double dminarg1,dminarg2;
00046 #define DMIN(a,b) (dminarg1=(a),dminarg2=(b),(dminarg1) < (dminarg2) ?\
00047         (dminarg1) : (dminarg2))
00048 static int iminarg1,iminarg2;
00049 #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
00050         (iminarg1) : (iminarg2))
00051 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00052 
00053 #define DVECTOR(size) ((double *) malloc((unsigned int) size*sizeof(double)))
00054 
00055 /************************************************************************/
00056 /* dpythag() -- a double routine for stable computation of Euclidean    */
00057 /*      length of vector [a, b] without overflow.                       */
00058 /************************************************************************/
00059 double dpythag(double a, double b)
00060 {
00061         double absa,absb;
00062         absa=fabs(a);
00063         absb=fabs(b);
00064         if (absa > absb) return absa*sqrt(1.0+DSQR(absb/absa));
00065         else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+DSQR(absa/absb)));
00066 }
00067 
00068 /************************************************************************/
00069 /* dsvdcmp() -- a routine (now 0-index) originally from NumRec in C     */
00070 /*      for doing SVD decomposition of an m-by-n matrix.                */
00071 /*      For what meager documentation there is, see NumRec in C, 2nd    */
00072 /*      edition, pg. 67-70.                                             */
00073 /* This function expects a to be an m-by-n matrix in row-major order,   */
00074 /*      w to be a double array of length n, and v to be an n-by-n       */
00075 /*      matrix.  Matrix a will be replaced by U, v will receive the     */
00076 /*      values of V (not V'), and w will get the diagonal components    */
00077 /*      of W, the singular values, where a = U*W*V'.                    */
00078 /************************************************************************/
00079 void dsvdcmp(double **a, int m, int n, double *w, double **v)
00080 {
00081     int flag,i,its,j,jj,k,l,nm;
00082     double anorm,c,f,g,h,s,scale,x,y,z,*rv1;
00083 
00084     rv1=DVECTOR(n);
00085     g=scale=anorm=0.0;
00086     for (i=0;i<n;i++) {
00087         l=i+1;
00088         rv1[i]=scale*g;
00089         g=s=scale=0.0;
00090         if (i < m) {
00091                 for (k=i;k<m;k++) scale += fabs(a[k][i]);
00092                 if (scale) {
00093                         for (k=i;k<m;k++) {
00094                                 a[k][i] /= scale;
00095                                 s += a[k][i]*a[k][i];
00096                         }
00097                         f=a[i][i];
00098                         g = -SIGN(sqrt(s),f);
00099                         h=f*g-s;
00100                         a[i][i]=f-g;
00101                         for (j=l;j<n;j++) {
00102                                 for (s=0.0,k=i;k<m;k++) s += a[k][i]*a[k][j];
00103                                 f=s/h;
00104                                 for (k=i;k<m;k++) a[k][j] += f*a[k][i];
00105                         }
00106                         for (k=i;k<m;k++) a[k][i] *= scale;
00107                 }
00108         }
00109         w[i]=scale *g;
00110         g=s=scale=0.0;
00111         if (i < m && i != (n - 1)) {
00112                 for (k=l;k<n;k++) scale += fabs(a[i][k]);
00113                 if (scale) {
00114                         for (k=l;k<n;k++) {
00115                                 a[i][k] /= scale;
00116                                 s += a[i][k]*a[i][k];
00117                         }
00118                         f=a[i][l];
00119                         g = -SIGN(sqrt(s),f);
00120                         h=f*g-s;
00121                         a[i][l]=f-g;
00122                         for (k=l;k<n;k++) rv1[k]=a[i][k]/h;
00123                         for (j=l;j<m;j++) {
00124                                 for (s=0.0,k=l;k<n;k++) s += a[j][k]*a[i][k];
00125                                 for (k=l;k<n;k++) a[j][k] += s*rv1[k];
00126                         }
00127                         for (k=l;k<n;k++) a[i][k] *= scale;
00128                 }
00129         }
00130         anorm=DMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
00131     }
00132     for (i = (n-1); i >= 0;i--) {
00133         if (i < (n-1)) {
00134                 if (g) {
00135                         for (j=l;j<n;j++) v[j][i]=(a[i][j]/a[i][l])/g;
00136                         for (j=l;j<n;j++) {
00137                                 for (s=0.0,k=l;k<n;k++) s += a[i][k]*v[k][j];
00138                                 for (k=l;k<n;k++) v[k][j] += s*v[k][i];
00139                         }
00140                 }
00141         for (j=l;j<n;j++) v[i][j]=v[j][i]=0.0;
00142         }
00143         v[i][i]=1.0;
00144         g=rv1[i];
00145         l=i;
00146     }
00147     for (i=IMIN(m,n)-1;i>=0;i--) {
00148         l=i+1;
00149         g=w[i];
00150         for (j=l;j<n;j++) a[i][j]=0.0;
00151         if (g) {
00152                 g=1.0/g;
00153                 for (j=l;j<n;j++) {
00154                         for (s=0.0,k=l;k<m;k++) s += a[k][i]*a[k][j];
00155                         f=(s/a[i][i])*g;
00156                         for (k=i;k<m;k++) a[k][j] += f*a[k][i];
00157                 }
00158                 for (j=i;j<m;j++) a[j][i] *= g;
00159         } else for (j=i;j<m;j++) a[j][i]=0.0;
00160         ++a[i][i];
00161     }
00162     for (k = (n-1); k>=0; k--) {
00163         for (its=1;its<=30;its++) {
00164                 flag=1;
00165                 for (l=k;l>=0;l--) {
00166                         nm=l-1;
00167                         if ((double)(fabs(rv1[l])+anorm) == anorm) {
00168                                 flag=0;
00169                                 break;
00170                         }
00171                         if ((double)(fabs(w[nm])+anorm) == anorm) break;
00172                 }
00173                 if (flag) {
00174                         c=0.0;
00175                         s=1.0;
00176                         for (i=l;i<=k;i++) {
00177                                 f=s*rv1[i];
00178                                 rv1[i]=c*rv1[i];
00179                                 if ((double)(fabs(f)+anorm) == anorm) break;
00180                                 g=w[i];
00181                                 h=dpythag(f,g);
00182                                 w[i]=h;
00183                                 h=1.0/h;
00184                                 c=g*h;
00185                                 s = -f*h;
00186                                 for (j=0;j<m;j++) {
00187                                         y=a[j][nm];
00188                                         z=a[j][i];
00189                                         a[j][nm]=y*c+z*s;
00190                                         a[j][i]=z*c-y*s;
00191                                 }
00192                         }
00193                 }
00194                 z=w[k];
00195                 if (l == k) {
00196                         if (z < 0.0) {
00197                                 w[k] = -z;
00198                                 for (j=0;j<n;j++) v[j][k] = -v[j][k];
00199                         }
00200                         break;
00201                 }
00202                 if (its == 30)
00203                     fprintf(stderr, "no convergence in 30 dsvdcmp iterations");
00204                 x=w[l];
00205                 nm=k-1;
00206                 y=w[nm];
00207                 g=rv1[nm];
00208                 h=rv1[k];
00209                 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00210                 g=dpythag(f,1.0);
00211                 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00212                 c=s=1.0;
00213                 for (j=l;j<=nm;j++) { 
00214                         i=j+1;
00215                         g=rv1[i];
00216                         y=w[i];
00217                         h=s*g;
00218                         g=c*g;
00219                         z=dpythag(f,h);
00220                         rv1[j]=z;
00221                         c=f/z;
00222                         s=h/z;
00223                         f=x*c+g*s;
00224                         g = g*c-x*s;
00225                         h=y*s;
00226                         y *= c;
00227                         for (jj=0;jj<n;jj++) {
00228                                 x=v[jj][j];
00229                                 z=v[jj][i];
00230                                 v[jj][j]=x*c+z*s;
00231                                 v[jj][i]=z*c-x*s;
00232                         }
00233                         z=dpythag(f,h);
00234                         w[j]=z;
00235                         if (z) {
00236                                 z=1.0/z;
00237                                 c=f*z;
00238                                 s=h*z;
00239                         }
00240                         f=c*g+s*y;
00241                         x=c*y-s*g;
00242                         for (jj=0;jj<m;jj++) {
00243                         y=a[jj][j];
00244                                 z=a[jj][i];
00245                                 a[jj][j]=y*c+z*s;
00246                                 a[jj][i]=z*c-y*s;
00247                         }
00248                 }
00249                 rv1[l]=0.0;
00250                 rv1[k]=f;
00251                 w[k]=x;
00252         }
00253     }
00254     free(rv1);
00255 }
00256 /* (C) Copr. 1986-92 Numerical Recipes Software 42,. */
00257