qboolean tri_tri_intersect( vector3 *V0, vector3 *V1, vector3 *V2, vector3 *U0, vector3 *U1, vector3 *U2 ) {
vector3 E1, E2;
vector3 N1, N2;
float d1, d2;
float du0, du1, du2, dv0, dv1, dv2;
vector3 D;
float isect1[2], isect2[2];
float du0du1, du0du2, dv0dv1, dv0dv2;
short index;
float vp0, vp1, vp2;
float up0, up1, up2;
float b, c, max;
int tmp;
/* compute plane equation of triangle(V0,V1,V2) */
SUB( &E1, V1, V0 );
SUB( &E2, V2, V0 );
CROSS( &N1, &E1, &E2 );
d1 = -DOT( &N1, V0 );
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0 = DOT( &N1, U0 ) + d1;
du1 = DOT( &N1, U1 ) + d1;
du2 = DOT( &N1, U2 ) + d1;
/* coplanarity robustness check */
#if USE_EPSILON_TEST
if ( fabsf( du0 ) < EPSILON ) du0 = 0.0f;
if ( fabsf( du1 ) < EPSILON ) du1 = 0.0f;
if ( fabsf( du2 ) < EPSILON ) du2 = 0.0f;
#endif
du0du1 = du0*du1;
du0du2 = du0*du2;
if ( du0du1 > 0.0f && du0du2 > 0.0f ) // same sign on all of them + not equal 0 ?
return qfalse; // no intersection occurs
/* compute plane of triangle (U0,U1,U2) */
SUB( &E1, U1, U0 );
SUB( &E2, U2, U0 );
CROSS( &N2, &E1, &E2 );
d2 = -DOT( &N2, U0 );
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0 = DOT( &N2, V0 ) + d2;
dv1 = DOT( &N2, V1 ) + d2;
dv2 = DOT( &N2, V2 ) + d2;
#if USE_EPSILON_TEST
if ( fabsf( dv0 ) < EPSILON ) dv0 = 0.0f;
if ( fabsf( dv1 ) < EPSILON ) dv1 = 0.0f;
if ( fabsf( dv2 ) < EPSILON ) dv2 = 0.0f;
#endif
dv0dv1 = dv0*dv1;
dv0dv2 = dv0*dv2;
if ( dv0dv1 > 0.0f && dv0dv2 > 0.0f ) // same sign on all of them + not equal 0 ?
return qfalse; // no intersection occurs
/* compute direction of intersection line */
CROSS( &D, &N1, &N2 );
/* compute and index to the largest component of D */
max = fabsf( D.x );
index = 0;
b = fabsf( D.y );
c = fabsf( D.z );
if ( b > max ) {
max = b;
index = 1;
}
if ( c > max ) {
max = c;
index = 2;
}
/* this is the simplified projection onto L*/
vp0 = V0->data[index];
vp1 = V1->data[index];
vp2 = V2->data[index];
up0 = U0->data[index];
up1 = U1->data[index];
up2 = U2->data[index];
/* compute interval for triangle 1 */
tmp = COMPUTE_INTERVALS( vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, &isect1[0], &isect1[1], &N1, V0, V1, V2, U0, U1, U2 );
if ( tmp != -1 ) return (qboolean)tmp;
/* compute interval for triangle 2 */
tmp = COMPUTE_INTERVALS( up0, up1, up2, du0, du1, du2, du0du1, du0du2, &isect2[0], &isect2[1], &N1, V0, V1, V2, U0, U1, U2 );
if ( tmp != -1 ) return (qboolean)tmp;
// sort
if ( isect1[0] > isect1[1] ) {
float c = isect1[0];
isect1[0] = isect1[1];
isect1[1] = c;
}
if ( isect2[0] > isect2[1] ) {
float c = isect2[0];
isect2[0] = isect2[1];
isect2[1] = c;
}
if ( isect1[1] < isect2[0] ||
isect2[1] < isect1[0] )
return qtrue;
return qfalse;
}
qboolean tri_tri_intersect(vec3_t V0,vec3_t V1,vec3_t V2,
vec3_t U0,vec3_t U1,vec3_t U2)
{
vec3_t E1,E2;
vec3_t N1,N2;
float d1,d2;
float du0,du1,du2,dv0,dv1,dv2;
vec3_t D;
float isect1[2], isect2[2];
float du0du1,du0du2,dv0dv1,dv0dv2;
short index;
float vp0,vp1,vp2;
float up0,up1,up2;
float b,c,max;
/* compute plane equation of triangle(V0,V1,V2) */
SUB(E1,V1,V0);
SUB(E2,V2,V0);
CROSS(N1,E1,E2);
d1=-DOT(N1,V0);
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0=DOT(N1,U0)+d1;
du1=DOT(N1,U1)+d1;
du2=DOT(N1,U2)+d1;
/* coplanarity robustness check */
#if USE_EPSILON_TEST
if(fabs(du0)<EPSILON) du0=0.0;
if(fabs(du1)<EPSILON) du1=0.0;
if(fabs(du2)<EPSILON) du2=0.0;
#endif
du0du1=du0*du1;
du0du2=du0*du2;
if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute plane of triangle (U0,U1,U2) */
SUB(E1,U1,U0);
SUB(E2,U2,U0);
CROSS(N2,E1,E2);
d2=-DOT(N2,U0);
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0=DOT(N2,V0)+d2;
dv1=DOT(N2,V1)+d2;
dv2=DOT(N2,V2)+d2;
#if USE_EPSILON_TEST
if(fabs(dv0)<EPSILON) dv0=0.0;
if(fabs(dv1)<EPSILON) dv1=0.0;
if(fabs(dv2)<EPSILON) dv2=0.0;
#endif
dv0dv1=dv0*dv1;
dv0dv2=dv0*dv2;
if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute direction of intersection line */
CROSS(D,N1,N2);
/* compute and index to the largest component of D */
max=fabs(D[0]);
index=0;
b=fabs(D[1]);
c=fabs(D[2]);
if(b>max) max=b,index=1;
if(c>max) max=c,index=2;
/* this is the simplified projection onto L*/
vp0=V0[index];
vp1=V1[index];
vp2=V2[index];
up0=U0[index];
up1=U1[index];
up2=U2[index];
/* compute interval for triangle 1 */
COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
/* compute interval for triangle 2 */
COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
SORT(isect1[0],isect1[1]);
SORT(isect2[0],isect2[1]);
if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return qtrue;
return qfalse;
}
inline int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
float U0[3],float U1[3],float U2[3], float res1[3], float res2[3])
{
float E1[3],E2[3];
float N1[3],N2[3],d1,d2;
float du0,du1,du2,dv0,dv1,dv2;
float D[3];
float isect1[2], isect2[2];
float du0du1,du0du2,dv0dv1,dv0dv2,du1du2,dv1dv2;
short index;
float vp0,vp1,vp2;
float up0,up1,up2;
float b,c,max;
/* compute plane equation of triangle(V0,V1,V2) */
SUB(E1,V1,V0);
SUB(E2,V2,V0);
CROSS(N1,E1,E2);
d1=-DOT(N1,V0);
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0=DOT(N1,U0)+d1;
du1=DOT(N1,U1)+d1;
du2=DOT(N1,U2)+d1;
/* coplanarity robustness check */
#if USE_EPSILON_TEST==TRUE
if(fabs(du0)<EPSILON) du0=0.0;
if(fabs(du1)<EPSILON) du1=0.0;
if(fabs(du2)<EPSILON) du2=0.0;
if (du1 == 0 && du2 == 0 && fabs(du0) < 1e-4)
du0 = 0.0;
if (du0 == 0 && du2 == 0 && fabs(du1) < 1e-4)
du1 = 0.0;
if (du0 == 0 && du1 == 0 && fabs(du2) < 1e-4)
du2 = 0.0;
#endif
du0du1=du0*du1;
du0du2=du0*du2;
du1du2=du1*du2;
if(du0du1>0.0f && du0du2>0.0f) { /* same sign on all of them + not equal 0 ? */
return NO_CONTACT; /* no intersection occurs */
}
/* compute plane of triangle (U0,U1,U2) */
SUB(E1,U1,U0);
SUB(E2,U2,U0);
CROSS(N2,E1,E2);
d2=-DOT(N2,U0);
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0=DOT(N2,V0)+d2;
dv1=DOT(N2,V1)+d2;
dv2=DOT(N2,V2)+d2;
#if USE_EPSILON_TEST==TRUE
if(fabs(dv0)<EPSILON) dv0=0.0;
if(fabs(dv1)<EPSILON) dv1=0.0;
if(fabs(dv2)<EPSILON) dv2=0.0;
if (dv1 == 0 && dv2 == 0 && fabs(dv0) < 1e-5)
dv0 = 0.0;
if (dv0 == 0 && dv2 == 0 && fabs(dv1) < 1e-5)
dv1 = 0.0;
if (dv0 == 0 && dv1 == 0 && fabs(dv2) < 1e-5)
dv2 = 0.0;
#endif
dv0dv1=dv0*dv1;
dv0dv2=dv0*dv2;
dv1dv2=dv1*dv2;
if(dv0dv1>0.0f && dv0dv2>0.0f) { /* same sign on all of them + not equal 0 ? */
return NO_CONTACT; /* no intersection occurs */
}
/* compute direction of intersection line */
CROSS(D,N1,N2);
/* compute and index to the largest component of D */
max=fabs(D[0]);
index=0;
b=fabs(D[1]);
c=fabs(D[2]);
if(b>max) max=b,index=1;
if(c>max) max=c,index=2;
/* this is the simplified projection onto L*/
vp0=V0[index];
vp1=V1[index];
vp2=V2[index];
up0=U0[index];
up1=U1[index];
up2=U2[index];
/* compute interval for triangle 1 */
COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
/* compute interval for triangle 2 */
COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
SORT(isect1[0],isect1[1]);
SORT(isect2[0],isect2[1]);
//if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return NO_CONTACT;
float res[4][3];
if(du0du1>0)
{
edge_tri_intersect(U2, U0, du2, du0, res[0]);
edge_tri_intersect(U2, U1, du2, du1, res[1]);
}
else if(du0du2>0)
{
edge_tri_intersect(U1, U0, du1, du0, res[0]);
edge_tri_intersect(U1, U2, du1, du2, res[1]);
}
else if(du1du2>0)
{
edge_tri_intersect(U0, U1, du0, du1, res[0]);
edge_tri_intersect(U0, U2, du0, du2, res[1]);
}
else if(du0==0)
{
SET(res[0], U0);
edge_tri_intersect(U1, U2, du1, du2, res[1]);
}
else if(du1==0)
{
SET(res[0], U1);
edge_tri_intersect(U0, U2, du0, du2, res[1]);
}
else if(du2==0)
{
SET(res[0], U2);
edge_tri_intersect(U0, U1, du0, du1, res[1]);
}
else
{
std::cerr << "contact error" << std::endl;
}
if(dv0dv1>0)
{
edge_tri_intersect(V2, V0, dv2, dv0, res[2]);
edge_tri_intersect(V2, V1, dv2, dv1, res[3]);
}
else if(dv0dv2>0)
{
edge_tri_intersect(V1, V0, dv1, dv0, res[2]);
edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
}
else if(dv1dv2>0)
{
edge_tri_intersect(V0, V1, dv0, dv1, res[2]);
edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
}
else if(dv0==0)
{
SET(res[2], V0);
edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
}
else if(dv1==0)
{
SET(res[2], V1);
edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
}
else if(dv2==0)
{
SET(res[2], V2);
edge_tri_intersect(V0, V1, dv0, dv1, res[3]);
}
else
{
std::cerr << "contact error" << std::endl;
}
for (int i=3;i>0;i--)
for (int j=0;j<i;j++)
{
if(res[j][index]>res[j+1][index])
{
for (int k=0;k<3;k++)
SWAP(res[j][k], res[j+1][k]);
}
}
SET(res1, res[1]);
SET(res2, res[2]);
return 1;
}
