2 Copyright (C) 1999-2007 id Software, Inc. and contributors.
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
5 This file is part of GtkRadiant.
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.
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.
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
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 ) )
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 ) )
37 void ntrerror( char *s ){
42 double *allocVect( int sz ){
45 ret = calloc( sizeof( double ), (size_t)sz );
49 void freeVect( double *ret ){
53 double **allocMatrix( int r,int c ){
56 ret = calloc( sizeof( double ), (size_t)( r * c ) );
60 void freeMatrix( double **ret,int r ){
64 void svdcmp( double** a, int m, int n, double* w, double** v ){
65 int flag,i,its,j,jj,k,l,nm;
67 double anorm = 0.0,g = 0.0,scale = 0.0;
72 ntrerror( "SVDCMP: You must augment A with extra zero rows" );
75 for ( i = 1; i <= n; i++ ) {
80 for ( k = i; k <= m; k++ ) scale += fabs( a[k][i] );
82 for ( k = i; k <= m; k++ ) {
84 s += a[k][i] * a[k][i];
87 g = -SIGN( sqrt( s ),f );
91 for ( j = l; j <= n; j++ ) {
92 for ( s = 0.0,k = i; k <= m; k++ ) s += a[k][i] * a[k][j];
94 for ( k = i; k <= m; k++ ) a[k][j] += f * a[k][i];
97 for ( k = i; k <= m; k++ ) a[k][i] *= scale;
102 if ( i <= m && i != n ) {
103 for ( k = l; k <= n; k++ ) scale += fabs( a[i][k] );
105 for ( k = l; k <= n; k++ ) {
107 s += a[i][k] * a[i][k];
110 g = -SIGN( sqrt( s ),f );
113 for ( k = l; k <= n; k++ ) rv1[k] = a[i][k] / h;
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];
120 for ( k = l; k <= n; k++ ) a[i][k] *= scale;
123 anorm = MAX( anorm,( fabs( w[i] ) + fabs( rv1[i] ) ) );
125 for ( i = n; i >= 1; i-- ) {
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];
135 for ( j = l; j <= n; j++ ) v[i][j] = v[j][i] = 0.0;
141 for ( i = n; i >= 1; i-- ) {
145 for ( j = l; j <= n; j++ ) a[i][j] = 0.0;
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];
156 for ( j = i; j <= m; j++ ) a[j][i] *= g;
159 for ( j = i; j <= m; j++ ) a[j][i] = 0.0;
163 for ( k = n; k >= 1; k-- ) {
164 for ( its = 1; its <= 30; its++ ) {
166 for ( l = k; l >= 1; l-- ) {
168 if ( fabs( rv1[l] ) + anorm == anorm ) {
172 if ( fabs( w[nm] ) + anorm == anorm ) {
179 for ( i = l; i <= k; i++ ) {
181 if ( fabs( f ) + anorm != anorm ) {
188 for ( j = 1; j <= m; j++ ) {
191 a[j][nm] = y * c + z * s;
192 a[j][i] = z * c - y * s;
201 for ( j = 1; j <= n; j++ ) v[j][k] = ( -v[j][k] );
206 ntrerror( "No convergence in 30 SVDCMP iterations" );
213 f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0 * h * y );
215 f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + SIGN( g,f ) ) ) - h ) ) / x;
217 for ( j = l; j <= nm; j++ ) {
231 for ( jj = 1; jj <= n; jj++ ) {
234 v[jj][j] = x * c + z * s;
235 v[jj][i] = z * c - x * s;
244 f = ( c * g ) + ( s * y );
245 x = ( c * y ) - ( s * g );
246 for ( jj = 1; jj <= m; jj++ ) {
249 a[jj][j] = y * c + z * s;
250 a[jj][i] = z * c - y * s;
263 void svbksb( double** u, double* w, double** v,int m, int n, double* b, double* x ){
266 tmp = allocVect( n );
267 for ( j = 1; j <= n; j++ )
271 for ( i = 1; i <= m; i++ )
277 for ( j = 1; j <= n; j++ )
280 for ( jj = 1; jj <= n; jj++ )
281 s += v[j][jj] * tmp[jj];
293 void DOsvd( float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize ){
304 if ( nframes > framesize ) {
311 da = allocMatrix( usedfs,nframes );
312 v = allocMatrix( nframes,nframes );
313 w = allocVect( nframes );
316 for ( i = 0; i < nframes; i++ )
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;
324 svdcmp( da,usedfs,nframes,w,v );
326 remap = calloc( sizeof( int ), (size_t)nframes );
329 for ( i = 0; i < nframes; i++ )
331 for ( j = 0; j < compressedsize; j++ )
334 for ( i = 0; i < nframes; i++ )
336 if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
337 mx = (float) fabs( w[i + 1] );
352 printf( "Warning: To many degrees of freedom! File size may increase\n" );
354 for ( i = 0; i < compressedsize; i++ )
357 for ( j = 0; j < framesize; j++ )
358 res[i * framesize + j] = 0;
362 for ( i = 0; i < nframes; i++ )
364 if ( remap[i] < 0 ) {
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];
375 freeMatrix( v,nframes );
376 freeMatrix( da,framesize );
382 void DOsvd( float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize ){
387 if ( nrows < framesize ) {
390 double **da = allocMatrix( nrows,framesize );
391 double **v = allocMatrix( framesize,framesize );
392 double *w = allocVect( framesize );
396 for ( j = 0; j < framesize; j++ )
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;
404 svdcmp( da,nrows,framesize,w,v );
406 remap = new int[framesize];
409 for ( i = 0; i < framesize; i++ )
411 for ( j = 0; j < compressedsize; j++ )
414 for ( i = 0; i < framesize; i++ )
416 if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
417 mx = fabs( w[i + 1] );
424 // josh **DO NOT** put your dof>nframes mod here
425 for ( i = 0; i < framesize; i++ )
427 if ( remap[i] < 0 ) {
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];
438 freeMatrix( v,framesize );
439 freeMatrix( da,nrows );
445 void DOsvdPlane( float *pnts,int npnts,float *n,float *base ){
447 double **da = allocMatrix( npnts,3 );
448 double **v = allocMatrix( 3,3 );
449 double *w = allocVect( 3 );
454 assert( npnts >= 3 );
458 for ( i = 1; i < npnts; i++ )
460 for ( j = 0; j < 3; j++ )
461 base[j] += pnts[i * 3 + j];
463 base[0] /= (float)( npnts );
464 base[1] /= (float)( npnts );
465 base[2] /= (float)( npnts );
467 for ( i = 0; i < 3; i++ )
469 for ( j = 0; j < npnts; j++ )
470 da[j + 1][i + 1] = pnts[j * 3 + i] - base[i];
473 svdcmp( da,npnts,3,w,v );
474 for ( i = 0; i < 3; i++ )
476 if ( fabs( w[i + 1] ) < mn ) {
477 mn = (float) fabs( w[i + 1] );
481 n[0] = (float) v[1][bestat + 1];
482 n[1] = (float) v[2][bestat + 1];
483 n[2] = (float) v[3][bestat + 1];
486 freeMatrix( da,npnts );