tools/quake2/qdata_heretic2/svdcmp.c

   1 /*
   2    Copyright (C) 1999-2007 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         printf( "%s\n",s );
  39         exit( 1 );
  40 }
  41
  42 double *allocVect( int sz ){
  43         double *ret;
  44
  45         ret = calloc( sizeof( double ), (size_t)sz );
  46         return ret;
  47 }
  48
  49 void freeVect( double *ret ){
  50         free( ret );
  51 }
  52
  53 double **allocMatrix( int r,int c ){
  54         double **ret;
  55
  56         ret = calloc( sizeof( double ), (size_t)( r * c ) );
  57         return ret;
  58 }
  59
  60 void freeMatrix( double **ret,int r ){
  61         free( ret );
  62 }
  63
  64 void svdcmp( double** a, int m, int n, double* w, double** v ){
  65         int flag,i,its,j,jj,k,l,nm;
  66         double c,f,h,s,x,y,z;
  67         double anorm = 0.0,g = 0.0,scale = 0.0;
  68         double *rv1;
  69         void nrerror();
  70
  71         if ( m < n ) {
  72                 ntrerror( "SVDCMP: You must augment A with extra zero rows" );
  73         }
  74         rv1 = allocVect( n );
  75         for ( i = 1; i <= n; i++ ) {
  76                 l = i + 1;
  77                 rv1[i] = scale * g;
  78                 g = s = scale = 0.0;
  79                 if ( i <= m ) {
  80                         for ( k = i; k <= m; k++ ) scale += fabs( a[k][i] );
  81                         if ( scale ) {
  82                                 for ( k = i; k <= m; k++ ) {
  83                                         a[k][i] /= scale;
  84                                         s += a[k][i] * a[k][i];
  85                                 }
  86                                 f = a[i][i];
  87                                 g = -SIGN( sqrt( s ),f );
  88                                 h = f * g - s;
  89                                 a[i][i] = f - g;
  90                                 if ( i != n ) {
  91                                         for ( j = l; j <= n; j++ ) {
  92                                                 for ( s = 0.0,k = i; k <= m; k++ ) s += a[k][i] * a[k][j];
  93                                                 f = s / h;
  94                                                 for ( k = i; k <= m; k++ ) a[k][j] += f * a[k][i];
  95                                         }
  96                                 }
  97                                 for ( k = i; k <= m; k++ ) a[k][i] *= scale;
  98                         }
  99                 }
 100                 w[i] = scale * g;
 101                 g = s = scale = 0.0;
 102                 if ( i <= m && i != n ) {
 103                         for ( k = l; k <= n; k++ ) scale += fabs( a[i][k] );
 104                         if ( scale ) {
 105                                 for ( k = l; k <= n; k++ ) {
 106                                         a[i][k] /= scale;
 107                                         s += a[i][k] * a[i][k];
 108                                 }
 109                                 f = a[i][l];
 110                                 g = -SIGN( sqrt( s ),f );
 111                                 h = f * g - s;
 112                                 a[i][l] = f - g;
 113                                 for ( k = l; k <= n; k++ ) rv1[k] = a[i][k] / h;
 114                                 if ( i != m ) {
 115                                         for ( j = l; j <= m; j++ ) {
 116                                                 for ( s = 0.0,k = l; k <= n; k++ ) s += a[j][k] * a[i][k];
 117                                                 for ( k = l; k <= n; k++ ) a[j][k] += s * rv1[k];
 118                                         }
 119                                 }
 120                                 for ( k = l; k <= n; k++ ) a[i][k] *= scale;
 121                         }
 122                 }
 123                 anorm = MAX( anorm,( fabs( w[i] ) + fabs( rv1[i] ) ) );
 124         }
 125         for ( i = n; i >= 1; i-- ) {
 126                 if ( i < n ) {
 127                         if ( g ) {
 128                                 for ( j = l; j <= n; j++ )
 129                                         v[j][i] = ( a[i][j] / a[i][l] ) / g;
 130                                 for ( j = l; j <= n; j++ ) {
 131                                         for ( s = 0.0,k = l; k <= n; k++ ) s += a[i][k] * v[k][j];
 132                                         for ( k = l; k <= n; k++ ) v[k][j] += s * v[k][i];
 133                                 }
 134                         }
 135                         for ( j = l; j <= n; j++ ) v[i][j] = v[j][i] = 0.0;
 136                 }
 137                 v[i][i] = 1.0;
 138                 g = rv1[i];
 139                 l = i;
 140         }
 141         for ( i = n; i >= 1; i-- ) {
 142                 l = i + 1;
 143                 g = w[i];
 144                 if ( i < n ) {
 145                         for ( j = l; j <= n; j++ ) a[i][j] = 0.0;
 146                 }
 147                 if ( g ) {
 148                         g = 1.0 / g;
 149                         if ( i != n ) {
 150                                 for ( j = l; j <= n; j++ ) {
 151                                         for ( s = 0.0,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                 }
 158                 else {
 159                         for ( j = i; j <= m; j++ ) a[j][i] = 0.0;
 160                 }
 161                 ++a[i][i];
 162         }
 163         for ( k = n; k >= 1; k-- ) {
 164                 for ( its = 1; its <= 30; its++ ) {
 165                         flag = 1;
 166                         for ( l = k; l >= 1; l-- ) {
 167                                 nm = l - 1;
 168                                 if ( fabs( rv1[l] ) + anorm == anorm ) {
 169                                         flag = 0;
 170                                         break;
 171                                 }
 172                                 if ( fabs( w[nm] ) + anorm == anorm ) {
 173                                         break;
 174                                 }
 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 ) {
 206                                 ntrerror( "No convergence in 30 SVDCMP iterations" );
 207                         }
 208                         x = w[l];
 209                         nm = k - 1;
 210                         y = w[nm];
 211                         g = rv1[nm];
 212                         h = rv1[k];
 213                         f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0 * h * y );
 214                         g = PYTHAG( f,1.0 );
 215                         f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + SIGN( g,f ) ) ) - h ) ) / x;
 216                         c = s = 1.0;
 217                         for ( j = l; j <= nm; j++ ) {
 218                                 i = j + 1;
 219                                 g = rv1[i];
 220                                 y = w[i];
 221                                 h = s * g;
 222                                 g = c * g;
 223                                 z = PYTHAG( f,h );
 224                                 rv1[j] = z;
 225                                 c = f / z;
 226                                 s = h / z;
 227                                 f = x * c + g * s;
 228                                 g = g * c - x * s;
 229                                 h = y * s;
 230                                 y = y * c;
 231                                 for ( jj = 1; jj <= n; jj++ ) {
 232                                         x = v[jj][j];
 233                                         z = v[jj][i];
 234                                         v[jj][j] = x * c + z * s;
 235                                         v[jj][i] = z * c - x * s;
 236                                 }
 237                                 z = PYTHAG( f,h );
 238                                 w[j] = z;
 239                                 if ( z ) {
 240                                         z = 1.0 / z;
 241                                         c = f * z;
 242                                         s = h * z;
 243                                 }
 244                                 f = ( c * g ) + ( s * y );
 245                                 x = ( c * y ) - ( s * g );
 246                                 for ( jj = 1; jj <= m; jj++ ) {
 247                                         y = a[jj][j];
 248                                         z = a[jj][i];
 249                                         a[jj][j] = y * c + z * s;
 250                                         a[jj][i] = z * c - y * s;
 251                                 }
 252                         }
 253                         rv1[l] = 0.0;
 254                         rv1[k] = f;
 255                         w[k] = x;
 256                 }
 257         }
 258         freeVect( rv1 );
 259 }
 260
 261
 262
 263 void svbksb( double** u, double* w, double** v,int m, int n, double* b, double* x ){
 264         int jj,j,i;
 265         double s,*tmp;
 266         tmp = allocVect( n );
 267         for ( j = 1; j <= n; j++ )
 268         {
 269                 s = 0.0;
 270                 if ( w[j] ) {
 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         int usedfs;
 295         int *remap;
 296         int i,j;
 297         double **da;
 298         double **v;
 299         double *w;
 300         int DOFerr;
 301         float mx;
 302         int bestat;
 303
 304         if ( nframes > framesize ) {
 305                 usedfs = nframes;
 306         }
 307         else{
 308                 usedfs = framesize;
 309         }
 310
 311         da = allocMatrix( usedfs,nframes );
 312         v = allocMatrix( nframes,nframes );
 313         w = allocVect( nframes );
 314
 315         DOFerr = 0; //false
 316         for ( i = 0; i < nframes; i++ )
 317         {
 318                 for ( j = 0; j < framesize; j++ )
 319                         da[j + 1][i + 1] = a[i * framesize + j];
 320                 for (; j < usedfs; j++ )
 321                         da[j + 1][i + 1] = 0.0;
 322         }
 323
 324         svdcmp( da,usedfs,nframes,w,v );
 325
 326         remap = calloc( sizeof( int ), (size_t)nframes );
 327
 328
 329         for ( i = 0; i < nframes; i++ )
 330                 remap[i] = -1;
 331         for ( j = 0; j < compressedsize; j++ )
 332         {
 333                 mx = -1.0f;
 334                 for ( i = 0; i < nframes; i++ )
 335                 {
 336                         if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
 337                                 mx = (float) fabs( w[i + 1] );
 338                                 bestat = i;
 339                         }
 340                 }
 341
 342                 if ( mx > 0 ) {
 343                         remap[bestat] = j;
 344                 }
 345                 else
 346                 {
 347                         DOFerr = 1; //true
 348                 }
 349         }
 350
 351         if ( DOFerr ) {
 352                 printf( "Warning:  To many degrees of freedom!  File size may increase\n" );
 353
 354                 for ( i = 0; i < compressedsize; i++ )
 355                 {
 356                         values[i] = 0;
 357                         for ( j = 0; j < framesize; j++ )
 358                                 res[i * framesize + j] = 0;
 359                 }
 360         }
 361
 362         for ( i = 0; i < nframes; i++ )
 363         {
 364                 if ( remap[i] < 0 ) {
 365                         w[i + 1] = 0.0;
 366                 }
 367                 else
 368                 {
 369                         values[remap[i]] = (float) w[i + 1];
 370                         for ( j = 0; j < framesize; j++ )
 371                                 res[remap[i] * framesize + j] = (float) da[j + 1][i + 1];
 372                 }
 373         }
 374         freeVect( w );
 375         freeMatrix( v,nframes );
 376         freeMatrix( da,framesize );
 377         free( remap );
 378 }
 379
 380 #else
 381
 382 void DOsvd( float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize ){
 383         int *remap;
 384         int i,j;
 385         int nrows;
 386         nrows = nframes;
 387         if ( nrows < framesize ) {
 388                 nrows = framesize;
 389         }
 390         double **da = allocMatrix( nrows,framesize );
 391         double **v = allocMatrix( framesize,framesize );
 392         double *w = allocVect( framesize );
 393         float mx;
 394         int bestat;
 395
 396         for ( j = 0; j < framesize; j++ )
 397         {
 398                 for ( i = 0; i < nframes; i++ )
 399                         da[j + 1][i + 1] = a[i * framesize + j];
 400                 for (; i < nrows; i++ )
 401                         da[j + 1][i + 1] = 0.0;
 402         }
 403
 404         svdcmp( da,nrows,framesize,w,v );
 405
 406         remap = new int[framesize];
 407
 408
 409         for ( i = 0; i < framesize; i++ )
 410                 remap[i] = -1;
 411         for ( j = 0; j < compressedsize; j++ )
 412         {
 413                 mx = -1.0f;
 414                 for ( i = 0; i < framesize; i++ )
 415                 {
 416                         if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
 417                                 mx = fabs( w[i + 1] );
 418                                 bestat = i;
 419                         }
 420                 }
 421                 assert( mx > -.5f );
 422                 remap[bestat] = j;
 423         }
 424         // josh **DO NOT** put your dof>nframes mod here
 425         for ( i = 0; i < framesize; i++ )
 426         {
 427                 if ( remap[i] < 0 ) {
 428                         w[i + 1] = 0.0;
 429                 }
 430                 else
 431                 {
 432                         values[remap[i]] = w[i + 1];
 433                         for ( j = 0; j < framesize; j++ )
 434                                 res[remap[i] * framesize + j] = v[j + 1][i + 1];
 435                 }
 436         }
 437         freeVect( w );
 438         freeMatrix( v,framesize );
 439         freeMatrix( da,nrows );
 440         delete[] remap;
 441 }
 442
 443 #endif
 444
 445 void DOsvdPlane( float *pnts,int npnts,float *n,float *base ){
 446         int i,j;
 447         double **da = allocMatrix( npnts,3 );
 448         double **v = allocMatrix( 3,3 );
 449         double *w = allocVect( 3 );
 450         float mn = 1E30f;
 451         int bestat;
 452
 453
 454         assert( npnts >= 3 );
 455         base[0] = pnts[0];
 456         base[1] = pnts[1];
 457         base[2] = pnts[2];
 458         for ( i = 1; i < npnts; i++ )
 459         {
 460                 for ( j = 0; j < 3; j++ )
 461                         base[j] += pnts[i * 3 + j];
 462         }
 463         base[0] /= (float)( npnts );
 464         base[1] /= (float)( npnts );
 465         base[2] /= (float)( npnts );
 466
 467         for ( i = 0; i < 3; i++ )
 468         {
 469                 for ( j = 0; j < npnts; j++ )
 470                         da[j + 1][i + 1] = pnts[j * 3 + i] - base[i];
 471         }
 472
 473         svdcmp( da,npnts,3,w,v );
 474         for ( i = 0; i < 3; i++ )
 475         {
 476                 if ( fabs( w[i + 1] ) < mn ) {
 477                         mn = (float) fabs( w[i + 1] );
 478                         bestat = i;
 479                 }
 480         }
 481         n[0] = (float) v[1][bestat + 1];
 482         n[1] = (float) v[2][bestat + 1];
 483         n[2] = (float) v[3][bestat + 1];
 484         freeVect( w );
 485         freeMatrix( v,3 );
 486         freeMatrix( da,npnts );
 487 }