getSep method
Should reimpliment sep
Implementation
bool getSep(Cylinder c1,Cylinder c2,Vector3 sep,Vector3 pos,Vector3 dep){
double t1x;
double t1y;
double t1z;
double t2x;
double t2y;
double t2z;
final sup = Vector3.zero();
double len;
double p1x = 0;
double p1y = 0;
double p1z = 0;
double p2x = 0;
double p2y = 0;
double p2z = 0;
final v01x=c1.position.x;
final v01y=c1.position.y;
final v01z=c1.position.z;
final v02x=c2.position.x;
final v02y=c2.position.y;
final v02z=c2.position.z;
double v0x=v02x-v01x;
double v0y=v02y-v01y;
double v0z=v02z-v01z;
if(v0x*v0x+v0y*v0y+v0z*v0z==0){
v0y=0.001;
}
double nx=-v0x;
double ny=-v0y;
double nz=-v0z;
supportPoint(c1,-nx,-ny,-nz,sup);
double v11x=sup.x;
double v11y=sup.y;
double v11z=sup.z;
supportPoint(c2,nx,ny,nz,sup);
double v12x=sup.x;
double v12y=sup.y;
double v12z=sup.z;
double v1x=v12x-v11x;
double v1y=v12y-v11y;
double v1z=v12z-v11z;
if(v1x*nx+v1y*ny+v1z*nz<=0){
return false;
}
nx=v1y*v0z-v1z*v0y;
ny=v1z*v0x-v1x*v0z;
nz=v1x*v0y-v1y*v0x;
if(nx*nx+ny*ny+nz*nz==0){
sep..setValues( v1x-v0x, v1y-v0y, v1z-v0z )..normalize();
pos.setValues( (v11x+v12x)*0.5, (v11y+v12y)*0.5, (v11z+v12z)*0.5 );
return true;
}
supportPoint(c1,-nx,-ny,-nz,sup);
double v21x=sup.x;
double v21y=sup.y;
double v21z=sup.z;
supportPoint(c2,nx,ny,nz,sup);
double v22x=sup.x;
double v22y=sup.y;
double v22z=sup.z;
double v2x=v22x-v21x;
double v2y=v22y-v21y;
double v2z=v22z-v21z;
if(v2x*nx+v2y*ny+v2z*nz<=0){
return false;
}
t1x=v1x-v0x;
t1y=v1y-v0y;
t1z=v1z-v0z;
t2x=v2x-v0x;
t2y=v2y-v0y;
t2z=v2z-v0z;
nx=t1y*t2z-t1z*t2y;
ny=t1z*t2x-t1x*t2z;
nz=t1x*t2y-t1y*t2x;
if(nx*v0x+ny*v0y+nz*v0z>0){
t1x=v1x;
t1y=v1y;
t1z=v1z;
v1x=v2x;
v1y=v2y;
v1z=v2z;
v2x=t1x;
v2y=t1y;
v2z=t1z;
t1x=v11x;
t1y=v11y;
t1z=v11z;
v11x=v21x;
v11y=v21y;
v11z=v21z;
v21x=t1x;
v21y=t1y;
v21z=t1z;
t1x=v12x;
t1y=v12y;
t1z=v12z;
v12x=v22x;
v12y=v22y;
v12z=v22z;
v22x=t1x;
v22y=t1y;
v22z=t1z;
nx=-nx;
ny=-ny;
nz=-nz;
}
int iterations=0;
while(true){
if(iterations++ > 100){
return false;
}
supportPoint(c1,-nx,-ny,-nz,sup);
supportPoint(c2,nx,ny,nz,sup);
double v31x=sup.x;
double v31y=sup.y;
double v31z=sup.z;
double v32x=sup.x;
double v32y=sup.y;
double v32z=sup.z;
double v3x=v32x-v31x;
double v3y=v32y-v31y;
double v3z=v32z-v31z;
if(v3x*nx+v3y*ny+v3z*nz<=0){
return false;
}
if((v1y*v3z-v1z*v3y)*v0x+(v1z*v3x-v1x*v3z)*v0y+(v1x*v3y-v1y*v3x)*v0z<0){
v2x=v3x;
v2y=v3y;
v2z=v3z;
v21x=v31x;
v21y=v31y;
v21z=v31z;
v22x=v32x;
v22y=v32y;
v22z=v32z;
t1x=v1x-v0x;
t1y=v1y-v0y;
t1z=v1z-v0z;
t2x=v3x-v0x;
t2y=v3y-v0y;
t2z=v3z-v0z;
nx=t1y*t2z-t1z*t2y;
ny=t1z*t2x-t1x*t2z;
nz=t1x*t2y-t1y*t2x;
continue;
}
if((v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z<0){
v1x=v3x;
v1y=v3y;
v1z=v3z;
v11x=v31x;
v11y=v31y;
v11z=v31z;
v12x=v32x;
v12y=v32y;
v12z=v32z;
t1x=v3x-v0x;
t1y=v3y-v0y;
t1z=v3z-v0z;
t2x=v2x-v0x;
t2y=v2y-v0y;
t2z=v2z-v0z;
nx=t1y*t2z-t1z*t2y;
ny=t1z*t2x-t1x*t2z;
nz=t1x*t2y-t1y*t2x;
continue;
}
bool hit=false;
while(true){
t1x=v2x-v1x;
t1y=v2y-v1y;
t1z=v2z-v1z;
t2x=v3x-v1x;
t2y=v3y-v1y;
t2z=v3z-v1z;
nx=t1y*t2z-t1z*t2y;
ny=t1z*t2x-t1x*t2z;
nz=t1x*t2y-t1y*t2x;
len=1/math.sqrt(nx*nx+ny*ny+nz*nz);
nx*=len;
ny*=len;
nz*=len;
if(nx*v1x+ny*v1y+nz*v1z>=0&&!hit){
double b0=(v1y*v2z-v1z*v2y)*v3x+(v1z*v2x-v1x*v2z)*v3y+(v1x*v2y-v1y*v2x)*v3z;
double b1=(v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z;
double b2=(v0y*v1z-v0z*v1y)*v3x+(v0z*v1x-v0x*v1z)*v3y+(v0x*v1y-v0y*v1x)*v3z;
double b3=(v2y*v1z-v2z*v1y)*v0x+(v2z*v1x-v2x*v1z)*v0y+(v2x*v1y-v2y*v1x)*v0z;
double sum=b0+b1+b2+b3;
if(sum<=0){
b0=0;
b1=(v2y*v3z-v2z*v3y)*nx+(v2z*v3x-v2x*v3z)*ny+(v2x*v3y-v2y*v3x)*nz;
b2=(v3y*v2z-v3z*v2y)*nx+(v3z*v2x-v3x*v2z)*ny+(v3x*v2y-v3y*v2x)*nz;
b3=(v1y*v2z-v1z*v2y)*nx+(v1z*v2x-v1x*v2z)*ny+(v1x*v2y-v1y*v2x)*nz;
sum=b1+b2+b3;
}
double inv=1/sum;
p1x=(v01x*b0+v11x*b1+v21x*b2+v31x*b3)*inv;
p1y=(v01y*b0+v11y*b1+v21y*b2+v31y*b3)*inv;
p1z=(v01z*b0+v11z*b1+v21z*b2+v31z*b3)*inv;
p2x=(v02x*b0+v12x*b1+v22x*b2+v32x*b3)*inv;
p2y=(v02y*b0+v12y*b1+v22y*b2+v32y*b3)*inv;
p2z=(v02z*b0+v12z*b1+v22z*b2+v32z*b3)*inv;
hit=true;
}
supportPoint(c1,-nx,-ny,-nz,sup);
supportPoint(c2,nx,ny,nz,sup);
double v41x=sup.x;
double v41y=sup.y;
double v41z=sup.z;
double v42x=sup.x;
double v42y=sup.y;
double v42z=sup.z;
double v4x=v42x-v41x;
double v4y=v42y-v41y;
double v4z=v42z-v41z;
double separation=-(v4x*nx+v4y*ny+v4z*nz);
if((v4x-v3x)*nx+(v4y-v3y)*ny+(v4z-v3z)*nz<=0.01||separation>=0){
if(hit){
sep.setValues( -nx, -ny, -nz );
pos.setValues( (p1x+p2x)*0.5, (p1y+p2y)*0.5, (p1z+p2z)*0.5 );
dep.x=separation;
return true;
}
return false;
}
if(
(v4y*v1z-v4z*v1y)*v0x+
(v4z*v1x-v4x*v1z)*v0y+
(v4x*v1y-v4y*v1x)*v0z<0
){
if(
(v4y*v2z-v4z*v2y)*v0x+
(v4z*v2x-v4x*v2z)*v0y+
(v4x*v2y-v4y*v2x)*v0z<0
){
v1x=v4x;
v1y=v4y;
v1z=v4z;
v11x=v41x;
v11y=v41y;
v11z=v41z;
v12x=v42x;
v12y=v42y;
v12z=v42z;
}
else{
v3x=v4x;
v3y=v4y;
v3z=v4z;
v31x=v41x;
v31y=v41y;
v31z=v41z;
v32x=v42x;
v32y=v42y;
v32z=v42z;
}
}
else{
if(
(v4y*v3z-v4z*v3y)*v0x+
(v4z*v3x-v4x*v3z)*v0y+
(v4x*v3y-v4y*v3x)*v0z<0
){
v2x=v4x;
v2y=v4y;
v2z=v4z;
v21x=v41x;
v21y=v41y;
v21z=v41z;
v22x=v42x;
v22y=v42y;
v22z=v42z;
}
else{
v1x=v4x;
v1y=v4y;
v1z=v4z;
v11x=v41x;
v11y=v41y;
v11z=v41z;
v12x=v42x;
v12y=v42y;
v12z=v42z;
}
}
}
}
}