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
ntrerror(char * s)37 void ntrerror( char *s ){
38 printf( "%s\n",s );
39 exit( 1 );
40 }
41
allocVect(int sz)42 double *allocVect( int sz ){
43 double *ret;
44
45 ret = calloc( sizeof( double ), (size_t)sz );
46 return ret;
47 }
48
freeVect(double * ret)49 void freeVect( double *ret ){
50 free( ret );
51 }
52
allocMatrix(int r,int c)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
freeMatrix(double ** ret,int r)60 void freeMatrix( double **ret,int r ){
61 free( ret );
62 }
63
svdcmp(double ** a,int m,int n,double * w,double ** v)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
svbksb(double ** u,double * w,double ** v,int m,int n,double * b,double * x)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
DOsvd(float * a,float * res,float * comp,float * values,int nframes,int framesize,int compressedsize)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
DOsvd(float * a,float * res,float * comp,float * values,int nframes,int framesize,int compressedsize)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
DOsvdPlane(float * pnts,int npnts,float * n,float * base)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 }
488