33 #ifndef DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
34 #define DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
38 #define FABS(x) ((float)fabs(x))
44 #define USE_EPSILON_TEST TRUE
45 #define EPSILON 0.000001
48 #define COPLANAR_CONTACT -1
49 #define INTERIAL_CONTACT 1
52 #define CROSS(dest, v1, v2) \
53 dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; \
54 dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; \
55 dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
57 #define DOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2])
59 #define SUB(dest, v1, v2) \
60 dest[0] = v1[0] - v2[0]; \
61 dest[1] = v1[1] - v2[1]; \
62 dest[2] = v1[2] - v2[2];
64 #define ADD(dest, v1, v2) \
65 dest[0] = v1[0] + v2[0]; \
66 dest[1] = v1[1] + v2[1]; \
67 dest[2] = v1[2] + v2[2];
69 #define MULT(dest, v, factor) \
70 dest[0] = factor * v[0]; \
71 dest[1] = factor * v[1]; \
72 dest[2] = factor * v[2];
74 #define DIV(dest, v1, v2) \
75 dest[0] = v1[0] / v2[0]; \
76 dest[1] = v1[1] / 2 [1]; \
77 dest[2] = v1[2] / v2[2];
79 #define SET(dest, src) \
102 #define ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1) \
103 isect0 = VV0 + (VV1 - VV0) * D0 / (D0 - D1); \
104 isect1 = VV0 + (VV2 - VV0) * D0 / (D0 - D2);
106 #define COMPUTE_INTERVALS( \
107 VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, isect1) \
112 ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
114 else if (D0D2 > 0.0f) \
117 ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
119 else if (D1 * D2 > 0.0f || D0 != 0.0f) \
122 ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1); \
124 else if (D1 != 0.0f) \
126 ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
128 else if (D2 != 0.0f) \
130 ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
135 return COPLANAR_CONTACT; \
139 float V0[3],
float V1[3],
float DV0,
float DV1,
float V[3])
141 float VV0[3], VV1[3];
161 float N1[3], N2[3], d1, d2;
162 float du0, du1, du2, dv0, dv1, dv2;
164 float isect1[2], isect2[2];
165 float du0du1, du0du2, dv0dv1, dv0dv2, du1du2, dv1dv2;
180 du0 =
DOT(N1, U0) + d1;
181 du1 =
DOT(N1, U1) + d1;
182 du2 =
DOT(N1, U2) + d1;
185 #if USE_EPSILON_TEST == TRUE
192 if (du1 == 0 && du2 == 0 && fabs(du0) < 1e-4)
194 if (du0 == 0 && du2 == 0 && fabs(du1) < 1e-4)
196 if (du0 == 0 && du1 == 0 && fabs(du2) < 1e-4)
203 if (du0du1 > 0.0f && du0du2 > 0.0f)
215 dv0 =
DOT(N2, V0) + d2;
216 dv1 =
DOT(N2, V1) + d2;
217 dv2 =
DOT(N2, V2) + d2;
219 #if USE_EPSILON_TEST == TRUE
226 if (dv1 == 0 && dv2 == 0 && fabs(dv0) < 1e-5)
228 if (dv0 == 0 && dv2 == 0 && fabs(dv1) < 1e-5)
230 if (dv0 == 0 && dv1 == 0 && fabs(dv2) < 1e-5)
237 if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
265 vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, isect1[0], isect1[1]);
269 up0, up1, up2, du0, du1, du2, du0du1, du0du2, isect2[0], isect2[1]);
271 SORT(isect1[0], isect1[1]);
272 SORT(isect2[0], isect2[1]);
309 std::cerr <<
"contact error" << std::endl;
344 std::cerr <<
"contact error" << std::endl;
347 for (
int i = 3; i > 0; i--)
348 for (
int j = 0; j < i; j++)
352 for (
int k = 0; k < 3; k++)
353 SWAP(res[j][k], res[j + 1][k]);
std::size_t index
Definition: SkelParser.cpp:1672
#define EPSILON
Definition: tri_tri_intersection_test.hpp:45
#define COMPUTE_INTERVALS( VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, isect1)
Definition: tri_tri_intersection_test.hpp:106
#define SUB(dest, v1, v2)
Definition: tri_tri_intersection_test.hpp:59
#define CROSS(dest, v1, v2)
Definition: tri_tri_intersection_test.hpp:52
#define NO_CONTACT
Definition: tri_tri_intersection_test.hpp:47
void edge_tri_intersect(float V0[3], float V1[3], float DV0, float DV1, float V[3])
Definition: tri_tri_intersection_test.hpp:138
#define DOT(v1, v2)
Definition: tri_tri_intersection_test.hpp:57
#define SET(dest, src)
Definition: tri_tri_intersection_test.hpp:79
#define SWAP(a, b)
Definition: tri_tri_intersection_test.hpp:94
#define SORT(a, b)
Definition: tri_tri_intersection_test.hpp:85
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])
Definition: tri_tri_intersection_test.hpp:150
#define MULT(dest, v, factor)
Definition: tri_tri_intersection_test.hpp:69