detectCollision method
Detect collision of the shapes provided
Implementation
@override
void detectCollision(Shape shape1,Shape shape2, ContactManifold manifold){
Cylinder c1;
Cylinder c2;
if(shape1.id < shape2.id){
c1=shape1 as Cylinder;
c2=shape2 as Cylinder;
}
else{
c1=shape2 as Cylinder;
c2=shape1 as Cylinder;
}
final p1=c1.position;
final p2=c2.position;
final p1x=p1.x;
final p1y=p1.y;
final p1z=p1.z;
final p2x=p2.x;
final p2y=p2.y;
final p2z=p2.z;
final h1=c1.halfHeight;
final h2=c2.halfHeight;
final n1=c1.normalDirection;
final n2=c2.normalDirection;
final d1=c1.halfDirection;
final d2=c2.halfDirection;
final r1=c1.radius;
final r2=c2.radius;
final n1x=n1.x;
final n1y=n1.y;
final n1z=n1.z;
final n2x=n2.x;
final n2y=n2.y;
final n2z=n2.z;
final d1x=d1.x;
final d1y=d1.y;
final d1z=d1.z;
final d2x=d2.x;
final d2y=d2.y;
final d2z=d2.z;
double dx=p1x-p2x;
double dy=p1y-p2y;
double dz=p1z-p2z;
double len;
double c1x;
double c1y;
double c1z;
double c2x;
double c2y;
double c2z;
double tx;
double ty;
double tz;
double sx;
double sy;
double sz;
double ex;
double ey;
double ez;
double depth1;
double depth2;
double dot;
double t1;
double t2;
final sep=Vector3.zero();
final pos=Vector3.zero();
final dep=Vector3.zero();
if(!getSep(c1,c2,sep,pos,dep))return;
double dot1 = sep.x*n1x+sep.y*n1y+sep.z*n1z;
double dot2 = sep.x*n2x+sep.y*n2y+sep.z*n2z;
bool right1 = dot1 > 0;
bool right2 = dot2 > 0;
if(!right1){
dot1 = -dot1;
}
if(!right2){
dot2 = -dot2;
}
var state=0;
if(dot1 > 0.999 || dot2 > 0.999){
if(dot1 > dot2){
state=1;
}
else{
state=2;
}
}
double nx;
double ny;
double nz;
double depth=dep.x;
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 = 0;
double a;
double b;
double e;
double f;
nx=sep.x;
ny=sep.y;
nz=sep.z;
switch(state){
case 0:
manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,depth,false);
break;
case 1:
if(right1){
c1x=p1x+d1x;
c1y=p1y+d1y;
c1z=p1z+d1z;
nx=n1x;
ny=n1y;
nz=n1z;
}
else{
c1x=p1x-d1x;
c1y=p1y-d1y;
c1z=p1z-d1z;
nx=-n1x;
ny=-n1y;
nz=-n1z;
}
dot=nx*n2x+ny*n2y+nz*n2z;
if(dot<0){
len=h2;
}
else{
len=-h2;
}
c2x=p2x+len*n2x;
c2y=p2y+len*n2y;
c2z=p2z+len*n2z;
if(dot2>=0.999999){
tx=-ny;
ty=nz;
tz=nx;
}
else{
tx=nx;
ty=ny;
tz=nz;
}
len=tx*n2x+ty*n2y+tz*n2z;
dx=len*n2x-tx;
dy=len*n2y-ty;
dz=len*n2z-tz;
len=math.sqrt(dx*dx+dy*dy+dz*dz);
if(len==0)break;
len=r2/len;
dx*=len;
dy*=len;
dz*=len;
tx=c2x+dx;
ty=c2y+dy;
tz=c2z+dz;
if(dot<-0.96||dot>0.96){
r00=n2x*n2x*1.5-0.5;
r01=n2x*n2y*1.5-n2z*0.866025403;
r02=n2x*n2z*1.5+n2y*0.866025403;
r10=n2y*n2x*1.5+n2z*0.866025403;
r11=n2y*n2y*1.5-0.5;
r12=n2y*n2z*1.5-n2x*0.866025403;
r20=n2z*n2x*1.5-n2y*0.866025403;
r21=n2z*n2y*1.5+n2x*0.866025403;
r22=n2z*n2z*1.5-0.5;
px=tx;
py=ty;
pz=tz;
pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);
tx=px-pd*nx-c1x;
ty=py-pd*ny-c1y;
tz=pz-pd*nz-c1z;
len=tx*tx+ty*ty+tz*tz;
if(len>r1*r1){
len=r1/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c1x+tx;
py=c1y+ty;
pz=c1z+tz;
manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+c2x;
py=(dy=py)+c2y;
pz=(dz=pz)+c2z;
pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);
if(pd<=0){
tx=px-pd*nx-c1x;
ty=py-pd*ny-c1y;
tz=pz-pd*nz-c1z;
len=tx*tx+ty*ty+tz*tz;
if(len>r1*r1){
len=r1/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c1x+tx;
py=c1y+ty;
pz=c1z+tz;
manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);
}
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+c2x;
py=(dy=py)+c2y;
pz=(dz=pz)+c2z;
pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);
if(pd<=0){
tx=px-pd*nx-c1x;
ty=py-pd*ny-c1y;
tz=pz-pd*nz-c1z;
len=tx*tx+ty*ty+tz*tz;
if(len>r1*r1){
len=r1/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c1x+tx;
py=c1y+ty;
pz=c1z+tz;
manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);
}
}
else{
sx=tx;
sy=ty;
sz=tz;
depth1=nx*(sx-c1x)+ny*(sy-c1y)+nz*(sz-c1z);
sx-=depth1*nx;
sy-=depth1*ny;
sz-=depth1*nz;
if(dot>0){
ex=tx+n2x*h2*2;
ey=ty+n2y*h2*2;
ez=tz+n2z*h2*2;
}
else{
ex=tx-n2x*h2*2;
ey=ty-n2y*h2*2;
ez=tz-n2z*h2*2;
}
depth2=nx*(ex-c1x)+ny*(ey-c1y)+nz*(ez-c1z);
ex-=depth2*nx;
ey-=depth2*ny;
ez-=depth2*nz;
dx=c1x-sx;
dy=c1y-sy;
dz=c1z-sz;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
a=dx*dx+dy*dy+dz*dz;
b=dx*tx+dy*ty+dz*tz;
e=tx*tx+ty*ty+tz*tz;
f=b*b-e*(a-r1*r1);
if(f<0)break;
f=math.sqrt(f);
t1=(b+f)/e;
t2=(b-f)/e;
if(t2<t1){
len=t1;
t1=t2;
t2=len;
}
if(t2>1)t2=1;
if(t1<0)t1=0;
tx=sx+(ex-sx)*t1;
ty=sy+(ey-sy)*t1;
tz=sz+(ez-sz)*t1;
ex=sx+(ex-sx)*t2;
ey=sy+(ey-sy)*t2;
ez=sz+(ez-sz)*t2;
sx=tx;
sy=ty;
sz=tz;
len=depth1+(depth2-depth1)*t1;
depth2=depth1+(depth2-depth1)*t2;
depth1=len;
if(depth1<0){
manifold.addPoint(sx,sy,sz,nx,ny,nz,pd,false);
}
if(depth2<0){
manifold.addPoint(ex,ey,ez,nx,ny,nz,pd,false);
}
}
break;
case 2:
if(right2){
c2x=p2x-d2x;
c2y=p2y-d2y;
c2z=p2z-d2z;
nx=-n2x;
ny=-n2y;
nz=-n2z;
}
else{
c2x=p2x+d2x;
c2y=p2y+d2y;
c2z=p2z+d2z;
nx=n2x;
ny=n2y;
nz=n2z;
}
dot=nx*n1x+ny*n1y+nz*n1z;
if(dot<0){
len=h1;
}
else{
len=-h1;
}
c1x=p1x+len*n1x;
c1y=p1y+len*n1y;
c1z=p1z+len*n1z;
if(dot1>=0.999999){
tx=-ny;
ty=nz;
tz=nx;
}
else{
tx=nx;
ty=ny;
tz=nz;
}
len=tx*n1x+ty*n1y+tz*n1z;
dx=len*n1x-tx;
dy=len*n1y-ty;
dz=len*n1z-tz;
len=math.sqrt(dx*dx+dy*dy+dz*dz);
if(len==0)break;
len=r1/len;
dx*=len;
dy*=len;
dz*=len;
tx=c1x+dx;
ty=c1y+dy;
tz=c1z+dz;
if(dot<-0.96||dot>0.96){
r00=n1x*n1x*1.5-0.5;
r01=n1x*n1y*1.5-n1z*0.866025403;
r02=n1x*n1z*1.5+n1y*0.866025403;
r10=n1y*n1x*1.5+n1z*0.866025403;
r11=n1y*n1y*1.5-0.5;
r12=n1y*n1z*1.5-n1x*0.866025403;
r20=n1z*n1x*1.5-n1y*0.866025403;
r21=n1z*n1y*1.5+n1x*0.866025403;
r22=n1z*n1z*1.5-0.5;
px=tx;
py=ty;
pz=tz;
pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);
tx=px-pd*nx-c2x;
ty=py-pd*ny-c2y;
tz=pz-pd*nz-c2z;
len=tx*tx+ty*ty+tz*tz;
if(len>r2*r2){
len=r2/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c2x+tx;
py=c2y+ty;
pz=c2z+tz;
manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+c1x;
py=(dy=py)+c1y;
pz=(dz=pz)+c1z;
pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);
if(pd<=0){
tx=px-pd*nx-c2x;
ty=py-pd*ny-c2y;
tz=pz-pd*nz-c2z;
len=tx*tx+ty*ty+tz*tz;
if(len>r2*r2){
len=r2/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c2x+tx;
py=c2y+ty;
pz=c2z+tz;
manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);
}
px=dx*r00+dy*r01+dz*r02;
py=dx*r10+dy*r11+dz*r12;
pz=dx*r20+dy*r21+dz*r22;
px=(dx=px)+c1x;
py=(dy=py)+c1y;
pz=(dz=pz)+c1z;
pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);
if(pd<=0){
tx=px-pd*nx-c2x;
ty=py-pd*ny-c2y;
tz=pz-pd*nz-c2z;
len=tx*tx+ty*ty+tz*tz;
if(len>r2*r2){
len=r2/math.sqrt(len);
tx*=len;
ty*=len;
tz*=len;
}
px=c2x+tx;
py=c2y+ty;
pz=c2z+tz;
manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);
}
}
else{
sx=tx;
sy=ty;
sz=tz;
depth1=nx*(sx-c2x)+ny*(sy-c2y)+nz*(sz-c2z);
sx-=depth1*nx;
sy-=depth1*ny;
sz-=depth1*nz;
if(dot>0){
ex=tx+n1x*h1*2;
ey=ty+n1y*h1*2;
ez=tz+n1z*h1*2;
}
else{
ex=tx-n1x*h1*2;
ey=ty-n1y*h1*2;
ez=tz-n1z*h1*2;
}
depth2=nx*(ex-c2x)+ny*(ey-c2y)+nz*(ez-c2z);
ex-=depth2*nx;
ey-=depth2*ny;
ez-=depth2*nz;
dx=c2x-sx;
dy=c2y-sy;
dz=c2z-sz;
tx=ex-sx;
ty=ey-sy;
tz=ez-sz;
a=dx*dx+dy*dy+dz*dz;
b=dx*tx+dy*ty+dz*tz;
e=tx*tx+ty*ty+tz*tz;
f=b*b-e*(a-r2*r2);
if(f<0)break;
f=math.sqrt(f);
t1=(b+f)/e;
t2=(b-f)/e;
if(t2<t1){
len=t1;
t1=t2;
t2=len;
}
if(t2>1)t2=1;
if(t1<0)t1=0;
tx=sx+(ex-sx)*t1;
ty=sy+(ey-sy)*t1;
tz=sz+(ez-sz)*t1;
ex=sx+(ex-sx)*t2;
ey=sy+(ey-sy)*t2;
ez=sz+(ez-sz)*t2;
sx=tx;
sy=ty;
sz=tz;
len=depth1+(depth2-depth1)*t1;
depth2=depth1+(depth2-depth1)*t2;
depth1=len;
if(depth1<0){
manifold.addPoint(sx,sy,sz,-nx,-ny,-nz,depth1,false);
}
if(depth2<0){
manifold.addPoint(ex,ey,ez,-nx,-ny,-nz,depth2,false);
}
}
break;
}
}