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