duyle2408's picture
upload data
f5f315c verified
Raw
History Blame Contribute Delete
24.5 kB
/* Triangle/triangle intersection test routine,
* by Tomas Moller, 1997.
* See article "A Fast Triangle-Triangle Intersection Test",
* Journal of Graphics Tools, 2(2), 1997
* updated: 2001-06-20 (added line of intersection)
*
* int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
* float U0[3],float U1[3],float U2[3])
*
* parameters: vertices of triangle 1: V0,V1,V2
* vertices of triangle 2: U0,U1,U2
* result : returns 1 if the triangles intersect, otherwise 0
*
* Here is a version withouts divisions (a little faster)
* int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
* float U0[3],float U1[3],float U2[3]);
*
* This version computes the line of intersection as well (if they are not
*coplanar): int tri_tri_intersect_with_isectline(float V0[3],float V1[3],float
*V2[3], float U0[3],float U1[3],float U2[3],int *coplanar, float
*isect_point1[3],float isect_point2[3]); coplanar returns whether the tris are
*coplanar isect_point1, isect_point2 are the endpoints of the line of
*intersection
*/
#include <math.h>
#include "defs.h"
#include "double_vec_ops.hpp"
#include "helper_math.h"
#define FABS(x) ((float)fabs(x)) /* implement as is fastest on your machine */
/* if USE_EPSILON_TEST is true then we do a check:
if |dv|<EPSILON then dv=0.0;
else no check is done (which is less robust)
*/
#define USE_EPSILON_TEST TRUE
#define EPSILON 0.000001
template <typename T> __host__ __device__ inline void sort(T *a, T *b) {
if (a > b) {
T c;
c = *a;
*a = *b;
b = *c;
}
return;
}
template <typename T> __host__ __device__ inline int sort(T *a, T *b) {
if (a > b) {
T c;
c = *a;
*a = *b;
b = *c;
return 1;
} else
return 0;
}
#define ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1) \
isect0 = VV0 + (VV1 - VV0) * D0 / (D0 - D1); \
isect1 = VV0 + (VV2 - VV0) * D0 / (D0 - D2);
#define COMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, \
isect1) \
if (D0D1 > 0.0f) { \
/* here we know that D0D2<=0.0 */ \
/* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
} else if (D0D2 > 0.0f) { \
/* here we know that d0d1<=0.0 */ \
ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
} else if (D1 * D2 > 0.0f || D0 != 0.0f) { \
/* here we know that d0d1<=0.0 or that D0!=0.0 */ \
ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1); \
} else if (D1 != 0.0f) { \
ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
} else if (D2 != 0.0f) { \
ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
} else { \
/* triangles are coplanar */ \
return coplanar_tri_tri(N1, V0, V1, V2, U0, U1, U2); \
}
template <typename T>
inline void compute_intervals(
)
/* this edge to edge test is based on Franlin Antonio's gem:
"Faster Line Segment Intersection", in Graphics Gems III,
pp. 199-202 */
#define EDGE_EDGE_TEST(V0, U0, U1) \
Bx = U0[i0] - U1[i0]; \
By = U0[i1] - U1[i1]; \
Cx = V0[i0] - U0[i0]; \
Cy = V0[i1] - U0[i1]; \
f = Ay * Bx - Ax * By; \
d = By * Cx - Bx * Cy; \
if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f)) { \
e = Ax * Cy - Ay * Cx; \
if (f > 0) { \
if (e >= 0 && e <= f) \
return 1; \
} else { \
if (e <= 0 && e >= f) \
return 1; \
} \
}
#define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2) \
{ \
float Ax, Ay, Bx, By, Cx, Cy, e, d, f; \
Ax = V1[i0] - V0[i0]; \
Ay = V1[i1] - V0[i1]; \
/* test edge U0,U1 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U0, U1); \
/* test edge U1,U2 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U1, U2); \
/* test edge U2,U1 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U2, U0); \
}
#define POINT_IN_TRI(V0, U0, U1, U2) \
{ \
float a, b, c, d0, d1, d2; \
/* is T1 completly inside T2? */ \
/* check if V0 is inside tri(U0,U1,U2) */ \
a = U1[i1] - U0[i1]; \
b = -(U1[i0] - U0[i0]); \
c = -a * U0[i0] - b * U0[i1]; \
d0 = a * V0[i0] + b * V0[i1] + c; \
\
a = U2[i1] - U1[i1]; \
b = -(U2[i0] - U1[i0]); \
c = -a * U1[i0] - b * U1[i1]; \
d1 = a * V0[i0] + b * V0[i1] + c; \
\
a = U0[i1] - U2[i1]; \
b = -(U0[i0] - U2[i0]); \
c = -a * U2[i0] - b * U2[i1]; \
d2 = a * V0[i0] + b * V0[i1] + c; \
if (d0 * d1 > 0.0) { \
if (d0 * d2 > 0.0) \
return 1; \
} \
}
template <typename T>
__host__ __device__
bool point_in_tri(vec3<T> V0, vec3<T>U0, vec3<T>U1, vec3<T>U2)
{
T a, b, c, d0, d1, d2;
/* is T1 completly inside T2? */
/* check if V0 is inside tri(U0,U1,U2) */
a = U1[i1] - U0[i1];
b = -(U1[i0] - U0[i0]);
c = -a * U0[i0] - b * U0[i1];
d0 = a * V0[i0] + b * V0[i1] + c;
a = U2[i1] - U1[i1];
b = -(U2[i0] - U1[i0]);
c = -a * U1[i0] - b * U1[i1];
d1 = a * V0[i0] + b * V0[i1] + c;
a = U0[i1] - U2[i1];
b = -(U0[i0] - U2[i0]);
c = -a * U2[i0] - b * U2[i1];
d2 = a * V0[i0] + b * V0[i1] + c;
if (d0 * d1 > 0.0) {
if (d0 * d2 > 0.0)
return true;
}
return false
}
template <typename T>
__host__ __device__
bool coplanar_tri_tri(
vec3<T> N, vec3<T> V0, vec3<T> V1, vec3<T> V2,
vec3<T> U0, vec3<T> U1, vec3<T> U2) {
vec3<T> A;
short i0, i1;
/* first project onto an axis-aligned plane, that maximizes the area */
/* of the triangles, compute indices: i0,i1. */
A.x = fabs(N.x);
A.y = fabs(N.y);
A.z = fabs(N.z);
if (A.x > A.y) {
if (A.x > A.z) {
i0 = 1; /* A[0] is greatest */
i1 = 2;
} else {
i0 = 0; /* A[2] is greatest */
i1 = 1;
}
} else /* A[0]<=A[1] */
{
if (A.z > A.y) {
i0 = 0; /* A[2] is greatest */
i1 = 1;
} else {
i0 = 0; /* A[1] is greatest */
i1 = 2;
}
}
/* test all edges of triangle 1 against the edges of triangle 2 */
EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2);
EDGE_AGAINST_TRI_EDGES(V1, V2, U0, U1, U2);
EDGE_AGAINST_TRI_EDGES(V2, V0, U0, U1, U2);
/* finally, test if tri1 is totally contained in tri2 or vice versa */
POINT_IN_TRI(V0, U0, U1, U2);
POINT_IN_TRI(U0, V0, V1, V2);
return false
}
int tri_tri_intersect(float V0[3], float V1[3], float V2[3], float U0[3],
float U1[3], float U2[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;
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;
#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 == TRUE
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 0;
return 1;
}
#define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, \
X0, X1) \
{ \
if (D0D1 > 0.0f) { \
/* here we know that D0D2<=0.0 */ \
/* that is D0, D1 are on the same side, D2 on the other or on the plane \
*/ \
A = VV2; \
B = (VV0 - VV2) * D2; \
C = (VV1 - VV2) * D2; \
X0 = D2 - D0; \
X1 = D2 - D1; \
} else if (D0D2 > 0.0f) { \
/* here we know that d0d1<=0.0 */ \
A = VV1; \
B = (VV0 - VV1) * D1; \
C = (VV2 - VV1) * D1; \
X0 = D1 - D0; \
X1 = D1 - D2; \
} else if (D1 * D2 > 0.0f || D0 != 0.0f) { \
/* here we know that d0d1<=0.0 or that D0!=0.0 */ \
A = VV0; \
B = (VV1 - VV0) * D0; \
C = (VV2 - VV0) * D0; \
X0 = D0 - D1; \
X1 = D0 - D2; \
} else if (D1 != 0.0f) { \
A = VV1; \
B = (VV0 - VV1) * D1; \
C = (VV2 - VV1) * D1; \
X0 = D1 - D0; \
X1 = D1 - D2; \
} else if (D2 != 0.0f) { \
A = VV2; \
B = (VV0 - VV2) * D2; \
C = (VV1 - VV2) * D2; \
X0 = D2 - D0; \
X1 = D2 - D1; \
} else { \
/* triangles are coplanar */ \
return coplanar_tri_tri(N1, V0, V1, V2, U0, U1, U2); \
} \
}
int NoDivTriTriIsect(float V0[3], float V1[3], float V2[3], float U0[3],
float U1[3], float U2[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;
short index;
float vp0, vp1, vp2;
float up0, up1, up2;
float bb, cc, max;
float a, b, c, x0, x1;
float d, e, f, y0, y1;
float xx, yy, xxyy, 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 == TRUE
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 == TRUE
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 = (float)FABS(D[0]);
index = 0;
bb = (float)FABS(D[1]);
cc = (float)FABS(D[2]);
if (bb > max)
max = bb, index = 1;
if (cc > max)
max = cc, 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 */
NEWCOMPUTE_INTERVALS(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, a, b, c,
x0, x1);
/* compute interval for triangle 2 */
NEWCOMPUTE_INTERVALS(up0, up1, up2, du0, du1, du2, du0du1, du0du2, d, e, f,
y0, y1);
xx = x0 * x1;
yy = y0 * y1;
xxyy = xx * yy;
tmp = a * xxyy;
isect1[0] = tmp + b * x1 * yy;
isect1[1] = tmp + c * x0 * yy;
tmp = d * xxyy;
isect2[0] = tmp + e * xx * y1;
isect2[1] = tmp + f * xx * y0;
SORT(isect1[0], isect1[1]);
SORT(isect2[0], isect2[1]);
if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
return 0;
return 1;
}
template <typename T>
inline void isect2(vec3<T> VTX0, vec3<T> VTX1, vec3<T> VTX2, T VV0, T VV1,
T VV2, T D0, T D1, T D2, T *isect0, T *isect1,
vec3<T> isectpoint0, vec3<T> isectpoint1) {
T tmp = D0 / (D0 - D1);
T diff[3];
*isect0 = VV0 + (VV1 - VV0) * tmp;
SUB(diff, VTX1, VTX0);
MULT(diff, diff, tmp);
ADD(isectpoint0, diff, VTX0);
tmp = D0 / (D0 - D2);
*isect1 = VV0 + (VV2 - VV0) * tmp;
SUB(diff, VTX2, VTX0);
MULT(diff, diff, tmp);
ADD(isectpoint1, VTX0, diff);
}
template <typename T>
__host__ __device__ inline bool
compute_intervals_isectline(vec3<T> VERT0, vec3<T> VERT1, vec3<T> VERT2, T VV0,
T VV1, T VV2, T D0, T D1, T D2, T D0D1, T D0D2,
T *isect0, T *isect1, vec3<T> isectpoint0,
vec3<T> isectpoint1) {
if (D0D1 > 0.0f) {
/* here we know that D0D2<=0.0 */
/* that is D0, D1 are on the same side, D2 on the other or on the plane */
isect2(VERT2, VERT0, VERT1, VV2, VV0, VV1, D2, D0, D1, isect0, isect1,
isectpoint0, isectpoint1);
} else if (D0D2 > 0.0f) {
/* here we know that d0d1<=0.0 */
isect2(VERT1, VERT0, VERT2, VV1, VV0, VV2, D1, D0, D2, isect0, isect1,
isectpoint0, isectpoint1);
} else if (D1 * D2 > 0.0f || D0 != 0.0f) {
/* here we know that d0d1<=0.0 or that D0!=0.0 */
isect2(VERT0, VERT1, VERT2, VV0, VV1, VV2, D0, D1, D2, isect0, isect1,
isectpoint0, isectpoint1);
} else if (D1 != 0.0f) {
isect2(VERT1, VERT0, VERT2, VV1, VV0, VV2, D1, D0, D2, isect0, isect1,
isectpoint0, isectpoint1);
} else if (D2 != 0.0f) {
isect2(VERT2, VERT0, VERT1, VV2, VV0, VV1, D2, D0, D1, isect0, isect1,
isectpoint0, isectpoint1);
} else {
/* triangles are coplanar */
return 1;
}
return 0;
}
template <typename T>
__host__ __device__ inline bool tri_tri_intersect_with_isectline(
vec3<T> V0, vec3<T> V1, vec3<T> V2, vec3<T> U0, vec3<T> U1, vec3<T> U2,
bool *coplanar, vec3<T> isect_point1[3], vec3<T> isect_point2[3]) {
vec3<T> E1, E2;
vec3<T> N1, N2, d1, d2;
vec3<T> D;
T du0, du1, du2, dv0, dv1, dv2;
vec2<T> isect1, isect2;
vec3<T> isectpointA1, isectpointA2;
vec3<T> isectpointB1, isectpointB2;
T du0du1, du0du2, dv0dv1, dv0dv2;
short index;
T vp0, vp1, vp2;
T up0, up1, up2;
T b, c, max;
T tmp, diff[3];
int smallest1, smallest2;
/* compute plane equation of triangle(V0,V1,V2) */
E1 = V1 - V0;
E2 = V2 - V0;
N1 = cross(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;
#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) */
E1 = U1 - U0;
E2 = U2 - U0;
N2 = cross(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;
#endif
dv0dv1 = dv0 * dv1;
dv0dv2 = dv0 * dv2;
/* same sign on all of them + not equal 0 ? */
if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
return 0; /* no intersection occurs */
/* compute direction of intersection line */
D = cross(N1, N2);
/* compute and index to the largest component of D */
max = fabs(D.x);
index = 0;
b = fabs(D.y);
c = fabs(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[index];
vp1 = V1[index];
vp2 = V2[index];
up0 = U0[index];
up1 = U1[index];
up2 = U2[index];
/* compute interval for triangle 1 */
*coplanar = compute_intervals_isectline(
V0, V1, V2, vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, &isect1[0],
&isect1[1], isectpointA1, isectpointA2);
if (*coplanar)
return coplanar_tri_tri(N1, V0, V1, V2, U0, U1, U2);
/* compute interval for triangle 2 */
compute_intervals_isectline(U0, U1, U2, up0, up1, up2, du0, du1, du2, du0du1,
du0du2, &isect2[0], &isect2[1], isectpointB1,
isectpointB2);
smallest1 = sort(&isect1[0], &isect1[1]);
smallest2 = sort(&isect2[0], &isect2[1]);
if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
return 0;
/* at this point, we know that the triangles intersect */
if (isect2[0] < isect1[0]) {
if (smallest1 == 0) {
SET(isect_point1, isectpointA1);
} else {
SET(isect_point1, isectpointA2);
}
if (isect2[1] < isect1[1]) {
if (smallest2 == 0) {
SET(isect_point2, isectpointB2);
} else {
SET(isect_point2, isectpointB1);
}
} else {
if (smallest1 == 0) {
SET(isect_point2, isectpointA2);
} else {
SET(isect_point2, isectpointA1);
}
}
} else {
if (smallest2 == 0) {
SET(isect_point1, isectpointB1);
} else {
SET(isect_point1, isectpointB2);
}
if (isect2[1] > isect1[1]) {
if (smallest1 == 0) {
SET(isect_point2, isectpointA2);
} else {
SET(isect_point2, isectpointA1);
}
} else {
if (smallest2 == 0) {
SET(isect_point2, isectpointB2);
} else {
SET(isect_point2, isectpointB1);
}
}
}
return 1;
}