void freeFunc(Func* funcPtr)
{
freePtr(funcPtr->name);
freePtr(funcPtr->parTypes);
freeVect(funcPtr->data);
freeVect(funcPtr->waitList);
}
void freeModel(_md)
{
size_t predNum, funcNum, axNum, taskNum,
factNum, restrNum, typeNum, srcNum, errNum;
freeServData(modelPtr);
/* Predicates */
forVect(predNum, preds_m)
freeFunc(getPredPtr(predNum));
/* Functions */
forVect(funcNum, funcs_m)
freeFunc(getFuncPtr(funcNum));
/* Axioms */
forVect(axNum, axioms_m)
freeAxiom(getAxPtr(axNum));
/* Tasks */
forVect(taskNum, taskPtrs_m)
freeTask(getTaskPtr(taskNum));
/* Temp Facts */
forVect(factNum, tmpFacts_m)
freeFact(ptr(Fact, tmpFacts_m) + factNum);
/* Restrictions */
forVect(restrNum, restrs_m)
freeRestriction(getRestrPtr(restrNum));
/* DataTypes */
forVect(typeNum, types_m)
freeDataType(getTypePtr(typeNum));
/* Sources */
forVect(srcNum, modelPtr->sources)
freeSource(ptr(Source, modelPtr->sources) + srcNum);
/* Errors */
forVect(errNum, modelPtr->errors)
freeError(ptr(Error, modelPtr->errors) + errNum);
/* Vectors */
freeVect(preds_m);
freeVect(funcs_m);
freeVect(axioms_m);
freeVect(taskPtrs_m);
freeVect(tmpFacts_m)
freeVect(restrs_m);
freeVect(types_m);
freeVect(modelPtr->errors);
freeVect(modelPtr->sources);
}
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;
}
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);
}
void freeAxiom(Axiom* axiomPtr)
{
size_t itNum;
if (axiomPtr->items != NULL)
for (itNum = 0; itNum < axiomPtr->itemCt; ++itNum)
{
freeExprNode(axiomPtr->items[itNum]);
}
freePtr(axiomPtr->items);
freeVect(axiomPtr->vars);
}
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);
}
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 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);
}
void freeRestriction(Restriction* restrPtr)
{
freeExprNode(restrPtr->root);
restrPtr->root = NULL;
freeVect(restrPtr->vars);
}
void freeDataType(DataType* typePtr)
{
freePtr(typePtr->name);
freeVect(typePtr->constNames);
}
void freeTask(Task* taskPtr)
{
freeVect(taskPtr->tmpFacts);
freeExprResult(taskPtr->resultPtr);
freePtr(taskPtr);
}
