void BSDE_SolidOBB_PlaneExtents(bsde_real *mins, bsde_real *maxs,
bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
bsde_real w[3];
bsde_real m, n, d;
int i;
w[0]=V3_DOT(xform+(0*4), norm);
w[1]=V3_DOT(xform+(1*4), norm);
w[2]=V3_DOT(xform+(2*4), norm);
d=V3_DOT(xform+3*4, norm);
m=0;
n=0;
for(i=0; i<3; i++)
{
if(w[i]>0)
{
m+=mins[i]*w[i];
n+=maxs[i]*w[i];
}else
{
m+=maxs[i]*w[i];
n+=mins[i]*w[i];
}
}
*min=m+d;
*max=n+d;
}
int BSDE_SolidHull_CheckCylinderCollide(BSDE_Solid *aobj, BSDE_Solid *bobj,
bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg)
{
bsde_real f;
bsde_real pt[3], pt2[3];
bsde_real m, n;
int i;
f=V3_DOT(aorg, bxform+2*4)-V3_DOT(borg, bxform+2*4);
if(f>(bobj->height*0.5))f=(bobj->height*0.5);
if(f<(-(bobj->height*0.5)))f=-(bobj->height*0.5);
pt[0]=borg[0]+f*bxform[2*4+0];
pt[1]=borg[1]+f*bxform[2*4+1];
pt[2]=borg[2]+f*bxform[2*4+2];
BSDE_SolidHull_NearestPoint(aobj, axform, pt, pt2);
if(V3_DIST(pt, pt2)>=bobj->radius)
return(0);
if(bobj->solidtype==BSDE_SOLID_CYLINDER)
{
f=V3_DOT(pt2, bxform+2*4)-V3_DOT(borg, bxform+2*4);
if(f>(bobj->height*0.5))return(0);
if(f<(-(bobj->height*0.5)))return(0);
}
return(1);
}
void BSDE_SolidOBB_ObbNearestPoint(
bsde_real *mins, bsde_real *maxs, bsde_real *pos,
bsde_real *org, bsde_real *pt)
{
bsde_real n[3], o[3];
bsde_real d;
int i;
V3_COPY(org, pt);
V3_COPY(pos+12, o);
for(i=0; i<3; i++)
{
V3_COPY(pos+(i*4), n);
d=V3_DOT(pt, n)-V3_DOT(o, n);
if(d<mins[i])
{
V3_ADDSCALE(pt, n, mins[i]-d, pt);
}
if(d>maxs[i])
{
V3_ADDSCALE(pt, n, maxs[i]-d, pt);
}
}
}
int BSDE_SolidOBB_CheckObbPlane(bsde_real *mins, bsde_real *maxs,
bsde_real *xform, bsde_real *norm, bsde_real *io)
{
bsde_real w[3];
bsde_real xwa, ywa, zwa;
bsde_real m, n;
int i;
w[0]=V3_DOT(xform+(0*4), norm);
w[1]=V3_DOT(xform+(1*4), norm);
w[2]=V3_DOT(xform+(2*4), norm);
xwa=xform[3*4+0]*norm[0];
ywa=xform[3*4+1]*norm[1];
zwa=xform[3*4+2]*norm[2];
m=0;
n=0;
for(i=0; i<3; i++)
{
if(w[i]>0)
{
m+=mins[i]*w[i];
n+=maxs[i]*w[i];
}else
{
m+=maxs[i]*w[i];
n+=mins[i]*w[i];
}
}
m+=xwa+ywa+zwa-norm[3];
n+=xwa+ywa+zwa-norm[3];
i=0;
if(m<0)i|=2;
if(n>=0)i|=1;
if(!i)
{
printf("BSDE_SolidOBB_CheckObbPlane: fail\n");
printf("[(%f %f %f) (%f %f %f)]\n",
mins[0], mins[1], mins[2],
maxs[0], maxs[1], maxs[2]);
printf("(%f %f %f %f)\n",
norm[0], norm[1], norm[2], norm[3]);
printf("%f %f\n", m, n);
}
if(io && (i==3))
BSDE_SolidOBB_ObbPlaneIntersect(
mins, maxs, xform, norm, io);
return(i);
}
int BSDE_SolidOBB_ObbImpactNormal(
bsde_real *amins, bsde_real *amaxs, bsde_real *aspos, bsde_real *aepos,
bsde_real *bmins, bsde_real *bmaxs, bsde_real *bspos, bsde_real *bepos,
bsde_real *vel, bsde_real *norm)
{
bsde_real rv[3], rn[3];
bsde_real ext[2];
int i;
rv[0]=V3_DOT(vel, bepos+0*4);
rv[1]=V3_DOT(vel, bepos+1*4);
rv[2]=V3_DOT(vel, bepos+2*4);
if(fabs(rv[0])>fabs(rv[1]))
{
if(fabs(rv[0])>fabs(rv[2]))
{
rn[0]=(rv[0]>0)?1:-1;
rn[1]=0;
rn[2]=0;
}else
{
rn[0]=0;
rn[1]=0;
rn[2]=(rv[2]>0)?1:-1;
}
}else
{
if(fabs(rv[1])>fabs(rv[2]))
{
rn[0]=0;
rn[1]=(rv[1]>0)?1:-1;
rn[2]=0;
}else
{
rn[0]=0;
rn[1]=0;
rn[2]=(rv[2]>0)?1:-1;
}
}
for(i=0; i<3; i++)
{
norm[i]=rn[0]*bepos[0*4+i]+
rn[1]*bepos[1*4+i]+
rn[2]*bepos[2*4+i];
}
BSDE_SolidOBB_ObbPlaneExtents(bmins, bmaxs, bepos,
norm, ext);
norm[3]=ext[0]; //near extent
return(0);
}
void BSDE_Mat4_InverseTransform(bsde_real *a, bsde_real *b)
{
b[0]=a[0]; b[1]=a[4]; b[2]=a[8]; b[3]=0;
b[4]=a[1]; b[5]=a[5]; b[6]=a[9]; b[7]=0;
b[8]=a[2]; b[9]=a[6]; b[10]=a[10]; b[11]=0;
b[12]=-a[12]; b[13]=-a[13]; b[14]=-a[14]; b[15]=1;
b[12]=-V3_DOT(a+3*4, b+0*4);
b[13]=-V3_DOT(a+3*4, b+1*4);
b[14]=-V3_DOT(a+3*4, b+2*4);
}
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);
}
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_SolidOBB_ObbPlaneExtents(bsde_real *mins, bsde_real *maxs,
bsde_real *xform, bsde_real *norm, bsde_real *ext)
{
bsde_real w[3];
bsde_real xwa, ywa, zwa;
bsde_real m, n;
int i;
w[0]=V3_DOT(xform+(0*4), norm);
w[1]=V3_DOT(xform+(1*4), norm);
w[2]=V3_DOT(xform+(2*4), norm);
xwa=xform[3*4+0]*norm[0];
ywa=xform[3*4+1]*norm[1];
zwa=xform[3*4+2]*norm[2];
m=0;
n=0;
for(i=0; i<3; i++)
{
if(w[i]>0)
{
m+=mins[i]*w[i];
n+=maxs[i]*w[i];
}else
{
m+=maxs[i]*w[i];
n+=mins[i]*w[i];
}
}
m+=xwa+ywa+zwa;
n+=xwa+ywa+zwa;
ext[0]=m;
ext[1]=n;
return(0);
}
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_SolidOBB_ObbPointCollide(
bsde_real *mins, bsde_real *maxs, bsde_real *pos, bsde_real *org)
{
bsde_real d;
int i;
for(i=0; i<3; i++)
{
d=V3_DOT(org, pos+(i*4))-pos[3*4+i];
if(d<mins[i])return(0);
if(d>maxs[i])return(0);
}
return(1);
}
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_SolidHull_ProjectLine(BSDE_Solid *obj,
bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
bsde_real pt[3];
bsde_real m, n, f;
int i;
m=999999999.0;
n=-999999999.0;
for(i=0; i<obj->n_vecs; i++)
{
BSDE_Plane_TransformPoint(obj->vecs+i*3, xform, pt);
f=V3_DOT(pt, norm);
if(f<m)m=f;
if(f>n)n=f;
}
*min=m;
*max=n;
}
void BSDE_SolidHull2_ProjectPointsLine(bsde_real *vecs, int nvecs,
bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
bsde_real pt[3];
bsde_real m, n, f;
int i;
m=999999999.0;
n=-999999999.0;
for(i=0; i<nvecs; i++)
{
BSDE_Plane_TransformPoint(vecs+i*3, xform, pt);
f=V3_DOT(pt, norm);
if(f<m)m=f;
if(f>n)n=f;
}
*min=m;
*max=n;
}
void BSDE_Update(BSDE_World *world, BSDE_Solid *cur, bsde_real dt)
{
bsde_real f;
int i;
f=V3_DIST(cur->e_org, cur->org)+V4_DIST(cur->e_rot, cur->rot);
// if(f<(0.10*dt))
if(f<(0.25*dt))
{
cur->idletime+=dt;
if(cur->idletime>=1)
cur->moveflags|=BSDE_MOVEFL_IDLE;
}else
{
cur->idletime=0;
// cur->moveflags&=~BSDE_MOVEFL_IDLE;
cur->moveflags&=~BSDE_MOVEFL_IDLE;
cur->stateflags&=~BSDE_STATEFL_MOVECACHE;
}
V3_COPY(cur->e_org, cur->org);
V4_COPY(cur->e_rot, cur->rot);
if(!bsde_finite(V3_DOT(cur->org, cur->org)))
{ V3_ZERO(cur->org); }
if(!bsde_finite(V4_DOT(cur->rot, cur->rot)))
{ V4_ZERO(cur->rot); cur->rot[3]=1; }
if(cur->moveflags&BSDE_MOVEFL_IDLE)
{
V3_ZERO(cur->tvel);
V3_ZERO(cur->vel);
// V3_ZERO(cur->avel);
}
}
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
}
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;
}
}
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);
}
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);
}
