mirror of
https://github.com/UberGames/GtkRadiant.git
synced 2024-12-11 04:51:10 +00:00
487 lines
10 KiB
C
487 lines
10 KiB
C
/*
|
|
Copyright (C) 1999-2007 id Software, Inc. and contributors.
|
|
For a list of contributors, see the accompanying CONTRIBUTORS file.
|
|
|
|
This file is part of GtkRadiant.
|
|
|
|
GtkRadiant is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
GtkRadiant is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with GtkRadiant; if not, write to the Free Software
|
|
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
static double at,bt,ct;
|
|
#define PYTHAG( a,b ) ( ( at = fabs( a ) ) > ( bt = fabs( b ) ) ? \
|
|
( ct = bt / at,at * sqrt( 1.0 + ct * ct ) ) : ( bt ? ( ct = at / bt,bt * sqrt( 1.0 + ct * ct ) ) : 0.0 ) )
|
|
|
|
static double maxarg1,maxarg2;
|
|
#define MAX( a,b ) ( maxarg1 = ( a ),maxarg2 = ( b ),( maxarg1 ) > ( maxarg2 ) ? \
|
|
( maxarg1 ) : ( maxarg2 ) )
|
|
#define SIGN( a,b ) ( ( b ) >= 0.0 ? fabs( a ) : -fabs( a ) )
|
|
|
|
void ntrerror( char *s ){
|
|
printf( "%s\n",s );
|
|
exit( 1 );
|
|
}
|
|
|
|
double *allocVect( int sz ){
|
|
double *ret;
|
|
|
|
ret = calloc( sizeof( double ), (size_t)sz );
|
|
return ret;
|
|
}
|
|
|
|
void freeVect( double *ret ){
|
|
free( ret );
|
|
}
|
|
|
|
double **allocMatrix( int r,int c ){
|
|
double **ret;
|
|
|
|
ret = calloc( sizeof( double ), (size_t)( r * c ) );
|
|
return ret;
|
|
}
|
|
|
|
void freeMatrix( double **ret,int r ){
|
|
free( ret );
|
|
}
|
|
|
|
void svdcmp( double** a, int m, int n, double* w, double** v ){
|
|
int flag,i,its,j,jj,k,l,nm;
|
|
double c,f,h,s,x,y,z;
|
|
double anorm = 0.0,g = 0.0,scale = 0.0;
|
|
double *rv1;
|
|
void nrerror();
|
|
|
|
if ( m < n ) {
|
|
ntrerror( "SVDCMP: You must augment A with extra zero rows" );
|
|
}
|
|
rv1 = allocVect( n );
|
|
for ( i = 1; i <= n; i++ ) {
|
|
l = i + 1;
|
|
rv1[i] = scale * g;
|
|
g = s = scale = 0.0;
|
|
if ( i <= m ) {
|
|
for ( k = i; k <= m; k++ ) scale += fabs( a[k][i] );
|
|
if ( scale ) {
|
|
for ( k = i; k <= m; k++ ) {
|
|
a[k][i] /= scale;
|
|
s += a[k][i] * a[k][i];
|
|
}
|
|
f = a[i][i];
|
|
g = -SIGN( sqrt( s ),f );
|
|
h = f * g - s;
|
|
a[i][i] = f - g;
|
|
if ( i != n ) {
|
|
for ( j = l; j <= n; j++ ) {
|
|
for ( s = 0.0,k = i; k <= m; k++ ) s += a[k][i] * a[k][j];
|
|
f = s / h;
|
|
for ( k = i; k <= m; k++ ) a[k][j] += f * a[k][i];
|
|
}
|
|
}
|
|
for ( k = i; k <= m; k++ ) a[k][i] *= scale;
|
|
}
|
|
}
|
|
w[i] = scale * g;
|
|
g = s = scale = 0.0;
|
|
if ( i <= m && i != n ) {
|
|
for ( k = l; k <= n; k++ ) scale += fabs( a[i][k] );
|
|
if ( scale ) {
|
|
for ( k = l; k <= n; k++ ) {
|
|
a[i][k] /= scale;
|
|
s += a[i][k] * a[i][k];
|
|
}
|
|
f = a[i][l];
|
|
g = -SIGN( sqrt( s ),f );
|
|
h = f * g - s;
|
|
a[i][l] = f - g;
|
|
for ( k = l; k <= n; k++ ) rv1[k] = a[i][k] / h;
|
|
if ( i != m ) {
|
|
for ( j = l; j <= m; j++ ) {
|
|
for ( s = 0.0,k = l; k <= n; k++ ) s += a[j][k] * a[i][k];
|
|
for ( k = l; k <= n; k++ ) a[j][k] += s * rv1[k];
|
|
}
|
|
}
|
|
for ( k = l; k <= n; k++ ) a[i][k] *= scale;
|
|
}
|
|
}
|
|
anorm = MAX( anorm,( fabs( w[i] ) + fabs( rv1[i] ) ) );
|
|
}
|
|
for ( i = n; i >= 1; i-- ) {
|
|
if ( i < n ) {
|
|
if ( g ) {
|
|
for ( j = l; j <= n; j++ )
|
|
v[j][i] = ( a[i][j] / a[i][l] ) / g;
|
|
for ( j = l; j <= n; j++ ) {
|
|
for ( s = 0.0,k = l; k <= n; k++ ) s += a[i][k] * v[k][j];
|
|
for ( k = l; k <= n; k++ ) v[k][j] += s * v[k][i];
|
|
}
|
|
}
|
|
for ( j = l; j <= n; j++ ) v[i][j] = v[j][i] = 0.0;
|
|
}
|
|
v[i][i] = 1.0;
|
|
g = rv1[i];
|
|
l = i;
|
|
}
|
|
for ( i = n; i >= 1; i-- ) {
|
|
l = i + 1;
|
|
g = w[i];
|
|
if ( i < n ) {
|
|
for ( j = l; j <= n; j++ ) a[i][j] = 0.0;
|
|
}
|
|
if ( g ) {
|
|
g = 1.0 / g;
|
|
if ( i != n ) {
|
|
for ( j = l; j <= n; j++ ) {
|
|
for ( s = 0.0,k = l; k <= m; k++ ) s += a[k][i] * a[k][j];
|
|
f = ( s / a[i][i] ) * g;
|
|
for ( k = i; k <= m; k++ ) a[k][j] += f * a[k][i];
|
|
}
|
|
}
|
|
for ( j = i; j <= m; j++ ) a[j][i] *= g;
|
|
}
|
|
else {
|
|
for ( j = i; j <= m; j++ ) a[j][i] = 0.0;
|
|
}
|
|
++a[i][i];
|
|
}
|
|
for ( k = n; k >= 1; k-- ) {
|
|
for ( its = 1; its <= 30; its++ ) {
|
|
flag = 1;
|
|
for ( l = k; l >= 1; l-- ) {
|
|
nm = l - 1;
|
|
if ( fabs( rv1[l] ) + anorm == anorm ) {
|
|
flag = 0;
|
|
break;
|
|
}
|
|
if ( fabs( w[nm] ) + anorm == anorm ) {
|
|
break;
|
|
}
|
|
}
|
|
if ( flag ) {
|
|
c = 0.0;
|
|
s = 1.0;
|
|
for ( i = l; i <= k; i++ ) {
|
|
f = s * rv1[i];
|
|
if ( fabs( f ) + anorm != anorm ) {
|
|
g = w[i];
|
|
h = PYTHAG( f,g );
|
|
w[i] = h;
|
|
h = 1.0 / h;
|
|
c = g * h;
|
|
s = ( -f * h );
|
|
for ( j = 1; j <= m; j++ ) {
|
|
y = a[j][nm];
|
|
z = a[j][i];
|
|
a[j][nm] = y * c + z * s;
|
|
a[j][i] = z * c - y * s;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
z = w[k];
|
|
if ( l == k ) {
|
|
if ( z < 0.0 ) {
|
|
w[k] = -z;
|
|
for ( j = 1; j <= n; j++ ) v[j][k] = ( -v[j][k] );
|
|
}
|
|
break;
|
|
}
|
|
if ( its == 30 ) {
|
|
ntrerror( "No convergence in 30 SVDCMP iterations" );
|
|
}
|
|
x = w[l];
|
|
nm = k - 1;
|
|
y = w[nm];
|
|
g = rv1[nm];
|
|
h = rv1[k];
|
|
f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0 * h * y );
|
|
g = PYTHAG( f,1.0 );
|
|
f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + SIGN( g,f ) ) ) - h ) ) / x;
|
|
c = s = 1.0;
|
|
for ( j = l; j <= nm; j++ ) {
|
|
i = j + 1;
|
|
g = rv1[i];
|
|
y = w[i];
|
|
h = s * g;
|
|
g = c * g;
|
|
z = PYTHAG( f,h );
|
|
rv1[j] = z;
|
|
c = f / z;
|
|
s = h / z;
|
|
f = x * c + g * s;
|
|
g = g * c - x * s;
|
|
h = y * s;
|
|
y = y * c;
|
|
for ( jj = 1; jj <= n; jj++ ) {
|
|
x = v[jj][j];
|
|
z = v[jj][i];
|
|
v[jj][j] = x * c + z * s;
|
|
v[jj][i] = z * c - x * s;
|
|
}
|
|
z = PYTHAG( f,h );
|
|
w[j] = z;
|
|
if ( z ) {
|
|
z = 1.0 / z;
|
|
c = f * z;
|
|
s = h * z;
|
|
}
|
|
f = ( c * g ) + ( s * y );
|
|
x = ( c * y ) - ( s * g );
|
|
for ( jj = 1; jj <= m; jj++ ) {
|
|
y = a[jj][j];
|
|
z = a[jj][i];
|
|
a[jj][j] = y * c + z * s;
|
|
a[jj][i] = z * c - y * s;
|
|
}
|
|
}
|
|
rv1[l] = 0.0;
|
|
rv1[k] = f;
|
|
w[k] = x;
|
|
}
|
|
}
|
|
freeVect( rv1 );
|
|
}
|
|
|
|
|
|
|
|
void svbksb( double** u, double* w, double** v,int m, int n, double* b, double* x ){
|
|
int jj,j,i;
|
|
double s,*tmp;
|
|
tmp = allocVect( n );
|
|
for ( j = 1; j <= n; j++ )
|
|
{
|
|
s = 0.0;
|
|
if ( w[j] ) {
|
|
for ( i = 1; i <= m; i++ )
|
|
s += u[i][j] * b[i];
|
|
s /= w[j];
|
|
}
|
|
tmp[j] = s;
|
|
}
|
|
for ( j = 1; j <= n; j++ )
|
|
{
|
|
s = 0.0;
|
|
for ( jj = 1; jj <= n; jj++ )
|
|
s += v[j][jj] * tmp[jj];
|
|
x[j] = s;
|
|
}
|
|
freeVect( tmp );
|
|
}
|
|
|
|
#undef SIGN
|
|
#undef MAX
|
|
#undef PYTHAG
|
|
|
|
|
|
#if 1
|
|
void DOsvd( float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize ){
|
|
int usedfs;
|
|
int *remap;
|
|
int i,j;
|
|
double **da;
|
|
double **v;
|
|
double *w;
|
|
int DOFerr;
|
|
float mx;
|
|
int bestat;
|
|
|
|
if ( nframes > framesize ) {
|
|
usedfs = nframes;
|
|
}
|
|
else{
|
|
usedfs = framesize;
|
|
}
|
|
|
|
da = allocMatrix( usedfs,nframes );
|
|
v = allocMatrix( nframes,nframes );
|
|
w = allocVect( nframes );
|
|
|
|
DOFerr = 0; //false
|
|
for ( i = 0; i < nframes; i++ )
|
|
{
|
|
for ( j = 0; j < framesize; j++ )
|
|
da[j + 1][i + 1] = a[i * framesize + j];
|
|
for (; j < usedfs; j++ )
|
|
da[j + 1][i + 1] = 0.0;
|
|
}
|
|
|
|
svdcmp( da,usedfs,nframes,w,v );
|
|
|
|
remap = calloc( sizeof( int ), (size_t)nframes );
|
|
|
|
|
|
for ( i = 0; i < nframes; i++ )
|
|
remap[i] = -1;
|
|
for ( j = 0; j < compressedsize; j++ )
|
|
{
|
|
mx = -1.0f;
|
|
for ( i = 0; i < nframes; i++ )
|
|
{
|
|
if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
|
|
mx = (float) fabs( w[i + 1] );
|
|
bestat = i;
|
|
}
|
|
}
|
|
|
|
if ( mx > 0 ) {
|
|
remap[bestat] = j;
|
|
}
|
|
else
|
|
{
|
|
DOFerr = 1; //true
|
|
}
|
|
}
|
|
|
|
if ( DOFerr ) {
|
|
printf( "Warning: To many degrees of freedom! File size may increase\n" );
|
|
|
|
for ( i = 0; i < compressedsize; i++ )
|
|
{
|
|
values[i] = 0;
|
|
for ( j = 0; j < framesize; j++ )
|
|
res[i * framesize + j] = 0;
|
|
}
|
|
}
|
|
|
|
for ( i = 0; i < nframes; i++ )
|
|
{
|
|
if ( remap[i] < 0 ) {
|
|
w[i + 1] = 0.0;
|
|
}
|
|
else
|
|
{
|
|
values[remap[i]] = (float) w[i + 1];
|
|
for ( j = 0; j < framesize; j++ )
|
|
res[remap[i] * framesize + j] = (float) da[j + 1][i + 1];
|
|
}
|
|
}
|
|
freeVect( w );
|
|
freeMatrix( v,nframes );
|
|
freeMatrix( da,framesize );
|
|
free( remap );
|
|
}
|
|
|
|
#else
|
|
|
|
void DOsvd( float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize ){
|
|
int *remap;
|
|
int i,j;
|
|
int nrows;
|
|
nrows = nframes;
|
|
if ( nrows < framesize ) {
|
|
nrows = framesize;
|
|
}
|
|
double **da = allocMatrix( nrows,framesize );
|
|
double **v = allocMatrix( framesize,framesize );
|
|
double *w = allocVect( framesize );
|
|
float mx;
|
|
int bestat;
|
|
|
|
for ( j = 0; j < framesize; j++ )
|
|
{
|
|
for ( i = 0; i < nframes; i++ )
|
|
da[j + 1][i + 1] = a[i * framesize + j];
|
|
for (; i < nrows; i++ )
|
|
da[j + 1][i + 1] = 0.0;
|
|
}
|
|
|
|
svdcmp( da,nrows,framesize,w,v );
|
|
|
|
remap = new int[framesize];
|
|
|
|
|
|
for ( i = 0; i < framesize; i++ )
|
|
remap[i] = -1;
|
|
for ( j = 0; j < compressedsize; j++ )
|
|
{
|
|
mx = -1.0f;
|
|
for ( i = 0; i < framesize; i++ )
|
|
{
|
|
if ( remap[i] < 0 && fabs( w[i + 1] ) > mx ) {
|
|
mx = fabs( w[i + 1] );
|
|
bestat = i;
|
|
}
|
|
}
|
|
assert( mx > -.5f );
|
|
remap[bestat] = j;
|
|
}
|
|
// josh **DO NOT** put your dof>nframes mod here
|
|
for ( i = 0; i < framesize; i++ )
|
|
{
|
|
if ( remap[i] < 0 ) {
|
|
w[i + 1] = 0.0;
|
|
}
|
|
else
|
|
{
|
|
values[remap[i]] = w[i + 1];
|
|
for ( j = 0; j < framesize; j++ )
|
|
res[remap[i] * framesize + j] = v[j + 1][i + 1];
|
|
}
|
|
}
|
|
freeVect( w );
|
|
freeMatrix( v,framesize );
|
|
freeMatrix( da,nrows );
|
|
delete[] remap;
|
|
}
|
|
|
|
#endif
|
|
|
|
void DOsvdPlane( float *pnts,int npnts,float *n,float *base ){
|
|
int i,j;
|
|
double **da = allocMatrix( npnts,3 );
|
|
double **v = allocMatrix( 3,3 );
|
|
double *w = allocVect( 3 );
|
|
float mn = 1E30f;
|
|
int bestat;
|
|
|
|
|
|
assert( npnts >= 3 );
|
|
base[0] = pnts[0];
|
|
base[1] = pnts[1];
|
|
base[2] = pnts[2];
|
|
for ( i = 1; i < npnts; i++ )
|
|
{
|
|
for ( j = 0; j < 3; j++ )
|
|
base[j] += pnts[i * 3 + j];
|
|
}
|
|
base[0] /= (float)( npnts );
|
|
base[1] /= (float)( npnts );
|
|
base[2] /= (float)( npnts );
|
|
|
|
for ( i = 0; i < 3; i++ )
|
|
{
|
|
for ( j = 0; j < npnts; j++ )
|
|
da[j + 1][i + 1] = pnts[j * 3 + i] - base[i];
|
|
}
|
|
|
|
svdcmp( da,npnts,3,w,v );
|
|
for ( i = 0; i < 3; i++ )
|
|
{
|
|
if ( fabs( w[i + 1] ) < mn ) {
|
|
mn = (float) fabs( w[i + 1] );
|
|
bestat = i;
|
|
}
|
|
}
|
|
n[0] = (float) v[1][bestat + 1];
|
|
n[1] = (float) v[2][bestat + 1];
|
|
n[2] = (float) v[3][bestat + 1];
|
|
freeVect( w );
|
|
freeMatrix( v,3 );
|
|
freeMatrix( da,npnts );
|
|
}
|