1
2 //! if OPC_TRITRI_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, but faster)
3 #define LOCAL_EPSILON 0.000001f
4
5 //! sort so that a<=b
6 #define SORT(a,b) \
7 if(a>b) \
8 { \
9 const float c=a; \
10 a=b; \
11 b=c; \
12 }
13
14 //! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
15 #define EDGE_EDGE_TEST(V0, U0, U1) \
16 Bx = U0[i0] - U1[i0]; \
17 By = U0[i1] - U1[i1]; \
18 Cx = V0[i0] - U0[i0]; \
19 Cy = V0[i1] - U0[i1]; \
20 f = Ay*Bx - Ax*By; \
21 d = By*Cx - Bx*Cy; \
22 if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f)) \
23 { \
24 const float e=Ax*Cy - Ay*Cx; \
25 if(f>0.0f) \
26 { \
27 if(e>=0.0f && e<=f) return TRUE; \
28 } \
29 else \
30 { \
31 if(e<=0.0f && e>=f) return TRUE; \
32 } \
33 }
34
35 //! TO BE DOCUMENTED
36 #define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2) \
37 { \
38 float Bx,By,Cx,Cy,d,f; \
39 const float Ax = V1[i0] - V0[i0]; \
40 const float Ay = V1[i1] - V0[i1]; \
41 /* test edge U0,U1 against V0,V1 */ \
42 EDGE_EDGE_TEST(V0, U0, U1); \
43 /* test edge U1,U2 against V0,V1 */ \
44 EDGE_EDGE_TEST(V0, U1, U2); \
45 /* test edge U2,U1 against V0,V1 */ \
46 EDGE_EDGE_TEST(V0, U2, U0); \
47 }
48
49 //! TO BE DOCUMENTED
50 #define POINT_IN_TRI(V0, U0, U1, U2) \
51 { \
52 /* is T1 completly inside T2? */ \
53 /* check if V0 is inside tri(U0,U1,U2) */ \
54 float a = U1[i1] - U0[i1]; \
55 float b = -(U1[i0] - U0[i0]); \
56 float c = -a*U0[i0] - b*U0[i1]; \
57 float d0 = a*V0[i0] + b*V0[i1] + c; \
58 \
59 a = U2[i1] - U1[i1]; \
60 b = -(U2[i0] - U1[i0]); \
61 c = -a*U1[i0] - b*U1[i1]; \
62 const float d1 = a*V0[i0] + b*V0[i1] + c; \
63 \
64 a = U0[i1] - U2[i1]; \
65 b = -(U0[i0] - U2[i0]); \
66 c = -a*U2[i0] - b*U2[i1]; \
67 const float d2 = a*V0[i0] + b*V0[i1] + c; \
68 if(d0*d1>0.0f) \
69 { \
70 if(d0*d2>0.0f) return TRUE; \
71 } \
72 }
73
74 //! TO BE DOCUMENTED
CoplanarTriTri(const Point & n,const Point & v0,const Point & v1,const Point & v2,const Point & u0,const Point & u1,const Point & u2)75 BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
76 {
77 float A[3];
78 short i0,i1;
79 /* first project onto an axis-aligned plane, that maximizes the area */
80 /* of the triangles, compute indices: i0,i1. */
81 A[0] = fabsf(n[0]);
82 A[1] = fabsf(n[1]);
83 A[2] = fabsf(n[2]);
84 if(A[0]>A[1])
85 {
86 if(A[0]>A[2])
87 {
88 i0=1; /* A[0] is greatest */
89 i1=2;
90 }
91 else
92 {
93 i0=0; /* A[2] is greatest */
94 i1=1;
95 }
96 }
97 else /* A[0]<=A[1] */
98 {
99 if(A[2]>A[1])
100 {
101 i0=0; /* A[2] is greatest */
102 i1=1;
103 }
104 else
105 {
106 i0=0; /* A[1] is greatest */
107 i1=2;
108 }
109 }
110
111 /* test all edges of triangle 1 against the edges of triangle 2 */
112 EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
113 EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
114 EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
115
116 /* finally, test if tri1 is totally contained in tri2 or vice versa */
117 POINT_IN_TRI(v0, u0, u1, u2);
118 POINT_IN_TRI(u0, v0, v1, v2);
119
120 return FALSE;
121 }
122
123 //! TO BE DOCUMENTED
124 #define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1) \
125 { \
126 if(D0D1>0.0f) \
127 { \
128 /* here we know that D0D2<=0.0 */ \
129 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
130 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \
131 } \
132 else if(D0D2>0.0f) \
133 { \
134 /* here we know that d0d1<=0.0 */ \
135 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \
136 } \
137 else if(D1*D2>0.0f || D0!=0.0f) \
138 { \
139 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
140 A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2; \
141 } \
142 else if(D1!=0.0f) \
143 { \
144 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \
145 } \
146 else if(D2!=0.0f) \
147 { \
148 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \
149 } \
150 else \
151 { \
152 /* triangles are coplanar */ \
153 return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2); \
154 } \
155 }
156
157 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
158 /**
159 * Triangle/triangle intersection test routine,
160 * by Tomas Moller, 1997.
161 * See article "A Fast Triangle-Triangle Intersection Test",
162 * Journal of Graphics Tools, 2(2), 1997
163 *
164 * Updated June 1999: removed the divisions -- a little faster now!
165 * Updated October 1999: added {} to CROSS and SUB macros
166 *
167 * int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
168 * float U0[3],float U1[3],float U2[3])
169 *
170 * \param V0 [in] triangle 0, vertex 0
171 * \param V1 [in] triangle 0, vertex 1
172 * \param V2 [in] triangle 0, vertex 2
173 * \param U0 [in] triangle 1, vertex 0
174 * \param U1 [in] triangle 1, vertex 1
175 * \param U2 [in] triangle 1, vertex 2
176 * \return true if triangles overlap
177 */
178 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TriTriOverlap(const Point & V0,const Point & V1,const Point & V2,const Point & U0,const Point & U1,const Point & U2)179 inline_ BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
180 {
181 // Stats
182 mNbPrimPrimTests++;
183
184 // Compute plane equation of triangle(V0,V1,V2)
185 Point E1 = V1 - V0;
186 Point E2 = V2 - V0;
187 const Point N1 = E1 ^ E2;
188 const float d1 =-N1 | V0;
189 // Plane equation 1: N1.X+d1=0
190
191 // Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
192 float du0 = (N1|U0) + d1;
193 float du1 = (N1|U1) + d1;
194 float du2 = (N1|U2) + d1;
195
196 // Coplanarity robustness check
197 #ifdef OPC_TRITRI_EPSILON_TEST
198 float absd1 = FastFabs(d1), sqmagN1 = N1.SquareMagnitude();
199 if (absd1>=sqmagN1)
200 {
201 if(FastFabs(du0)<=LOCAL_EPSILON*absd1) du0 = 0.0f;
202 if(FastFabs(du1)<=LOCAL_EPSILON*absd1) du1 = 0.0f;
203 if(FastFabs(du2)<=LOCAL_EPSILON*absd1) du2 = 0.0f;
204 }
205 else
206 {
207 if(FastFabs(du0)<=LOCAL_EPSILON*FCMax2(absd1, FCMin2(sqmagN1, U0.SquareMagnitude()))) du0 = 0.0f;
208 if(FastFabs(du1)<=LOCAL_EPSILON*FCMax2(absd1, FCMin2(sqmagN1, U1.SquareMagnitude()))) du1 = 0.0f;
209 if(FastFabs(du2)<=LOCAL_EPSILON*FCMax2(absd1, FCMin2(sqmagN1, U2.SquareMagnitude()))) du2 = 0.0f;
210 }
211 #endif
212 const float du0du1 = du0 * du1;
213 const float du0du2 = du0 * du2;
214
215 if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
216 return FALSE; // no intersection occurs
217
218 // Compute plane of triangle (U0,U1,U2)
219 E1 = U1 - U0;
220 E2 = U2 - U0;
221 const Point N2 = E1 ^ E2;
222 const float d2=-N2 | U0;
223 // plane equation 2: N2.X+d2=0
224
225 // put V0,V1,V2 into plane equation 2
226 float dv0 = (N2|V0) + d2;
227 float dv1 = (N2|V1) + d2;
228 float dv2 = (N2|V2) + d2;
229
230 #ifdef OPC_TRITRI_EPSILON_TEST
231 float absd2 = FastFabs(d2), sqmagN2 = N2.SquareMagnitude();
232 if (absd2>=sqmagN2)
233 {
234 if(FastFabs(dv0)<=LOCAL_EPSILON*absd2) dv0 = 0.0f;
235 if(FastFabs(dv1)<=LOCAL_EPSILON*absd2) dv1 = 0.0f;
236 if(FastFabs(dv2)<=LOCAL_EPSILON*absd2) dv2 = 0.0f;
237 }
238 else
239 {
240 if(FastFabs(dv0)<=LOCAL_EPSILON*FCMax2(absd2, FCMin2(sqmagN2, V0.SquareMagnitude()))) dv0 = 0.0f;
241 if(FastFabs(dv1)<=LOCAL_EPSILON*FCMax2(absd2, FCMin2(sqmagN2, V1.SquareMagnitude()))) dv1 = 0.0f;
242 if(FastFabs(dv2)<=LOCAL_EPSILON*FCMax2(absd2, FCMin2(sqmagN2, V2.SquareMagnitude()))) dv2 = 0.0f;
243 }
244 #endif
245
246 const float dv0dv1 = dv0 * dv1;
247 const float dv0dv2 = dv0 * dv2;
248
249 if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
250 return FALSE; // no intersection occurs
251
252 // Compute direction of intersection line
253 const Point D = N1^N2;
254
255 // Compute and index to the largest component of D
256 float max=fabsf(D[0]);
257 short index=0;
258 float bb=fabsf(D[1]);
259 float cc=fabsf(D[2]);
260 if(bb>max) max=bb,index=1;
261 if(cc>max) max=cc,index=2;
262
263 // This is the simplified projection onto L
264 const float vp0 = V0[index];
265 const float vp1 = V1[index];
266 const float vp2 = V2[index];
267
268 const float up0 = U0[index];
269 const float up1 = U1[index];
270 const float up2 = U2[index];
271
272 // Compute interval for triangle 1
273 float a,b,c,x0,x1;
274 NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
275
276 // Compute interval for triangle 2
277 float d,e,f,y0,y1;
278 NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
279
280 const float xx=x0*x1;
281 const float yy=y0*y1;
282 const float xxyy=xx*yy;
283
284 float isect1[2], isect2[2];
285
286 float tmp=a*xxyy;
287 isect1[0]=tmp+b*x1*yy;
288 isect1[1]=tmp+c*x0*yy;
289
290 tmp=d*xxyy;
291 isect2[0]=tmp+e*xx*y1;
292 isect2[1]=tmp+f*xx*y0;
293
294 SORT(isect1[0],isect1[1]);
295 SORT(isect2[0],isect2[1]);
296
297 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE;
298 return TRUE;
299 }
300