tools/quake2/qdata_heretic2/svdcmp.c

   1 /*\r
   2 Copyright (C) 1999-2007 id Software, Inc. and contributors.\r
   3 For a list of contributors, see the accompanying CONTRIBUTORS file.\r
   4 \r
   5 This file is part of GtkRadiant.\r
   6 \r
   7 GtkRadiant is free software; you can redistribute it and/or modify\r
   8 it under the terms of the GNU General Public License as published by\r
   9 the Free Software Foundation; either version 2 of the License, or\r
  10 (at your option) any later version.\r
  11 \r
  12 GtkRadiant is distributed in the hope that it will be useful,\r
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  15 GNU General Public License for more details.\r
  16 \r
  17 You should have received a copy of the GNU General Public License\r
  18 along with GtkRadiant; if not, write to the Free Software\r
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA\r
  20 */\r
  21 \r
  22 #include <stdlib.h>\r
  23 #include <stdio.h>\r
  24 #include <assert.h>\r
  25 #include <string.h>\r
  26 #include <math.h>\r
  27 \r
  28 static double at,bt,ct;\r
  29 #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \\r
  30                      (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))\r
  31 \r
  32   static double maxarg1,maxarg2;\r
  33 #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\\r
  34                   (maxarg1) : (maxarg2))\r
  35 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))\r
  36 \r
  37 void ntrerror(char *s)\r
  38 {\r
  39   printf("%s\n",s);\r
  40   exit(1);\r
  41 }\r
  42 \r
  43 double *allocVect(int sz)\r
  44 {\r
  45         double *ret;\r
  46         \r
  47         ret = calloc(sizeof(double), (size_t)sz);\r
  48         return ret;\r
  49 }\r
  50 \r
  51 void freeVect(double *ret)\r
  52 {\r
  53         free(ret);\r
  54 }\r
  55 \r
  56 double **allocMatrix(int r,int c)\r
  57 {\r
  58         double **ret;\r
  59                 \r
  60         ret = calloc(sizeof(double), (size_t)(r*c));\r
  61         return ret;\r
  62 }\r
  63 \r
  64 void freeMatrix(double **ret,int r)\r
  65 {\r
  66         free(ret);\r
  67 }\r
  68 \r
  69 void svdcmp(double** a, int m, int n, double* w, double** v)\r
  70 {\r
  71   int flag,i,its,j,jj,k,l,nm;\r
  72   double c,f,h,s,x,y,z;\r
  73   double anorm=0.0,g=0.0,scale=0.0;\r
  74   double *rv1;\r
  75   void nrerror();\r
  76 \r
  77   if (m < n) ntrerror("SVDCMP: You must augment A with extra zero rows");\r
  78   rv1=allocVect(n);\r
  79   for (i=1;i<=n;i++) {\r
  80     l=i+1;\r
  81     rv1[i]=scale*g;\r
  82     g=s=scale=0.0;\r
  83     if (i <= m) {\r
  84       for (k=i;k<=m;k++) scale += fabs(a[k][i]);\r
  85       if (scale) {\r
  86         for (k=i;k<=m;k++) {\r
  87           a[k][i] /= scale;\r
  88           s += a[k][i]*a[k][i];\r
  89         }\r
  90         f=a[i][i];\r
  91         g = -SIGN(sqrt(s),f);\r
  92         h=f*g-s;\r
  93         a[i][i]=f-g;\r
  94         if (i != n) {\r
  95           for (j=l;j<=n;j++) {\r
  96             for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];\r
  97             f=s/h;\r
  98             for (k=i;k<=m;k++) a[k][j] += f*a[k][i];\r
  99           }\r
 100         }\r
 101         for (k=i;k<=m;k++) a[k][i] *= scale;\r
 102       }\r
 103     }\r
 104     w[i]=scale*g;\r
 105     g=s=scale=0.0;\r
 106     if (i <= m && i != n) {\r
 107       for (k=l;k<=n;k++) scale += fabs(a[i][k]);\r
 108       if (scale) {\r
 109         for (k=l;k<=n;k++) {\r
 110           a[i][k] /= scale;\r
 111           s += a[i][k]*a[i][k];\r
 112         }\r
 113         f=a[i][l];\r
 114         g = -SIGN(sqrt(s),f);\r
 115         h=f*g-s;\r
 116         a[i][l]=f-g;\r
 117         for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;\r
 118         if (i != m) {\r
 119           for (j=l;j<=m;j++) {\r
 120             for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];\r
 121             for (k=l;k<=n;k++) a[j][k] += s*rv1[k];\r
 122           }\r
 123         }\r
 124         for (k=l;k<=n;k++) a[i][k] *= scale;\r
 125       }\r
 126     }\r
 127     anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));\r
 128   }\r
 129   for (i=n;i>=1;i--) {\r
 130     if (i < n) {\r
 131       if (g) {\r
 132         for (j=l;j<=n;j++)\r
 133           v[j][i]=(a[i][j]/a[i][l])/g;\r
 134         for (j=l;j<=n;j++) {\r
 135           for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];\r
 136           for (k=l;k<=n;k++) v[k][j] += s*v[k][i];\r
 137         }\r
 138       }\r
 139       for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;\r
 140     }\r
 141     v[i][i]=1.0;\r
 142     g=rv1[i];\r
 143     l=i;\r
 144   }\r
 145   for (i=n;i>=1;i--) {\r
 146     l=i+1;\r
 147     g=w[i];\r
 148     if (i < n)\r
 149       for (j=l;j<=n;j++) a[i][j]=0.0;\r
 150     if (g) {\r
 151       g=1.0/g;\r
 152       if (i != n) {\r
 153         for (j=l;j<=n;j++) {\r
 154           for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];\r
 155           f=(s/a[i][i])*g;\r
 156           for (k=i;k<=m;k++) a[k][j] += f*a[k][i];\r
 157         }\r
 158       }\r
 159       for (j=i;j<=m;j++) a[j][i] *= g;\r
 160     } else {\r
 161       for (j=i;j<=m;j++) a[j][i]=0.0;\r
 162     }\r
 163     ++a[i][i];\r
 164   }\r
 165   for (k=n;k>=1;k--) {\r
 166     for (its=1;its<=30;its++) {\r
 167       flag=1;\r
 168       for (l=k;l>=1;l--) {\r
 169         nm=l-1;\r
 170         if (fabs(rv1[l])+anorm == anorm) {\r
 171           flag=0;\r
 172           break;\r
 173         }\r
 174         if (fabs(w[nm])+anorm == anorm) break;\r
 175       }\r
 176       if (flag) {\r
 177         c=0.0;\r
 178         s=1.0;\r
 179         for (i=l;i<=k;i++) {\r
 180           f=s*rv1[i];\r
 181           if (fabs(f)+anorm != anorm) {\r
 182             g=w[i];\r
 183             h=PYTHAG(f,g);\r
 184             w[i]=h;\r
 185             h=1.0/h;\r
 186             c=g*h;\r
 187             s=(-f*h);\r
 188             for (j=1;j<=m;j++) {\r
 189               y=a[j][nm];\r
 190               z=a[j][i];\r
 191               a[j][nm]=y*c+z*s;\r
 192               a[j][i]=z*c-y*s;\r
 193             }\r
 194           }\r
 195         }\r
 196       }\r
 197       z=w[k];\r
 198       if (l == k) {\r
 199         if (z < 0.0) {\r
 200           w[k] = -z;\r
 201           for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);\r
 202         }\r
 203         break;\r
 204       }\r
 205       if (its == 30) ntrerror("No convergence in 30 SVDCMP iterations");\r
 206       x=w[l];\r
 207       nm=k-1;\r
 208       y=w[nm];\r
 209       g=rv1[nm];\r
 210       h=rv1[k];\r
 211       f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);\r
 212       g=PYTHAG(f,1.0);\r
 213       f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;\r
 214       c=s=1.0;\r
 215       for (j=l;j<=nm;j++) {\r
 216         i=j+1;\r
 217         g=rv1[i];\r
 218         y=w[i];\r
 219         h=s*g;\r
 220         g=c*g;\r
 221         z=PYTHAG(f,h);\r
 222         rv1[j]=z;\r
 223         c=f/z;\r
 224         s=h/z;\r
 225         f=x*c+g*s;\r
 226         g=g*c-x*s;\r
 227         h=y*s;\r
 228         y=y*c;\r
 229         for (jj=1;jj<=n;jj++) {\r
 230           x=v[jj][j];\r
 231           z=v[jj][i];\r
 232           v[jj][j]=x*c+z*s;\r
 233           v[jj][i]=z*c-x*s;\r
 234         }\r
 235         z=PYTHAG(f,h);\r
 236         w[j]=z;\r
 237         if (z) {\r
 238           z=1.0/z;\r
 239           c=f*z;\r
 240           s=h*z;\r
 241         }\r
 242         f=(c*g)+(s*y);\r
 243         x=(c*y)-(s*g);\r
 244         for (jj=1;jj<=m;jj++) {\r
 245           y=a[jj][j];\r
 246           z=a[jj][i];\r
 247           a[jj][j]=y*c+z*s;\r
 248           a[jj][i]=z*c-y*s;\r
 249         }\r
 250       }\r
 251       rv1[l]=0.0;\r
 252       rv1[k]=f;\r
 253       w[k]=x;\r
 254     }\r
 255   }\r
 256   freeVect(rv1);\r
 257 }\r
 258 \r
 259 \r
 260 \r
 261 void svbksb(double** u, double* w, double** v,int m, int n, double* b, double* x)\r
 262 {  \r
 263         int jj,j,i; \r
 264         double s,*tmp;\r
 265         tmp=allocVect(n);\r
 266         for (j=1;j<=n;j++) \r
 267         {    \r
 268                 s=0.0;    \r
 269                 if (w[j]) \r
 270                 {\r
 271                         for (i=1;i<=m;i++) \r
 272                                 s += u[i][j]*b[i];   \r
 273                         s /= w[j];   \r
 274                 }   \r
 275                 tmp[j]=s; \r
 276         }\r
 277         for (j=1;j<=n;j++) \r
 278         {   \r
 279                 s=0.0;  \r
 280                 for (jj=1;jj<=n;jj++) \r
 281                         s += v[j][jj]*tmp[jj];\r
 282                 x[j]=s;\r
 283         } \r
 284         freeVect(tmp);\r
 285 }\r
 286 \r
 287 #undef SIGN\r
 288 #undef MAX\r
 289 #undef PYTHAG\r
 290 \r
 291 \r
 292 #if 1\r
 293 void DOsvd(float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize)\r
 294 {\r
 295         int usedfs;\r
 296         int *remap;\r
 297         int i,j;\r
 298         double **da;\r
 299         double **v;\r
 300         double *w;\r
 301         int DOFerr;\r
 302         float mx;\r
 303         int bestat;\r
 304 \r
 305         if (nframes>framesize)\r
 306                 usedfs=nframes;\r
 307         else\r
 308                 usedfs=framesize;\r
 309 \r
 310         da=allocMatrix(usedfs,nframes);\r
 311         v=allocMatrix(nframes,nframes);\r
 312         w=allocVect(nframes);\r
 313 \r
 314         DOFerr = 0; //false\r
 315         for (i=0;i<nframes;i++)\r
 316         {\r
 317                 for (j=0;j<framesize;j++)\r
 318                         da[j+1][i+1]=a[i*framesize+j];\r
 319                 for (;j<usedfs;j++)\r
 320                         da[j+1][i+1]=0.0;\r
 321         }\r
 322 \r
 323         svdcmp(da,usedfs,nframes,w,v);\r
 324 \r
 325         remap = calloc(sizeof(int), (size_t)nframes);\r
 326 \r
 327 \r
 328         for (i=0;i<nframes;i++)\r
 329                 remap[i]=-1;\r
 330         for (j=0;j<compressedsize;j++)\r
 331         {\r
 332                 mx=-1.0f;\r
 333                 for (i=0;i<nframes;i++)\r
 334                 {\r
 335                         if (remap[i]<0&&fabs(w[i+1])>mx)\r
 336                         {\r
 337                                 mx=(float) fabs(w[i+1]);\r
 338                                 bestat=i;\r
 339                         }\r
 340                 }\r
 341 \r
 342                 if(mx>0)\r
 343                 {\r
 344                         remap[bestat]=j;\r
 345                 }\r
 346                 else\r
 347                 {\r
 348                         DOFerr = 1; //true\r
 349                 }\r
 350         }\r
 351 \r
 352         if(DOFerr)\r
 353         {\r
 354                 printf("Warning:  To many degrees of freedom!  File size may increase\n");\r
 355 \r
 356                 for (i=0;i<compressedsize;i++)\r
 357                 {\r
 358                         values[i]=0;\r
 359                         for (j=0;j<framesize;j++)\r
 360                                 res[i*framesize+j]=0;\r
 361                 }\r
 362         }\r
 363 \r
 364         for (i=0;i<nframes;i++)\r
 365         {\r
 366                 if (remap[i]<0)\r
 367                         w[i+1]=0.0;\r
 368                 else\r
 369                 {\r
 370                         values[remap[i]]=(float) w[i+1];\r
 371                         for (j=0;j<framesize;j++)\r
 372                                 res[remap[i]*framesize+j]=(float) da[j+1][i+1];\r
 373                 }\r
 374         }\r
 375         freeVect(w);\r
 376         freeMatrix(v,nframes);\r
 377         freeMatrix(da,framesize);\r
 378         free(remap);\r
 379 }\r
 380 \r
 381 #else\r
 382 \r
 383 void DOsvd(float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize)\r
 384 {\r
 385         int *remap;\r
 386         int i,j;\r
 387         int nrows;\r
 388         nrows=nframes;\r
 389         if (nrows<framesize)\r
 390                 nrows=framesize;\r
 391         double **da=allocMatrix(nrows,framesize);\r
 392         double **v=allocMatrix(framesize,framesize);\r
 393         double *w=allocVect(framesize);\r
 394         float mx;\r
 395         int bestat;\r
 396 \r
 397         for (j=0;j<framesize;j++)\r
 398         {\r
 399                 for (i=0;i<nframes;i++)\r
 400                         da[j+1][i+1]=a[i*framesize+j];\r
 401                 for (;i<nrows;i++)\r
 402                         da[j+1][i+1]=0.0;\r
 403         }\r
 404 \r
 405         svdcmp(da,nrows,framesize,w,v);\r
 406 \r
 407         remap=new int[framesize];\r
 408 \r
 409 \r
 410         for (i=0;i<framesize;i++)\r
 411                 remap[i]=-1;\r
 412         for (j=0;j<compressedsize;j++)\r
 413         {\r
 414                 mx=-1.0f;\r
 415                 for (i=0;i<framesize;i++)\r
 416                 {\r
 417                         if (remap[i]<0&&fabs(w[i+1])>mx)\r
 418                         {\r
 419                                 mx=fabs(w[i+1]);\r
 420                                 bestat=i;\r
 421                         }\r
 422                 }\r
 423                 assert(mx>-.5f);\r
 424                 remap[bestat]=j;\r
 425         }\r
 426         // josh **DO NOT** put your dof>nframes mod here\r
 427         for (i=0;i<framesize;i++)\r
 428         {\r
 429                 if (remap[i]<0)\r
 430                         w[i+1]=0.0;\r
 431                 else\r
 432                 {\r
 433                         values[remap[i]]=w[i+1];\r
 434                         for (j=0;j<framesize;j++)\r
 435                                 res[remap[i]*framesize+j]=v[j+1][i+1];\r
 436                 }\r
 437         }\r
 438         freeVect(w);\r
 439         freeMatrix(v,framesize);\r
 440         freeMatrix(da,nrows);\r
 441         delete[] remap;\r
 442 }\r
 443 \r
 444 #endif\r
 445 \r
 446 void DOsvdPlane(float *pnts,int npnts,float *n,float *base)\r
 447 {\r
 448         int i,j;\r
 449         double **da=allocMatrix(npnts,3);\r
 450         double **v=allocMatrix(3,3);\r
 451         double *w=allocVect(3);\r
 452         float mn=1E30f;\r
 453         int bestat;\r
 454 \r
 455 \r
 456         assert(npnts>=3);\r
 457         base[0]=pnts[0];\r
 458         base[1]=pnts[1];\r
 459         base[2]=pnts[2];\r
 460         for (i=1;i<npnts;i++)\r
 461         {\r
 462                 for (j=0;j<3;j++)\r
 463                         base[j]+=pnts[i*3+j];\r
 464         }\r
 465         base[0]/=(float)(npnts);\r
 466         base[1]/=(float)(npnts);\r
 467         base[2]/=(float)(npnts);\r
 468 \r
 469         for (i=0;i<3;i++)\r
 470         {\r
 471                 for (j=0;j<npnts;j++)\r
 472                         da[j+1][i+1]=pnts[j*3+i]-base[i];\r
 473         }\r
 474 \r
 475         svdcmp(da,npnts,3,w,v);\r
 476         for (i=0;i<3;i++)\r
 477         {\r
 478                 if (fabs(w[i+1])<mn)\r
 479                 {\r
 480                         mn=(float) fabs(w[i+1]);\r
 481                         bestat=i;\r
 482                 }\r
 483         }\r
 484         n[0]=(float) v[1][bestat+1];\r
 485         n[1]=(float) v[2][bestat+1];\r
 486         n[2]=(float) v[3][bestat+1];\r
 487         freeVect(w);\r
 488         freeMatrix(v,3);\r
 489         freeMatrix(da,npnts);\r
 490 }\r