[ODE] cylinder - tri
slipch
slipch at gsc-game.kiev.ua
Mon Dec 1 22:41:22 MST 2003
Hello All
This may help to add collision between flat ended cylinder and
static tri-mesh
--
Best regards,
Konstantin Slipchenko mailto:slipch at gsc-game.kiev.ua
-------------- next part --------------
//dTriCyl may be used to do collision between cylinder in o1 parameter and a triangle
//definite by vertices v0,v1,v2. It fills contact geometry info
//in contact parameter. o2 - represent some tri-mesh geometry class.
//Konstantin Slpchenko
#include "../dCylinder/dCylinder.h"
bool inline cylinderCrossesLine(const dReal* p,const dReal* R,dReal hlz,
const dReal* v0,const dReal* v1,const dReal* l,dVector3 pos){
dReal _cos=dDOT14(l,R);
if(!(dFabs(_cos)<1.f)) return false;
dReal sin2=1.f-_cos*_cos;
dVector3 vp={v0[0]-p[0],v0[1]-p[1],v0[2]-p[2]};
dReal c1=dDOT(vp,l);
dReal c2=dDOT14(vp,R);
dReal t=(c2*_cos-c1)/sin2;
dReal q=(c2-c1*_cos)/sin2;
if(dFabs(q)>hlz) return false;
dVector3 v01={v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]};
dReal sidelength2=dDOT(v01,v01);
if(t*t>sidelength2) return false;
pos[0]=v0[0]+l[0]*t;
pos[1]=v0[1]+l[1]*t;
pos[2]=v0[2]+l[2]*t;
return true;
}
extern "C" int dTriCyl (
const dReal* v0,const dReal* v1,const dReal* v2,
dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip
)
{
// dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (dGeomGetClass(o1)== dCylinderClassUser);
const dReal *R = dGeomGetRotation(o1);
const dReal* p=dGeomGetPosition(o1);
dReal radius;
dReal hlz;
dGeomCylinderGetParams(o1,&radius,&hlz);
hlz/=2.f;
// find number of contacts requested
int maxc = flags & NUMC_MASK;
if (maxc < 1) maxc = 1;
if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed
dVector3 triAx;
dVector3 triSideAx0={v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]};
dVector3 triSideAx1={v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]};
dVector3 triSideAx2={v0[0]-v2[0],v0[1]-v2[1],v0[2]-v2[2]};
dCROSS(triAx,=,triSideAx0,triSideAx1);
int code=0;
dReal signum, outDepth,cos0,cos1,cos2,sin1;
////////////////////////////////////////////////////////////////////////////
//sepparation along tri plane normal;///////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
dNormalize3(triAx);
//cos0=dDOT14(triAx,R+0);
cos1=dFabs(dDOT14(triAx,R+1));
//cos2=dDOT14(triAx,R+2);
//sin1=_sqrt(cos0*cos0+cos2*cos2);
////////////////////////
//another way //////////
cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
sin1=_sqrt(REAL(1.)-cos1*cos1);
//////////////////////////////
dReal sidePr=cos1*hlz+sin1*radius;
dReal dist=dDOT(triAx,v0)-dDOT(triAx,p);
if(dist>0.f) return 0;
dReal depth=sidePr-dFabs(dist);
outDepth=depth;
signum=dist>0.f ? 1.f : -1.f;
code=0;
if(depth<0.f) return 0;
dReal depth0,depth1,depth2,dist0,dist1,dist2;
bool isPdist0,isPdist1,isPdist2;
bool testV0,testV1,testV2;
bool sideTestV00,sideTestV01,sideTestV02;
bool sideTestV10,sideTestV11,sideTestV12;
bool sideTestV20,sideTestV21,sideTestV22;
//////////////////////////////////////////////////////////////////////////////
//cylinder axis - one of the triangle vertexes touches cylinder's flat surface
//////////////////////////////////////////////////////////////////////////////
dist0=dDOT14(v0,R+1)-dDOT14(p,R+1);
dist1=dDOT14(v1,R+1)-dDOT14(p,R+1);
dist2=dDOT14(v2,R+1)-dDOT14(p,R+1);
isPdist0=dist0>0.f;
isPdist1=dist1>0.f;
isPdist2=dist2>0.f;
depth0=hlz-dFabs(dist0);
depth1=hlz-dFabs(dist1);
depth2=hlz-dFabs(dist2);
testV0=depth0>0.f;
testV1=depth1>0.f;
testV2=depth2>0.f;
if(isPdist0==isPdist1 && isPdist1== isPdist2) //(here and lower) check the tryangle is on one side of the cylinder
{
if(depth0>depth1)
if(depth0>depth2)
if(testV0){
if(depth0<outDepth)
{
signum= isPdist0 ? 1.f : -1.f;
outDepth=depth0;
code=1;
}
}
else
return 0;
else
if(testV2){
if(depth2<outDepth)
{
outDepth=depth2;
signum= isPdist2 ? 1.f : -1.f;
code=3;
}
}
else
return 0;
else
if(depth1>depth2)
if(testV1){
if(depth1<outDepth)
{
outDepth=depth1;
signum= isPdist1 ? 1.f : -1.f;
code=2;
}
}
else
return 0;
else
if(testV2){
if(depth2<outDepth)
{
outDepth=depth2;
signum= isPdist2 ? 1.f : -1.f;
code=2;
}
}
else return 0;
}
dVector3 axis,outAx;
dReal posProj;
dReal pointDepth=0.f;
#define TEST(vx,ox1,ox2,c) \
{\
posProj=dDOT14(v##vx,R+1)-dDOT14(p,R+1);\
\
axis[0]=v##vx[0]-p[0]-R[1]*posProj;\
axis[1]=v##vx[1]-p[1]-R[5]*posProj;\
axis[2]=v##vx[2]-p[2]-R[9]*posProj;\
\
dNormalize3(axis);\
\
\
dist0=dDOT(v0,axis)-dDOT(p,axis);\
dist1=dDOT(v1,axis)-dDOT(p,axis);\
dist2=dDOT(v2,axis)-dDOT(p,axis);\
\
isPdist0=dist0>0.f;\
isPdist1=dist1>0.f;\
isPdist2=dist2>0.f;\
\
depth0=radius-dFabs(dist0);\
depth1=radius-dFabs(dist1);\
depth2=radius-dFabs(dist2);\
\
sideTestV##vx##0=depth0>0.f;\
sideTestV##vx##1=depth1>0.f;\
sideTestV##vx##2=depth2>0.f;\
\
if(isPdist0==isPdist1 && isPdist1== isPdist2)\
\
{\
if(sideTestV##vx##0||sideTestV##vx##1||sideTestV##vx##2){\
if(!(depth##vx<depth##ox1 || depth##vx<depth##ox2))\
{\
if(depth##vx<outDepth && depth##vx > pointDepth)\
{\
pointDepth=depth##vx;\
signum= isPdist##vx ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
code=c;\
}\
}\
}\
else return 0;\
\
\
\
}\
}
if(testV0) TEST(0,1,2,4)
if(testV1 ) TEST(1,2,0,5)
//&& sideTestV01
if(testV2 ) TEST(2,0,1,6)
//&& sideTestV02 && sideTestV12
#undef TEST
dVector3 tpos,pos;
if(code>3) outDepth=pointDepth; //deepest vertex axis used if its depth less than outDepth
//else{
//bool outV0=!(testV0&&sideTestV00&&sideTestV10&&sideTestV20);
//bool outV1=!(testV1&&sideTestV01&&sideTestV11&&sideTestV21);
//bool outV2=!(testV2&&sideTestV02&&sideTestV12&&sideTestV22);
bool outV0=true;
bool outV1=true;
bool outV2=true;
/////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
///crosses between triangle sides and cylinder axis//////////////////////////
/////////////////////////////////////////////////////////////////////////////
#define TEST(ax,nx,ox,c) if(cylinderCrossesLine(p,R+1,hlz,v##ax,v##nx,triSideAx##ax,tpos)) {\
dCROSS114(axis,=,triSideAx##ax,R+1);\
dNormalize3(axis);\
dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\
dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\
\
isPdist##ax=dist##ax>0.f;\
isPdist##ox=dist##ox>0.f;\
\
if(isPdist##ax == isPdist##ox)\
{\
depth##ax=radius-dFabs(dist##ax);\
depth##ox=radius-dFabs(dist##ox);\
\
if(depth##ax>0.f){\
if(depth##ax<outDepth && depth##ax>depth##ox) \
{\
outDepth=depth##ax;\
signum= isPdist##ax ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
pos[0]=tpos[0];\
pos[1]=tpos[1];\
pos[2]=tpos[2];\
code=c;\
}\
}\
else if(depth##ox<0.f) return 0;\
\
}\
}
dNormalize3(triSideAx0);
if(outV0&&outV1)
TEST(0,1,2,7)
dNormalize3(triSideAx1);
if(outV1&&outV2)
TEST(1,2,0,8)
dNormalize3(triSideAx2);
if(outV2&&outV0)
TEST(2,0,1,9)
#undef TEST
////////////////////////////////////
//test cylinder rings on triangle sides////
////////////////////////////////////
dVector3 tAx,cen;
dReal sign;
bool cs;
#define TEST(ax,nx,ox,c) \
{\
posProj=dDOT(p,triSideAx##ax)-dDOT(v##ax,triSideAx##ax);\
axis[0]=p[0]-v0[0]-triSideAx##ax[0]*posProj;\
axis[1]=p[1]-v0[1]-triSideAx##ax[1]*posProj;\
axis[2]=p[2]-v0[2]-triSideAx##ax[2]*posProj;\
\
sign=dDOT14(axis,R+1)>0.f ? 1.f :-1.f;\
cen[0]=p[0]-sign*R[1]*hlz;\
cen[1]=p[1]-sign*R[5]*hlz;\
cen[2]=p[2]-sign*R[9]*hlz;\
\
cs=circleLineIntersection(R+1,cen,radius,triSideAx##ax,v##ax,-sign,tpos);\
\
axis[0]=tpos[0]-cen[0];\
axis[1]=tpos[1]-cen[1];\
axis[2]=tpos[2]-cen[2];\
\
if(cs){ \
\
cos0=dDOT14(axis,R+0);\
cos2=dDOT14(axis,R+2);\
tAx[0]=R[2]*cos0-R[0]*cos2;\
tAx[1]=R[6]*cos0-R[4]*cos2;\
tAx[2]=R[10]*cos0-R[8]*cos2;\
\
dCROSS(axis,=,triSideAx##ax,tAx);\
\
}\
dNormalize3(axis);\
dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\
if(dist##ax*dDOT(axis,triSideAx##nx)>0.f){\
\
cos0=dDOT14(axis,R+0);\
cos1=dFabs(dDOT14(axis,R+1));\
cos2=dDOT14(axis,R+2);\
\
\
sin1=_sqrt(cos0*cos0+cos2*cos2);\
\
sidePr=cos1*hlz+sin1*radius;\
\
\
dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\
\
isPdist##ax=dist##ax>0.f;\
isPdist##ox=dist##ox>0.f;\
\
if(isPdist##ax == isPdist##ox) \
\
{\
depth##ax=sidePr-dFabs(dist##ax);\
depth##ox=sidePr-dFabs(dist##ox);\
\
if(depth##ax>0.f){\
if(depth##ax<outDepth) \
{\
outDepth=depth##ax;\
signum= isPdist##ax ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
pos[0]=tpos[0];\
pos[1]=tpos[1];\
pos[2]=tpos[2];\
code=c;\
}\
}\
else if(depth##ox<0.f) return 0;\
\
\
}\
}\
}
if(7!=code)
TEST(0,1,2,10)
if(8!=code)
TEST(1,2,0,11)
if(9!=code)
TEST(2,0,1,12)
#undef TEST
//}
//////////////////////////////////////////////////////////////////////
///if we get to this poit tri touches cylinder///////////////////////
/////////////////////////////////////////////////////////////////////
dVector3 norm;
unsigned int ret;
if(code==0){
norm[0]=triAx[0]*signum;
norm[1]=triAx[1]*signum;
norm[2]=triAx[2]*signum;
dReal Q1 = -signum*dDOT14(triAx,R+0);
dReal Q2 = -signum*dDOT14(triAx,R+1);
dReal Q3 = -signum*dDOT14(triAx,R+2);
dReal factor =_sqrt(Q1*Q1+Q3*Q3);
factor= factor ? factor :1.f;
dReal A1 = radius * Q1/factor;
dReal A2 = hlz*Q2;
dReal A3 = radius * Q3/factor;
pos[0]=p[0];
pos[1]=p[1];
pos[2]=p[2];
pos[0]-= A1*R[0];
pos[1]-= A1*R[4];
pos[2]-= A1*R[8];
pos[0]-= A3*R[2];
pos[1]-= A3*R[6];
pos[2]-= A3*R[10];
pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];
ret=0;
dVector3 cross0, cross1, cross2;
dReal ds0,ds1,ds2;
dCROSS(cross0,=,triAx,triSideAx0);
ds0=dDOT(cross0,v0);
dCROSS(cross1,=,triAx,triSideAx1);
ds1=dDOT(cross1,v1);
dCROSS(cross2,=,triAx,triSideAx2);
ds2=dDOT(cross2,v2);
if(dDOT(cross0,pos)-ds0>0.f &&
dDOT(cross1,pos)-ds1>0.f &&
dDOT(cross2,pos)-ds2>0.f){
contact->pos[0] = pos[0];
contact->pos[1] = pos[1];
contact->pos[2] = pos[2];
contact->depth = outDepth;
ret=1;
}
if(dFabs(Q2)>M_SQRT1_2){
CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
} else {
CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
}
}
else if(code<7)//1-6
{
ret=1;
contact->depth = outDepth;
switch((code-1)%3){
case 0:
contact->pos[0]=v0[0];
contact->pos[1]=v0[1];
contact->pos[2]=v0[2];
break;
case 1:
contact->pos[0]=v1[0];
contact->pos[1]=v1[1];
contact->pos[2]=v1[2];
break;
case 2:
contact->pos[0]=v2[0];
contact->pos[1]=v2[1];
contact->pos[2]=v2[2];
break;
}
if(code<4){//1-3
norm[0]=R[1]*signum;
norm[1]=R[5]*signum;
norm[2]=R[9]*signum;
}
else {
norm[0]=outAx[0]*signum;
norm[1]=outAx[1]*signum;
norm[2]=outAx[2]*signum;
}
}
else {//7-12
ret=1;
contact->depth = outDepth;
norm[0]=outAx[0]*signum;
norm[1]=outAx[1]*signum;
norm[2]=outAx[2]*signum;
contact->pos[0] = pos[0];
contact->pos[1] = pos[1];
contact->pos[2] = pos[2];
}
if((int)ret>maxc) ret=(unsigned int)maxc;
for (unsigned int i=0; i<ret; ++i) {
CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o2);
CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o1);
CONTACT(contact,i*skip)->normal[0] = norm[0];
CONTACT(contact,i*skip)->normal[1] = norm[1];
CONTACT(contact,i*skip)->normal[2] = norm[2];
}
return ret;
}
More information about the ODE
mailing list