int BSDE_SolidSphere_CollideContact(
bsde_real arad, bsde_real *aorg, bsde_real brad, bsde_real *borg,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
bsde_real dv[3];
bsde_real l, f;
V3_SUB(aorg, borg, dv);
l=V3_NORMALIZE(dv, dv);
if(l>=(arad+brad))return(0);
// l=V3_DOT(dv, dv);
// f=arad*arad+brad*brad;
// if(l>=f)return(0);
l=V3_NORMALIZE(dv, dv);
if(org)
{
f=(brad+(l-arad))*0.5;
if(f<0)f=0;
V3_ADDSCALE(borg, dv, f, org);
V3_COPY(dv, norm);
*dist=(arad+brad)-l;
}
return(1);
}
void BSDE_Mat3_Normalize(bsde_real *rot)
{
V3_NORMALIZE(rot+0, rot+0);
V3_ADDSCALE(rot+3, rot+0, -V3_DOT(rot+3, rot+0), rot+3);
V3_NORMALIZE(rot+3, rot+3);
V3_CROSS(rot+0, rot+3, rot+6);
}
int BSDE_SolidSphere_CollideObbContact(
bsde_real arad, bsde_real *aorg,
bsde_real *bmins, bsde_real *bmaxs, bsde_real *bpos,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
bsde_real pt[3], dv[3];
bsde_real ext[4];
bsde_real bn[3];
bsde_real d0, d1, d, bd, bo;
int i, j;
BSDE_SolidOBB_ObbNearestPoint(bmins, bmaxs, bpos, aorg, pt);
V3_SUB(aorg, pt, dv);
d=V3_NORMALIZE(dv, dv);
if(d<0.0001)
{
dv[2]=1;
pt[2]-=arad;
}
if(d>=arad)return(0);
V3_COPY(dv, bn);
bd=arad-d;
for(i=0; i<3; i++)
{
ext[0]=V3_DOT(aorg, bpos+i*4)-arad;
ext[1]=V3_DOT(aorg, bpos+i*4)+arad;
BSDE_SolidOBB_ObbPlaneExtents(bmins, bmaxs, bpos,
bpos+i*4, ext+2);
d0=(ext[0]>ext[2])?ext[0]:ext[2];
d1=(ext[1]<ext[3])?ext[1]:ext[3];
d=d1-d0;
if(d<0)return(0);
if(d<bd)
{
V3_COPY(bpos+i*4, bn);
bd=d;
}
}
if(org)
{
V3_COPY(pt, org);
V3_NORMALIZE(bn, bn);
V3_COPY(bn, norm);
*dist=bd;
}
return(1);
}
int BSDE_SolidHull_ContactHull(BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real anv[64*4], bnv[64*4];
bsde_real bn[4], bo[3], tn[4], pt[3];
bsde_real bd;
bsde_real m, n, o, p, d, f;
int i, j, k;
i=BSDE_SolidHull2_CalcCollide(aobj, bobj,
aobj->norms, bobj->norms,
aobj->vecs, bobj->vecs, aobj->n_faces, bobj->n_faces,
aobj->n_vecs, bobj->n_vecs,
axform, bxform, org, norm, dist);
return(i);
V3_SUB(aorg, borg, bn);
V3_NORMALIZE(bn, norm);
BSDE_PlaneExtents(aobj, norm, &m, &n);
BSDE_PlaneExtents(bobj, norm, &o, &p);
m=(m>o)?m:o; n=(n<p)?n:p;
d=V3_DOT(borg, norm);
V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
}
int BSDE_SolidSphere_CollideObbContact(
bsde_real arad, bsde_real *aorg,
bsde_real *bmins, bsde_real *bmaxs, bsde_real *pos,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
bsde_real pt[3], dv[3];
bsde_real d;
BSDE_SolidOBB_ObbNearestPoint(bmins, bmaxs, pos, aorg, pt);
V3_SUB(aorg, pt, dv);
d=V3_NORMALIZE(dv, dv);
if(d<0.0001)
{
dv[2]=1;
pt[2]-=arad;
}
if(d>=arad)return(0);
if(org)
{
V3_COPY(pt, org);
V3_COPY(dv, norm);
*dist=arad-d;
}
return(1);
}
void BSDE_SolidHull_ModifyHullFaces(BSDE_Solid *obj, bsde_real *norms, int n)
{
bsde_real d;
int i;
if(obj->vecs) { free(obj->vecs); obj->vecs=NULL; }
if(obj->faces) { free(obj->faces); obj->faces=NULL; }
if(obj->facevecs) { free(obj->facevecs); obj->facevecs=NULL; }
if(obj->norms)free(obj->norms);
obj->norms=(bsde_real *)malloc(4*n*sizeof(bsde_real));
obj->n_faces=n;
for(i=0; i<n; i++)
{
V3_NORMALIZE(norms+i*4, obj->norms+i*4);
obj->norms[i*4+3]=norms[i*4+3];
}
if((obj->mass>0) && (obj->volume>0))
d=obj->mass/obj->volume;
else d=1;
BSDE_SolidHull_MakeHullFaces(obj);
obj->mass=d*obj->volume;
BSDE_BuildInertiaTensor(obj);
}
void BSDE_Predict(BSDE_World *world, BSDE_Solid *cur, bsde_real dt)
{
bsde_real qa[4], qb[4], n[3], l;
bsde_real ma[9], mb[9], mc[9];
int i;
if(cur->stateflags&BSDE_STATEFL_MOVECACHE)
return;
// if(cur->moveflags&BSDE_MOVEFL_IDLE)
// return;
//predict origin and rotation
V3_ADDSCALE(cur->org, cur->vel, dt, cur->e_org);
l=V3_NORMALIZE(cur->avel, n);
// printf("spin (%f %f %f) %f\n", n[0], n[1], n[2], l);
BSDE_Quat_FromAxis(n, l*dt, qa);
BSDE_Quat_Multiply(qa, cur->rot, qb);
// BSDE_Quat_Multiply(cur->rot, qa, qb);
BSDE_Quat_Normalize(qb, cur->e_rot);
V3_COPY(cur->vel, cur->e_vel);
V3_COPY(cur->tvel, cur->e_tvel);
//calculate current inertia tensor
BSDE_WorldInertiaTensor(cur);
//calculate current transform
BSDE_Quat_ToMatrix(cur->rot, cur->xform);
for(i=0; i<3; i++)cur->xform[3*4+i]+=cur->org[i];
//calculate predicted transform
BSDE_Quat_ToMatrix(cur->e_rot, cur->e_xform);
for(i=0; i<3; i++)cur->e_xform[3*4+i]+=cur->e_org[i];
//calculate move volume
BSDE_BoxMove(cur, cur->mmins, cur->mmaxs);
//clear prediction flag (used for expensive internal predictions)
cur->moveflags&=~BSDE_MOVEFL_PREDICT;
if(cur->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_IDLE))
cur->stateflags|=BSDE_STATEFL_MOVECACHE;
}
int BSDE_SolidHull2_CollideOBB(
bsde_real *amins, bsde_real *amaxs,
bsde_real *bmins, bsde_real *bmaxs,
bsde_real *axform, bsde_real *bxform,
bsde_real *rorg, bsde_real *rnorm, bsde_real *rdist)
{
bsde_real apts[8*3], bpts[8*3];
bsde_real ant[6*4], bnt[6*4];
bsde_real dir[4];
bsde_real d0, d1, d2, d3, m, n, d;
int i;
BSDE_SolidOBB3_GenPointsLocal(amins, amaxs, apts);
BSDE_SolidOBB3_GenPointsLocal(bmins, bmaxs, bpts);
BSDE_SolidOBB3_GenNormsLocal(amins, amaxs, ant);
BSDE_SolidOBB3_GenNormsLocal(bmins, bmaxs, bnt);
if(0)
{
V3_SUB(axform+3*4, bxform+3*4, dir);
V3_NORMALIZE(dir, dir);
dir[3]=0;
BSDE_SolidHull2_ProjectPointsLine(apts, 8,
axform, dir, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bpts, 8,
bxform, dir, &d2, &d3);
m=(d0>d2)?d0:d2;
n=(d1<d3)?d1:d3;
if(m>n)return(0); //somehow, no collision
d=(m+n)/2-V3_DOT(bxform+3*4, dir);
V3_ADDSCALE(bxform+3*4, dir, d, rorg);
V3_COPY(dir, rnorm);
// V3_SCALE(dir, -1, rnorm);
*rdist=n-m;
return(1);
}
i=BSDE_SolidHull2_CalcCollide(NULL, NULL,
ant, bnt, apts, bpts, 6, 6, 8, 8,
axform, bxform, rorg, rnorm, rdist);
// printf("Collide OBB %d (%g %g %g) %g\n", i,
// rnorm[0], rnorm[1], rnorm[2], rdist);
return(i);
}
int BSDE_SolidHull_ContactBox(BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real bpts[8*4];
static bsde_real bnv[6*4];
bsde_real bn[4], bo[3], tn[4], pt[3];
bsde_real bd;
bsde_real m, n, o, p, d, f;
int i, j, k;
BSDE_SolidOBB3_GenPointsLocal(bobj->mins, bobj->maxs, bpts);
BSDE_SolidOBB3_GenNormsLocal(bobj->mins, bobj->maxs, bnv);
if(aobj->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_SEMISTATIC))
{
i=BSDE_SolidHull2_CalcCollideStatic(aobj, bobj,
aobj->norms, bnv,
aobj->vecs, bpts, aobj->n_faces, 6,
aobj->n_vecs, 8,
axform, bxform, org, norm, dist);
return(i);
}
i=BSDE_SolidHull2_CalcCollide(aobj, bobj,
aobj->norms, bnv,
aobj->vecs, bpts, aobj->n_faces, 6,
aobj->n_vecs, 8,
axform, bxform, org, norm, dist);
return(i);
#if 0
V3_SUB(aorg, borg, bn);
V3_NORMALIZE(bn, norm);
BSDE_PlaneExtents(aobj, norm, &m, &n);
BSDE_PlaneExtents(bobj, norm, &o, &p);
m=(m>o)?m:o; n=(n<p)?n:p;
d=V3_DOT(borg, norm);
V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
#endif
}
void BSDE_PredictPosTime(BSDE_Solid *cur, bsde_real dt,
bsde_real *org, bsde_real *rot, bsde_real *xform)
{
bsde_real qa[4], qb[4], n[3], l;
int i;
if(dt>0)
{
V3_ADDSCALE(cur->org, cur->vel, dt, org);
l=V3_NORMALIZE(cur->avel, n);
BSDE_Quat_FromAxis(n, l*dt, qa);
BSDE_Quat_Multiply(qa, cur->rot, qb);
BSDE_Quat_Normalize(qb, rot);
}else
{
V3_COPY(cur->org, org);
V3_COPY(cur->rot, rot);
}
BSDE_Quat_ToMatrix(rot, xform);
for(i=0; i<3; i++)xform[3*4+i]+=org[i];
}
int BSDE_SolidHull_ContactSphere(BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real bpts[8*4];
static bsde_real bnv[6*4];
bsde_real bn[4], bo[3], tn[4], pt[3];
bsde_real bd;
bsde_real m, n, o, p, d, f;
int i, j, k;
if(aobj->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_SEMISTATIC))
{
// printf("Sphere Static Hukk\n");
i=BSDE_SolidHull2_CalcCollideStaticSphere(aobj, bobj,
aobj->norms, aobj->vecs, aobj->n_faces,
aobj->n_vecs, axform, bxform, org, norm, dist);
return(i);
}
#if 1
V3_SUB(aorg, borg, bn);
V3_NORMALIZE(bn, norm);
BSDE_PlaneExtents(aobj, norm, &m, &n);
BSDE_PlaneExtents(bobj, norm, &o, &p);
m=(m>o)?m:o; n=(n<p)?n:p;
d=V3_DOT(borg, norm);
V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
#endif
}
int BSDE_SolidHull2_CalcCollide(
BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *anorm, bsde_real *bnorm,
bsde_real *avecs, bsde_real *bvecs,
int anf, int bnf, int anv, int bnv,
bsde_real *axform, bsde_real *bxform,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real pts0[16*4], pts1[16*4];
bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[3];
bsde_real ldir[4], lorg[3];
bsde_real m, n, bd, d0, d1, d2, d3;
bsde_real f;
int i, j, k;
V3_ZERO(bn);
bd=999999;
// bd=0;
// printf("m0\n");
V3_SUB(axform+3*4, bxform+3*4, ldir);
V3_NORMALIZE(ldir, ldir);
#if 0
BSDE_SolidHull2_ProjectPointsLine(avecs, anv, axform, ldir, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv, bxform, ldir, &d2, &d3);
m=(d0>d2)?d0:d2; n=(d1<d3)?d1:d3;
ldir[3]=(m+n)/2;
V4_COPY(ldir, bn); bd=ldir[3]-m;
#endif
#if 1
for(i=0; i<anf; i++)
{
// printf("N (%g %g %g %g)\n",
// anorm[i*4+0], anorm[i*4+1],
// anorm[i*4+2], anorm[i*4+3]);
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, dir, &m, &n);
if(m>dir[3])return(0);
// if(n<dir[3])continue;
// if((dir[3]-m)<0.01)continue;
// f=(dir[3]-m)*(2.0-fabs(V3_DOT(bn, norm)));
f=dir[3]-m;
if(f<bd)
// if((dir[3]-m)<bd)
{
// V4_COPY(dir, bn);
V4_SCALE(dir, -1, bn);
bd=dir[3]-m;
}
}
for(i=0; i<bnf; i++)
{
BSDE_Plane_TransformNormal(bnorm+i*4, bxform, dir);
BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
axform, dir, &m, &n);
if(m>dir[3])return(0);
// if(n<dir[3])continue;
// if((dir[3]-m)<0.01)continue;
// f=(dir[3]-m)*(2.0-fabs(V3_DOT(bn, norm)));
f=dir[3]-m;
if(f<bd)
// if((dir[3]-m)<bd)
{
// V4_SCALE(dir, -1, bn);
V4_COPY(dir, bn);
bd=dir[3]-m;
}
}
#if 0
for(i=0; i<anf; i++)
for(j=0; j<bnf; j++)
{
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir1);
BSDE_Plane_TransformNormal(bnorm+j*4, bxform, dir2);
V3_CROSS(dir1, dir2, dir);
bd=V3_NORMALIZE(dir, dir);
if(bd<0.01)continue;
BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
axform, dir, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, dir, &d2, &d3);
m=(d0>d2)?d0:d2;
n=(d1<d3)?d1:d3;
if(m>n)return(0);
dir[3]=(m+n)/2;
if((n-m)<bd)
{
// V4_COPY(dir, bn);
V4_SCALE(dir, -1, bn);
bd=n-m;
}
}
#endif
#endif
// printf("m1\n");
BSDE_SolidHull_MakePlaneFace(bn, pts0);
j=4;
// for(i=0; i<j; i++)
// printf("\t(%g %g %g)\n", pts0[i*3+0], pts0[i*3+1], pts0[i*3+2]);
for(i=0; i<anf; i++)
{
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
}
for(i=0; i<bnf; i++)
{
BSDE_Plane_TransformNormal(bnorm+i*4, bxform, dir);
j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
}
#if 1
if((j>0) && (bd>0.15))
{
// printf("A\n");
V3_ZERO(lorg);
for(k=0; k<j; k++) { V3_ADD(lorg, pts0+k*3, lorg); }
V3_SCALE(lorg, 1.0/j, lorg)
V3_SUB(axform+3*4, lorg, dir1);
// V3_NORMALIZE(dir1, dir1);
V3_SUB(lorg, bxform+3*4, dir2);
// V3_NORMALIZE(dir2, dir2);
V3_ADD(dir1, dir2, dir);
if(aobj && bobj)
{
V3_SUB(bobj->vel, aobj->vel, dir1);
// V3_SUB(aobj->vel, bobj->vel, dir1);
V3_ADDSCALE(dir, dir1, 10, dir);
}
V3_NORMALIZE(dir, dir);
dir[3]=0;
printf("A %f %f %f\n", dir[0], dir[1], dir[2]);
BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
axform, dir, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, dir, &d2, &d3);
m=(d0>d2)?d0:d2;
n=(d1<d3)?d1:d3;
if(m>n)return(0); //somehow, no collision
bd=(m+n)/2-V3_DOT(bxform+3*4, dir);
V3_ADDSCALE(bxform+3*4, dir, bd, org);
V3_COPY(dir, norm);
*dist=n-m;
return(1);
}
#endif
#if 1
// if(!j || (bd>0.1))
// if(!j)
// if(0)
// if(bd>0.15)
// if(bd>0.20)
// if(bd>0.01)
if(bd>0.25)
// if(bd>0.10)
{
printf("B\n");
V3_ZERO(dir1); V3_ZERO(dir2);
for(i=0; i<anv; i++)
{ V3_ADD(dir1, avecs+i*3, dir1); }
for(i=0; i<bnv; i++)
{ V3_ADD(dir2, bvecs+i*3, dir2); }
V3_SCALE(dir1, 1.0/anv, dir1);
V3_SCALE(dir2, 1.0/bnv, dir2);
V3_SUB(dir1, dir2, dir);
// V3_SUB(dir2, dir1, dir);
// V3_SUB(axform+3*4, bxform+3*4, dir);
// V3_SUB(axform+3*4, bxform+3*4, dir);
if(aobj && bobj)
{
V3_SUB(bobj->vel, aobj->vel, dir1);
// V3_SUB(aobj->vel, bobj->vel, dir1);
V3_ADDSCALE(dir, dir1, 10, dir);
}
V3_NORMALIZE(dir, dir);
dir[3]=0;
BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
axform, dir, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, dir, &d2, &d3);
m=(d0>d2)?d0:d2;
n=(d1<d3)?d1:d3;
if(m>n)return(0); //somehow, no collision
bd=(m+n)/2-V3_DOT(bxform+3*4, dir);
V3_ADDSCALE(bxform+3*4, dir, bd, org);
V3_COPY(dir, norm);
// V3_SCALE(dir, -1, norm);
*dist=n-m;
return(1);
}
#endif
/*
if(!j)
{
printf("clipped away\n");
return(0);
}
*/
for(i=0; i<j; i++)
{
V3_COPY(pts0+i*3, org+i*4);
org[i*4+3]=fabs(V3_NDOT(pts0+i*3, bn));
}
V3_COPY(bn, norm);
// V3_SCALE(bn, -1, norm);
*dist=bd;
return(j);
}
static void computeCurvature(VerTex *vertex,int nvt,FaCe *face, int nfc,int* ref_tab,int nb,float* curv)
{
int n,m,reference;
VECTOR *v_n, *v_e1,*v_e2,*v;
float nx,ny,nz,area,dx,dy,dz,y,r2,u1,u2,YR2,R4;
v_n=VectorAlloc(3,MATRIX_REAL);
v_e1=VectorAlloc(3,MATRIX_REAL);
v_e2=VectorAlloc(3,MATRIX_REAL);
v=VectorAlloc(3,MATRIX_REAL);
for (n=0; n<nb; n++)
{
reference=ref_tab[n];
//first need to compute normal
nx=ny=nz=area=0;
for (m=0; m<vertex[reference].fnum; m++)
{
nx+=face[vertex[reference].f[m]].nx*face[vertex[reference].f[m]].area;
ny+=face[vertex[reference].f[m]].ny*face[vertex[reference].f[m]].area;
nz+=face[vertex[reference].f[m]].nz*face[vertex[reference].f[m]].area;
area+=face[vertex[reference].f[m]].area;
}
nx/=area;
ny/=area;
nz/=area;
VECTOR_LOAD(v_n,nx,ny,nz);
//now need to compute the tangent plane!
VECTOR_LOAD(v,ny,nz,nx);
V3_CROSS_PRODUCT(v_n,v,v_e1);
if ((V3_LEN_IS_ZERO(v_e1)))
{
if (nz!=0)
VECTOR_LOAD(v,ny,-nz,nx)
else if (ny!=0)
VECTOR_LOAD(v,-ny,nz,nx)
else
VECTOR_LOAD(v,ny,nz,-nx);
V3_CROSS_PRODUCT(v_n,v,v_e1);
}
V3_CROSS_PRODUCT(v_n,v_e1,v_e2);
V3_NORMALIZE(v_e1,v_e1);
V3_NORMALIZE(v_e2,v_e2);
//finally compute curvature by fitting a 1-d quadratic r->a*r*r: curv=2*a
for (YR2=0,R4=0,m=0; m<vertex[reference].vnum; m++)
{
dx=vertex[vertex[reference].v[m]].x-vertex[reference].x;
dy=vertex[vertex[reference].v[m]].y-vertex[reference].y;
dz=vertex[vertex[reference].v[m]].z-vertex[reference].z;
VECTOR_LOAD(v,dx,dy,dz);
y=V3_DOT(v,v_n);
u1=V3_DOT(v_e1,v);
u2=V3_DOT(v_e2,v);
r2=u1*u1+u2*u2;
YR2+=y*r2;
R4+=r2*r2;
}
curv[n]=2*YR2/R4;
}
VectorFree(&v);
VectorFree(&v_n);
VectorFree(&v_e1);
VectorFree(&v_e2);
}
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;
}
}
int BSDE_SolidHull2_CalcCollideStatic(
BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *anorm, bsde_real *bnorm,
bsde_real *avecs, bsde_real *bvecs,
int anf, int bnf, int anv, int bnv,
bsde_real *axform, bsde_real *bxform,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real pts0[64*4], pts1[64*4];
bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[4];
bsde_real ldir[4], lorg[4];
bsde_real m, n, bd, d0, d1, d2, d3;
bsde_real f;
int i, j, k;
V4_ZERO(bn);
bd=999999;
// V3_SCALE(bobj->vel, -1, bn);
// V3_COPY(bobj->vel, bn);
// V3_NORMALIZE(bn, bn);
// bd=5;
// bd=999999;
// BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
// bxform, bn, &m, &n);
// bn[3]=0;
for(i=0; i<anf; i++)
{
// printf("N (%g %g %g %g)\n",
// anorm[i*4+0], anorm[i*4+1],
// anorm[i*4+2], anorm[i*4+3]);
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, dir, &m, &n);
if(m>dir[3])return(0);
// if(n<dir[3])continue;
// if((dir[3]-m)<0.01)continue;
// f=V3_NDOT(bxform+3*4, dir);
f=V3_NDOT(bobj->org, dir);
if(f<0)continue;
f=dir[3]-m;
if(f<bd)
{
// V4_COPY(dir, bn);
V4_SCALE(dir, -1, bn);
bd=f;
}
}
if(bd>999990)
{
printf("no plane\n");
// V3_SCALE(bobj->vel, -1, bn);
// V3_COPY(bobj->vel, bn);
V3_SUB(bobj->org, bobj->e_org, bn);
V3_NORMALIZE(bn, bn);
printf("Fake %f %f %f\n", bn[0], bn[1], bn[2]);
BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
axform, bn, &d0, &d1);
BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
bxform, bn, &d2, &d3);
m=(d0>d2)?d0:d2;
n=(d1<d3)?d1:d3;
bd=n-m;
bn[3]=n;
f=V3_NDOT(bobj->org, bn);
V3_ADDSCALE(bobj->org, bn, -f-bd, org);
org[3]=bd;
V3_COPY(bn, norm);
*dist=bd;
return(1);
}
BSDE_SolidHull_MakePlaneFace(bn, pts0);
j=4;
// for(i=0; i<j; i++)
// printf("\t(%g %g %g)\n", pts0[i*3+0], pts0[i*3+1], pts0[i*3+2]);
#if 0
for(i=0; i<anf; i++)
{
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
}
#endif
for(i=0; i<bnf; i++)
{
BSDE_Plane_TransformNormal(bnorm+i*4, bxform, dir);
j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
}
if(!j)
{
printf("clipped away\n");
f=V3_NDOT(bobj->org, bn);
V3_ADDSCALE(bobj->org, bn, -f, org);
org[3]=bd;
V3_COPY(bn, norm);
// V3_SCALE(bn, -1, norm);
*dist=bd;
return(1);
return(0);
}
for(i=0; i<j; i++)
{
V3_COPY(pts0+i*3, org+i*4);
// org[i*4+3]=fabs(V3_NDOT(pts0+i*3, bn));
org[i*4+3]=bd;
}
V3_COPY(bn, norm);
// V3_SCALE(bn, -1, norm);
*dist=bd;
return(j);
}
int BSDE_SolidHull2_CalcCollideStaticSphere(
BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *anorm, bsde_real *avecs, int anf, int anv,
bsde_real *axform, bsde_real *bxform,
bsde_real *org, bsde_real *norm, bsde_real *dist)
{
static bsde_real pts0[64*4], pts1[64*4];
bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[3];
bsde_real ldir[4], lorg[3];
bsde_real m, n, o, p, d, bd, d0, d1, d2, d3;
bsde_real f;
int i, j, k;
V4_ZERO(bn);
bd=999999;
for(i=0; i<anf; i++)
{
BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
f=V3_DOT(bxform+12, dir);
m=f-bobj->radius; n=f+bobj->radius;
if(m>dir[3])return(0);
f=V3_NDOT(bxform+3*4, dir);
// f=V3_NDOT(bobj->org, dir);
if(f<0)continue;
f=dir[3]-m;
if(f<bd)
{
// V4_COPY(dir, bn);
V4_SCALE(dir, -1, bn);
bd=f;
}
}
if(bd>999990)
{
printf("no plane (Sph)\n");
BSDE_SolidHull_NearestPoint(aobj, axform, bxform+12, bo);
if(V3_DIST(bxform+12, bo)<0.001)V3_COPY(axform+12, bo);
// V3_SUB(borg, bo, dir);
V3_SUB(bo, bxform+3*4, 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(bxform+3*4, dir);
V3_ADDSCALE(bxform+3*4, dir, ((m+n)/2)-d, org);
*dist=n-m;
return(1);
}
V3_ADDSCALE(bxform+3*4, bn, -bobj->radius, org);
org[3]=bd;
V3_COPY(bn, norm);
// V3_SCALE(bn, -1, norm);
*dist=bd;
return(1);
}
