detectCollision method
Detect collision of the shapes provided
Implementation
@override
void detectCollision(Shape shape1,Shape shape2,ContactManifold manifold ) {
Cylinder c;
Box b;
flip = shape1 is Cylinder;
if(!flip){
c = shape2 as Cylinder;
b = shape1 as Box;
}
else{
c = shape1 as Cylinder;
b = shape2 as Box;
}
Vector3 sep=Vector3.zero();
Vector3 pos=Vector3.zero();
Vector3 dep=Vector3.zero();
if(!getSep(b,c,sep,pos,dep))return;
double pbx=b.position.x;
double pby=b.position.y;
double pbz=b.position.z;
double pcx=c.position.x;
double pcy=c.position.y;
double pcz=c.position.z;
double bw=b.halfWidth;
double bh=b.halfHeight;
double bd=b.halfDepth;
double ch=c.halfHeight;
double r=c.radius;
List<double> D = b.dimentions;
double nwx=D[0];//b.normalDirectionWidth.x;
double nwy=D[1];//b.normalDirectionWidth.y;
double nwz=D[2];//b.normalDirectionWidth.z;
double nhx=D[3];//b.normalDirectionHeight.x;
double nhy=D[4];//b.normalDirectionHeight.y;
double nhz=D[5];//b.normalDirectionHeight.z;
double ndx=D[6];//b.normalDirectionDepth.x;
double ndy=D[7];//b.normalDirectionDepth.y;
double ndz=D[8];//b.normalDirectionDepth.z;
double dwx=D[9];//b.halfDirectionWidth.x;
double dwy=D[10];//b.halfDirectionWidth.y;
double dwz=D[11];//b.halfDirectionWidth.z;
double dhx=D[12];//b.halfDirectionHeight.x;
double dhy=D[13];//b.halfDirectionHeight.y;
double dhz=D[14];//b.halfDirectionHeight.z;
double ddx=D[15];//b.halfDirectionDepth.x;
double ddy=D[16];//b.halfDirectionDepth.y;
double ddz=D[17];//b.halfDirectionDepth.z;
double ncx=c.normalDirection.x;
double ncy=c.normalDirection.y;
double ncz=c.normalDirection.z;
double dcx=c.halfDirection.x;
double dcy=c.halfDirection.y;
double dcz=c.halfDirection.z;
double nx=sep.x;
double ny=sep.y;
double nz=sep.z;
double dotw=nx*nwx+ny*nwy+nz*nwz;
double doth=nx*nhx+ny*nhy+nz*nhz;
double dotd=nx*ndx+ny*ndy+nz*ndz;
double dotc=nx*ncx+ny*ncy+nz*ncz;
bool right1=dotw>0;
bool right2=doth>0;
bool right3=dotd>0;
bool right4=dotc>0;
if(!right1)dotw=-dotw;
if(!right2)doth=-doth;
if(!right3)dotd=-dotd;
if(!right4)dotc=-dotc;
int state = 0;
if(dotc>0.999){
if(dotw>0.999){
if(dotw>dotc){state=1;}
else{ state=4;}
}
else if(doth>0.999){
if(doth>dotc){state=2;}
else{ state=4;}
}
else if(dotd>0.999){
if(dotd>dotc){state=3;}
else{ state=4;}
}
else{ state=4;}
}
else{
if(dotw>0.999){state=1;}
else if(doth>0.999){state=2;}
else if(dotd>0.999){state=3;}
}
double cbx = 0;
double cby = 0;
double cbz = 0;
double ccx = 0;
double ccy = 0;
double ccz = 0;
double r00;
double r01;
double r02;
double r10;
double r11;
double r12;
double r20;
double r21;
double r22;
double px;
double py;
double pz;
double pd;
double dot;
double len;
double tx;
double ty;
double tz;
double td;
double dx;
double dy;
double dz;
double d1x;
double d1y;
double d1z;
double d2x;
double d2y;
double d2z;
double sx;
double sy;
double sz;
double sd;
double ex;
double ey;
double ez;
double ed;
double dot1;
double dot2;
double t1;
double dir1x = 0;
double dir1y = 0;
double dir1z = 0;
double dir2x = 0;
double dir2y = 0;
double dir2z = 0;
double dir1l = 0;
double dir2l = 0;
if(state==0){
//manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,b,c,0,0,false);
manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,flip);
}
else if(state==4){
if(right4){
ccx=pcx-dcx;
ccy=pcy-dcy;
ccz=pcz-dcz;
nx=-ncx;
ny=-ncy;
nz=-ncz;
}
else{
ccx=pcx+dcx;
ccy=pcy+dcy;
ccz=pcz+dcz;
nx=ncx;
ny=ncy;
nz=ncz;
}
double v1x = 0;
double v1y = 0;
double v1z = 0;
double v2x = 0;
double v2y = 0;
double v2z = 0;
double v3x = 0;
double v3y = 0;
double v3z = 0;
double v4x = 0;
double v4y = 0;
double v4z = 0;
dot=1;
state=0;
dot1=nwx*nx+nwy*ny+nwz*nz;
if(dot1<dot){
dot=dot1;
state=0;
}
if(-dot1<dot){
dot=-dot1;
state=1;
}
dot1=nhx*nx+nhy*ny+nhz*nz;
if(dot1<dot){
dot=dot1;
state=2;
}
if(-dot1<dot){
dot=-dot1;
state=3;
}
dot1=ndx*nx+ndy*ny+ndz*nz;
if(dot1<dot){
dot=dot1;
state=4;
}
if(-dot1<dot){
dot=-dot1;
state=5;
}
List<double> v = b.elements;
switch(state){
case 0:
//v=b.vertex1;
v1x=v[0];//v.x;
v1y=v[1];//v.y;
v1z=v[2];//v.z;
//v=b.vertex3;
v2x=v[6];//v.x;
v2y=v[7];//v.y;
v2z=v[8];//v.z;
//v=b.vertex4;
v3x=v[9];//v.x;
v3y=v[10];//v.y;
v3z=v[11];//v.z;
//v=b.vertex2;
v4x=v[3];//v.x;
v4y=v[4];//v.y;
v4z=v[5];//v.z;
break;
case 1:
//v=b.vertex6;
v1x=v[15];//v.x;
v1y=v[16];//v.y;
v1z=v[17];//v.z;
//v=b.vertex8;
v2x=v[21];//v.x;
v2y=v[22];//v.y;
v2z=v[23];//v.z;
//v=b.vertex7;
v3x=v[18];//v.x;
v3y=v[19];//v.y;
v3z=v[20];//v.z;
//v=b.vertex5;
v4x=v[12];//v.x;
v4y=v[13];//v.y;
v4z=v[14];//v.z;
break;
case 2:
//v=b.vertex5;
v1x=v[12];//v.x;
v1y=v[13];//v.y;
v1z=v[14];//v.z;
//v=b.vertex1;
v2x=v[0];//v.x;
v2y=v[1];//v.y;
v2z=v[2];//v.z;
//v=b.vertex2;
v3x=v[3];//v.x;
v3y=v[4];//v.y;
v3z=v[5];//v.z;
//v=b.vertex6;
v4x=v[15];//v.x;
v4y=v[16];//v.y;
v4z=v[17];//v.z;
break;
case 3:
//v=b.vertex8;
v1x=v[21];//v.x;
v1y=v[22];//v.y;
v1z=v[23];//v.z;
//v=b.vertex4;
v2x=v[9];//v.x;
v2y=v[10];//v.y;
v2z=v[11];//v.z;
//v=b.vertex3;
v3x=v[6];//v.x;
v3y=v[7];//v.y;
v3z=v[8];//v.z;
//v=b.vertex7;
v4x=v[18];//v.x;
v4y=v[19];//v.y;
v4z=v[20];//v.z;
break;
case 4:
//v=b.vertex5;
v1x=v[12];//v.x;
v1y=v[13];//v.y;
v1z=v[14];//v.z;
//v=b.vertex7;
v2x=v[18];//v.x;
v2y=v[19];//v.y;
v2z=v[20];//v.z;
//v=b.vertex3;
v3x=v[6];//v.x;
v3y=v[7];//v.y;
v3z=v[8];//v.z;
//v=b.vertex1;
v4x=v[0];//v.x;
v4y=v[1];//v.y;
v4z=v[2];//v.z;
break;
case 5:
//v=b.vertex2;
v1x=v[3];//v.x;
v1y=v[4];//v.y;
v1z=v[5];//v.z;
//v=b.vertex4;
v2x=v[9];//v.x;
v2y=v[10];//v.y;
v2z=v[11];//v.z;
//v=b.vertex8;
v3x=v[21];//v.x;
v3y=v[22];//v.y;
v3z=v[23];//v.z;
//v=b.vertex6;
v4x=v[15];//v.x;
v4y=v[16];//v.y;
v4z=v[17];//v.z;
break;
}
pd=nx*(v1x-ccx)+ny*(v1y-ccy)+nz*(v1z-ccz);
if(pd<=0)manifold.addPoint(v1x,v1y,v1z,-nx,-ny,-nz,pd,flip);
pd=nx*(v2x-ccx)+ny*(v2y-ccy)+nz*(v2z-ccz);
if(pd<=0)manifold.addPoint(v2x,v2y,v2z,-nx,-ny,-nz,pd,flip);
pd=nx*(v3x-ccx)+ny*(v3y-ccy)+nz*(v3z-ccz);
if(pd<=0)manifold.addPoint(v3x,v3y,v3z,-nx,-ny,-nz,pd,flip);
pd=nx*(v4x-ccx)+ny*(v4y-ccy)+nz*(v4z-ccz);
if(pd<=0)manifold.addPoint(v4x,v4y,v4z,-nx,-ny,-nz,pd,flip);
}
else{
switch(state){
case 1:
if(right1){
cbx=pbx+dwx;
cby=pby+dwy;
cbz=pbz+dwz;
nx=nwx;
ny=nwy;
nz=nwz;
}
else{
cbx=pbx-dwx;
cby=pby-dwy;
cbz=pbz-dwz;
nx=-nwx;
ny=-nwy;
nz=-nwz;
}
dir1x=nhx;
dir1y=nhy;
dir1z=nhz;
dir1l=bh;
dir2x=ndx;
dir2y=ndy;
dir2z=ndz;
dir2l=bd;
break;
case 2:
if(right2){
cbx=pbx+dhx;
cby=pby+dhy;
cbz=pbz+dhz;
nx=nhx;
ny=nhy;
nz=nhz;
}
else{
cbx=pbx-dhx;
cby=pby-dhy;
cbz=pbz-dhz;
nx=-nhx;
ny=-nhy;
nz=-nhz;
}
dir1x=nwx;
dir1y=nwy;
dir1z=nwz;
dir1l=bw;
dir2x=ndx;
dir2y=ndy;
dir2z=ndz;
dir2l=bd;
break;
case 3:
if(right3){
cbx=pbx+ddx;
cby=pby+ddy;
cbz=pbz+ddz;
nx=ndx;
ny=ndy;
nz=ndz;
}
else{
cbx=pbx-ddx;
cby=pby-ddy;
cbz=pbz-ddz;
nx=-ndx;
ny=-ndy;
nz=-ndz;
}
dir1x=nwx;
dir1y=nwy;
dir1z=nwz;
dir1l=bw;
dir2x=nhx;
dir2y=nhy;
dir2z=nhz;
dir2l=bh;
break;
}
dot=nx*ncx+ny*ncy+nz*ncz;
if(dot<0){len=ch;}
else{ len=-ch;}
ccx=pcx+len*ncx;
ccy=pcy+len*ncy;
ccz=pcz+len*ncz;
if(dotc>=0.999999){
tx=-ny;
ty=nz;
tz=nx;
}
else{
tx=nx;
ty=ny;
tz=nz;
}
len=tx*ncx+ty*ncy+tz*ncz;
dx=len*ncx-tx;
dy=len*ncy-ty;
dz=len*ncz-tz;
len=math.sqrt(dx*dx+dy*dy+dz*dz);
if(len==0)return;
len=r/len;
dx*=len;
dy*=len;
dz*=len;
tx=ccx+dx;
ty=ccy+dy;
tz=ccz+dz;
if(dot<-0.96||dot>0.96){
r00=ncx*ncx*1.5-0.5;
r01=ncx*ncy*1.5-ncz*0.866025403;
r02=ncx*ncz*1.5+ncy*0.866025403;
r10=ncy*ncx*1.5+ncz*0.866025403;
r11=ncy*ncy*1.5-0.5;
r12=ncy*ncz*1.5-ncx*0.866025403;
r20=ncz*ncx*1.5-ncy*0.866025403;
r21=ncz*ncy*1.5+ncx*0.866025403;
r22=ncz*ncz*1.5-0.5;
px=tx;
py=ty;
pz=tz;
pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);
tx=px-pd*nx-cbx;
ty=py-pd*ny-cby;
tz=pz-pd*nz-cbz;
sd=dir1x*tx+dir1y*ty+dir1z*tz;
ed=dir2x*tx+dir2y*ty+dir2z*tz;
if(sd<-dir1l){sd=-dir1l;}
else if(sd>dir1l){sd=dir1l;}
if(ed<-dir2l){ed=-dir2l;}
else if(ed>dir2l){ed=dir2l;}
tx=sd*dir1x+ed*dir2x;
ty=sd*dir1y+ed*dir2y;
tz=sd*dir1z+ed*dir2z;
px=cbx+tx;
py=cby+ty;
pz=cbz+tz;
manifold.addPoint(px,py,pz,nx,ny,nz,pd,flip);
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+ccx;
py=(dy=py)+ccy;
pz=(dz=pz)+ccz;
pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);
if(pd<=0){
tx=px-pd*nx-cbx;
ty=py-pd*ny-cby;
tz=pz-pd*nz-cbz;
sd=dir1x*tx+dir1y*ty+dir1z*tz;
ed=dir2x*tx+dir2y*ty+dir2z*tz;
if(sd<-dir1l){sd=-dir1l;}
else if(sd>dir1l){sd=dir1l;}
if(ed<-dir2l){ed=-dir2l;}
else if(ed>dir2l){ed=dir2l;}
tx=sd*dir1x+ed*dir2x;
ty=sd*dir1y+ed*dir2y;
tz=sd*dir1z+ed*dir2z;
px=cbx+tx;
py=cby+ty;
pz=cbz+tz;
//manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,2,0,false);
manifold.addPoint(px,py,pz,nx,ny,nz,pd,flip);
}
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+ccx;
py=(dy=py)+ccy;
pz=(dz=pz)+ccz;
pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);
if(pd<=0){
tx=px-pd*nx-cbx;
ty=py-pd*ny-cby;
tz=pz-pd*nz-cbz;
sd=dir1x*tx+dir1y*ty+dir1z*tz;
ed=dir2x*tx+dir2y*ty+dir2z*tz;
if(sd<-dir1l){sd=-dir1l;}
else if(sd>dir1l){sd=dir1l;}
if(ed<-dir2l){ed=-dir2l;}
else if(ed>dir2l){ed=dir2l;}
tx=sd*dir1x+ed*dir2x;
ty=sd*dir1y+ed*dir2y;
tz=sd*dir1z+ed*dir2z;
px=cbx+tx;
py=cby+ty;
pz=cbz+tz;
//manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,3,0,false);
manifold.addPoint(px,py,pz,nx,ny,nz,pd,flip);
}
}
else{
sx=tx;
sy=ty;
sz=tz;
sd=nx*(sx-cbx)+ny*(sy-cby)+nz*(sz-cbz);
sx-=sd*nx;
sy-=sd*ny;
sz-=sd*nz;
if(dot>0){
ex=tx+dcx*2;
ey=ty+dcy*2;
ez=tz+dcz*2;
}else{
ex=tx-dcx*2;
ey=ty-dcy*2;
ez=tz-dcz*2;
}
ed=nx*(ex-cbx)+ny*(ey-cby)+nz*(ez-cbz);
ex-=ed*nx;
ey-=ed*ny;
ez-=ed*nz;
d1x=sx-cbx;
d1y=sy-cby;
d1z=sz-cbz;
d2x=ex-cbx;
d2y=ey-cby;
d2z=ez-cbz;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z;
doth=d2x*dir1x+d2y*dir1y+d2z*dir1z;
dot1=dotw-dir1l;
dot2=doth-dir1l;
if(dot1>0){
if(dot2>0)return;
t1=dot1/(dot1-dot2);
sx=sx+tx*t1;
sy=sy+ty*t1;
sz=sz+tz*t1;
sd=sd+td*t1;
d1x=sx-cbx;
d1y=sy-cby;
d1z=sz-cbz;
dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
else if(dot2>0){
t1=dot1/(dot1-dot2);
ex=sx+tx*t1;
ey=sy+ty*t1;
ez=sz+tz*t1;
ed=sd+td*t1;
d2x=ex-cbx;
d2y=ey-cby;
d2z=ez-cbz;
doth=d2x*dir1x+d2y*dir1y+d2z*dir1z;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
dot1=dotw+dir1l;
dot2=doth+dir1l;
if(dot1<0){
if(dot2<0)return;
t1=dot1/(dot1-dot2);
sx=sx+tx*t1;
sy=sy+ty*t1;
sz=sz+tz*t1;
sd=sd+td*t1;
d1x=sx-cbx;
d1y=sy-cby;
d1z=sz-cbz;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
else if(dot2<0){
t1=dot1/(dot1-dot2);
ex=sx+tx*t1;
ey=sy+ty*t1;
ez=sz+tz*t1;
ed=sd+td*t1;
d2x=ex-cbx;
d2y=ey-cby;
d2z=ez-cbz;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z;
doth=d2x*dir2x+d2y*dir2y+d2z*dir2z;
dot1=dotw-dir2l;
dot2=doth-dir2l;
if(dot1>0){
if(dot2>0)return;
t1=dot1/(dot1-dot2);
sx=sx+tx*t1;
sy=sy+ty*t1;
sz=sz+tz*t1;
sd=sd+td*t1;
d1x=sx-cbx;
d1y=sy-cby;
d1z=sz-cbz;
dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
else if(dot2>0){
t1=dot1/(dot1-dot2);
ex=sx+tx*t1;
ey=sy+ty*t1;
ez=sz+tz*t1;
ed=sd+td*t1;
d2x=ex-cbx;
d2y=ey-cby;
d2z=ez-cbz;
doth=d2x*dir2x+d2y*dir2y+d2z*dir2z;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
td=ed-sd;
}
dot1=dotw+dir2l;
dot2=doth+dir2l;
if(dot1<0){
if(dot2<0)return;
t1=dot1/(dot1-dot2);
sx=sx+tx*t1;
sy=sy+ty*t1;
sz=sz+tz*t1;
sd=sd+td*t1;
}
else if(dot2<0){
t1=dot1/(dot1-dot2);
ex=sx+tx*t1;
ey=sy+ty*t1;
ez=sz+tz*t1;
ed=sd+td*t1;
}
if(sd<0){
//manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,b,c,1,0,false);
manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,flip);
}
if(ed<0){
//manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,b,c,4,0,false);
manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,flip);
}
}
}
}