tools/quake2/qdata_heretic2/svdcmp.c

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