MRI *
MRIScomputeDistanceMap(MRI_SURFACE *mris, MRI *mri_distance, int ref_vertex_no)
{
int vno ;
VERTEX *v ;
double circumference, angle, distance ;
VECTOR *v1, *v2 ;
if (mri_distance == NULL)
mri_distance = MRIalloc(mris->nvertices, 1, 1, MRI_FLOAT) ;
v1 = VectorAlloc(3, MATRIX_REAL) ; v2 = VectorAlloc(3, MATRIX_REAL) ;
v = &mris->vertices[ref_vertex_no] ;
VECTOR_LOAD(v1, v->x, v->y, v->z) ; /* radius vector */
circumference = M_PI * 2.0 * V3_LEN(v1) ;
for (vno = 0 ; vno < mris->nvertices ; vno++)
{
v = &mris->vertices[vno] ;
if (vno == Gdiag_no)
DiagBreak() ;
VECTOR_LOAD(v2, v->x, v->y, v->z) ; /* radius vector */
angle = fabs(Vector3Angle(v1, v2)) ;
distance = circumference * angle / (2.0 * M_PI) ;
MRIsetVoxVal(mri_distance, vno, 0, 0, 0, distance) ;
}
VectorFree(&v1) ; VectorFree(&v2) ;
return(mri_distance) ;
}
int BSDE_SolidHull_ContactGeneric(BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
bsde_real *org, bsde_real *dir, bsde_real *dist)
{
bsde_real norm[4], bn[4], bo[3];
bsde_real bd;
bsde_real m, n, o, p, d, f;
int i;
#if 0
if(bobj->solidtype==BSDE_SOLID_SPHERE)
{
BSDE_SolidHull_NearestPoint(aobj, axform, borg, bo);
if(V3_DIST(borg, bo)<0.001)V3_COPY(aorg, bo);
// V3_SUB(borg, bo, dir);
V3_SUB(bo, borg, dir);
V3_NORMALIZE(dir, dir);
// dir[3]=V3_DOT(bo, dir);
BSDE_PlaneExtents(aobj, dir, &m, &n);
BSDE_PlaneExtents(bobj, dir, &o, &p);
m=(m>o)?m:o; n=(n<p)?n:p;
d=V3_DOT(borg, dir);
V3_ADDSCALE(borg, dir, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
#if 0
V3_SUB(bo, borg, bn);
d=bobj->radius/V3_LEN(bn);
// V3_SCALE(bn, d, bn);
// V3_ADD(borg, bn, org);
V3_ADDSCALE(borg, bn, d, org);
BSDE_PlaneExtents(aobj, dir, &m, &n);
*dist=V3_DOT(org, dir)-n;
return(1);
#endif
}
#endif
if(bobj->solidtype==BSDE_SOLID_SPHERE)
{
i=BSDE_SolidHull_ContactSphere(aobj, bobj,
axform, bxform, aorg, borg,
org, dir, dist);
return(i);
}
if((bobj->solidtype==BSDE_SOLID_AABB) ||
(bobj->solidtype==BSDE_SOLID_OBB))
{
i=BSDE_SolidHull_ContactBox(aobj, bobj,
axform, bxform, aorg, borg,
org, dir, dist);
return(i);
}
V3_SUB(aorg, borg, bn);
V3_NORMALIZE(bn, dir);
BSDE_PlaneExtents(aobj, dir, &m, &n);
BSDE_PlaneExtents(bobj, dir, &o, &p);
m=(m>o)?m:o; n=(n<p)?n:p;
d=V3_DOT(borg, dir);
V3_ADDSCALE(borg, dir, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
}
void BSDE_SolidHull_MakeHullFaces(BSDE_Solid *obj)
{
static bsde_real vecbuf[64*64*3];
static bsde_real pointbuf[64*3], pointbuf2[64*3];
static int facebuf[64*64];
static int facevbuf[64];
int i, j, k, l, n, nf, nv;
int p0, p1, p2;
bsde_real *v0, *v1, *v2;
bsde_real dv0[3], dv1[3], dv2[3];
bsde_real f, g, h;
n=0;
nv=0;
nf=0;
// printf("build");
for(i=0; i<obj->n_faces; i++)
{
BSDE_SolidHull_MakePlaneFace(obj->norms+(i*4), pointbuf);
l=4;
for(j=0; j<obj->n_faces; j++)
{
if(i==j)continue;
l=BSDE_SolidHull_ClipFace(obj->norms+(j*4),
pointbuf, pointbuf2, l);
for(k=0; k<(l*3); k++)pointbuf[k]=pointbuf2[k];
}
if(!l)continue;
// printf("%d:%d", i, l);
for(j=0; j<l; j++)
{
for(k=0; k<nv; k++)
if(V3_DIST(pointbuf+j*3,
vecbuf+k*3)<0.00001)
break;
if(k==nv)
// if(1)
{
V3_COPY(pointbuf+j*3, vecbuf+k*3);
nv++;
}
facebuf[n++]=k;
}
facevbuf[nf]=l;
nf++;
}
// printf("\n");
// printf("facevecs");
// for(i=0; i<nf; i++)printf(" %d:%d", i, facevbuf[i]);
// printf("\n");
if(nf<obj->n_faces)
{
printf("MakeHullFaces: %d faces clipped away\n",
obj->faces-nf);
}
// obj->n_faces=nf;
obj->n_vecs=nv;
obj->vecs=(bsde_real *)malloc(nv*3*sizeof(bsde_real));
memcpy(obj->vecs, vecbuf, nv*3*sizeof(bsde_real));
obj->faces=(int *)malloc(n*sizeof(int));
memcpy(obj->faces, facebuf, n*sizeof(int));
obj->facevecs=(int *)malloc(nf*sizeof(int));
memcpy(obj->facevecs, facevbuf, nf*sizeof(int));
k=0;
h=0;
for(i=0; i<nf; i++)
{
l=facevbuf[i];
for(j=1; j<(l-1); j++)
{
p0=facebuf[k];
p1=facebuf[k+j];
p2=facebuf[k+j+1];
if((p0==p1) || (p1==p2) || (p2==p0))continue;
v0=vecbuf+p0*3;
v1=vecbuf+p1*3;
v2=vecbuf+p2*3;
V3_SUB(v1, v0, dv0);
V3_SUB(v2, v0, dv1);
V3_CROSS(dv0, dv1, dv2);
f=V3_NORMALIZE(dv2, dv2);
g=f*0.5;
V3_CROSS(dv0, v0, dv2);
f=V3_NORMALIZE(dv2, dv2);
g*=f*0.5;
V3_CROSS(dv1, v0, dv2);
f=V3_NORMALIZE(dv2, dv2);
g*=f*0.5;
h+=g;
}
k+=l;
}
obj->volume=h;
V3_SET(obj->mins, 999999, 999999, 999999);
V3_SET(obj->maxs, -999999, -999999, -999999);
for(i=0; i<nv; i++)
{
if(vecbuf[i*3+0]<obj->mins[0])obj->mins[0]=vecbuf[i*3+0];
if(vecbuf[i*3+0]>obj->maxs[0])obj->maxs[0]=vecbuf[i*3+0];
if(vecbuf[i*3+1]<obj->mins[1])obj->mins[1]=vecbuf[i*3+1];
if(vecbuf[i*3+1]>obj->maxs[1])obj->maxs[1]=vecbuf[i*3+1];
if(vecbuf[i*3+2]<obj->mins[2])obj->mins[2]=vecbuf[i*3+2];
if(vecbuf[i*3+2]>obj->maxs[2])obj->maxs[2]=vecbuf[i*3+2];
}
f=0;
for(i=0; i<nv; i++)
{ g=V3_LEN(vecbuf+i*3); if(g>f)f=g; }
obj->radius=f;
obj->stateflags&=~BSDE_STATEFL_BBOX;
if(obj->n_faces==6)
{
j=0;
for(i=0; i<obj->n_faces; i++)
{
v0=obj->norms+i*4;
if((v0[0]==1) && (v0[1]==0) && (v0[2]==0))j|=1;
if((v0[0]==-1) && (v0[1]==0) && (v0[2]==0))j|=2;
if((v0[0]==0) && (v0[1]==1) && (v0[2]==0))j|=4;
if((v0[0]==0) && (v0[1]==-1) && (v0[2]==0))j|=8;
if((v0[0]==0) && (v0[1]==0) && (v0[2]==1))j|=16;
if((v0[0]==0) && (v0[1]==0) && (v0[2]==-1))j|=32;
}
if(j==63)obj->stateflags|=BSDE_STATEFL_BBOX;
}
}
void BSDE_UpdateVel(BSDE_World *world, BSDE_Solid *cur, bsde_real dt)
{
bsde_real f;
#if 1
if(!bsde_finite(V3_DOT(cur->ivel, cur->ivel)))
{ V3_ZERO(cur->ivel); }
if(!bsde_finite(V3_DOT(cur->itvel, cur->itvel)))
{ V3_ZERO(cur->itvel); }
#endif
#if 0
if(V3_LEN(cur->ivel)>10)
{
V3_SCALE(cur->ivel, 10/V3_LEN(cur->ivel), cur->ivel);
}
if((V3_LEN(cur->itvel)/cur->mass)>10)
{
V3_SCALE(cur->itvel, 10/(V3_LEN(cur->itvel)/cur->mass),
cur->itvel);
}
#endif
V3_ADD(cur->vel, cur->ivel, cur->vel);
V3_ADD(cur->tvel, cur->itvel, cur->tvel);
V3_ZERO(cur->ivel);
V3_ZERO(cur->itvel);
if(V3_LEN(cur->vel)<0.01) { V3_ZERO(cur->vel); }
if((V3_LEN(cur->tvel)/cur->mass)<0.01) { V3_ZERO(cur->tvel); }
#if 1
if(!bsde_finite(V3_DOT(cur->vel, cur->vel)))
{ V3_ZERO(cur->vel); }
if(!bsde_finite(V3_DOT(cur->tvel, cur->tvel)))
{ V3_ZERO(cur->tvel); }
#endif
#if 0
if(V3_LEN(cur->vel)>100)
{
V3_SCALE(cur->vel, 100/V3_LEN(cur->vel), cur->vel);
}
if((V3_LEN(cur->tvel)/cur->mass)>10)
{
V3_SCALE(cur->tvel, 10/(V3_LEN(cur->vel)/cur->mass),
cur->tvel);
}
#endif
if((cur->solidtype==BSDE_SOLID_AABB) ||
(cur->moveflags&BSDE_MOVEFL_NOSPIN))
{
BSDE_Quat_Identity(cur->rot);
V3_ZERO(cur->tvel);
}
if(cur->moveflags&BSDE_MOVEFL_NOMOVE)
{
V3_ZERO(cur->vel);
}
// f=V3_DIST(cur->e_org, cur->org)+V4_DIST(cur->e_rot, cur->rot);
f=V3_LEN(cur->vel)+V3_LEN(cur->avel);
if(f>=(0.25*dt))
{
cur->idletime=0;
cur->moveflags&=~BSDE_MOVEFL_IDLE;
}
}
static int
update_histograms(MRI_SURFACE *mris, MRI_SURFACE *mris_avg, float ***histograms, int nbins) {
int vno, vno2, vno_avg ;
double volume_dist, surface_dist, circumference, angle ;
VERTEX *v1, *v2 ;
VECTOR *vec1, *vec2 ;
MHT *mht ;
float **histogram, min_dist ;
mht = MHTfillVertexTableRes(mris_avg, NULL, CURRENT_VERTICES, 2.0) ;
vec1 = VectorAlloc(3, MATRIX_REAL) ;
vec2 = VectorAlloc(3, MATRIX_REAL) ;
v1 = &mris->vertices[0] ;
VECTOR_LOAD(vec1, v1->cx, v1->cy, v1->cz) ; /* radius vector */
circumference = M_PI * 2.0 * V3_LEN(vec1) ;
MRISclearMarks(mris_avg) ;
#if 0
for (vno = 0 ; vno < mris->nvertices ; vno++) {
if ((vno % 1000) == 0) {
printf("\r%d of %d ", vno, mris->nvertices) ;
fflush(stdout) ;
}
v1 = &mris->vertices[vno] ;
VECTOR_LOAD(vec1, v1->cx, v1->cy, v1->cz) ; /* radius vector */
vno_avg = MHTfindClosestVertexNo(mht, mris_avg, v1, &min_dist) ; /* which histogram to increment */
if (vno_avg < 0)
continue ;
if (vno_avg == Gdiag_no)
DiagBreak() ;
histogram = histograms[vno_avg] ;
mris_avg->vertices[vno_avg].marked = 1 ;
for (vno2 = 0 ; vno2 < mris->nvertices ; vno2++) {
if (vno2 == vno)
continue ;
v2 = &mris->vertices[vno2] ;
VECTOR_LOAD(vec2, v2->cx, v2->cy, v2->cz) ; /* radius vector */
volume_dist = sqrt(SQR(v1->origx-v2->origx)+SQR(v1->origy-v2->origy)+SQR(v1->origz-v2->origz)) ;
if (nint(volume_dist) >= nbins || nint(volume_dist) < 0)
continue ;
angle = fabs(Vector3Angle(vec1, vec2)) ;
surface_dist = circumference * angle / (2.0 * M_PI) ;
if (surface_dist > nbins*MAX_SURFACE_SCALE)
surface_dist = nbins*MAX_SURFACE_SCALE ;
if (surface_dist < 1)
surface_dist = 1 ;
histogram[nint(volume_dist)][nint(surface_dist)]++ ;
if (mht->buckets[0][0] != NULL)
DiagBreak() ;
}
}
MHTfree(&mht) ;
#endif
/* map back ones that were missed */
/* printf("\nfilling holes in mapping\n") ;*/
mht = MHTfillVertexTableRes(mris, NULL, CURRENT_VERTICES, 2.0) ;
for (vno_avg = 0 ; vno_avg < mris_avg->nvertices ; vno_avg++) {
if (mris_avg->vertices[vno_avg].marked > 0)
continue ;
if ((vno_avg % 1000) == 0) {
printf("\r%d of %d ", vno_avg, mris_avg->nvertices) ;
fflush(stdout) ;
}
vno = MHTfindClosestVertexNo(mht, mris, &mris_avg->vertices[vno_avg], &min_dist) ;
if (vno < 0)
continue ;
v1 = &mris->vertices[vno] ;
VECTOR_LOAD(vec1, v1->cx, v1->cy, v1->cz) ; /* radius vector */
if (vno_avg < 0)
continue ;
if (vno_avg == Gdiag_no)
DiagBreak() ;
histogram = histograms[vno_avg] ;
mris_avg->vertices[vno_avg].marked = 1 ;
for (vno2 = 0 ; vno2 < mris->nvertices ; vno2++) {
if (vno2 == vno)
continue ;
v2 = &mris->vertices[vno2] ;
VECTOR_LOAD(vec2, v2->cx, v2->cy, v2->cz) ; /* radius vector */
volume_dist = sqrt(SQR(v1->origx-v2->origx)+SQR(v1->origy-v2->origy)+SQR(v1->origz-v2->origz)) ;
if (nint(volume_dist) >= nbins || nint(volume_dist) < 0)
continue ;
angle = fabs(Vector3Angle(vec1, vec2)) ;
surface_dist = circumference * angle / (2.0 * M_PI) ;
if (surface_dist > nbins*MAX_SURFACE_SCALE)
surface_dist = nbins*MAX_SURFACE_SCALE ;
if (surface_dist < 1)
surface_dist = 1 ;
histogram[nint(volume_dist)][nint(surface_dist)]++ ;
}
}
MHTfree(&mht) ;
printf("\n") ;
VectorFree(&vec1) ;
VectorFree(&vec2) ;
return(NO_ERROR) ;
}
