Line data Source code
1 : /* Triangle/triangle intersection test routine,
2 : * by Tomas Moller, 1997.
3 : * See article "A Fast Triangle-Triangle Intersection Test",
4 : * Journal of Graphics Tools, 2(2), 1997
5 : *
6 : * int tri_tri_intersect(number V0[3],number V1[3],number V2[3],
7 : * number U0[3],number U1[3],number U2[3])
8 : *
9 : * parameters: vertices of triangle 1: V0,V1,V2
10 : * vertices of triangle 2: U0,U1,U2
11 : * result : returns 1 if the triangles intersect, otherwise 0
12 : *
13 : */
14 :
15 : #include <cmath>
16 : #include <cstring>
17 : #include <cstdlib>
18 : #include "math_util.h"
19 :
20 : using namespace ug;
21 :
22 : /* if USE_EPSILON_TEST is true then we do a check:
23 : if |dv|<EPSILON then dv=0.0;
24 : else no check is done (which is less robust)
25 : */
26 : // change by sreiter: Note that a new variable snapThreshold was added to
27 : // tri_tri_intersect. EPSILON is now only used for the coplanarity check...
28 : #define USE_EPSILON_TEST TRUE
29 : #define EPSILON 1.e-12
30 :
31 :
32 : /* some macros */
33 : #define CROSS(dest,v1,v2) \
34 : dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
35 : dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
36 : dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
37 :
38 : #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
39 :
40 : #define SUB(dest,v1,v2) \
41 : dest[0]=v1[0]-v2[0]; \
42 : dest[1]=v1[1]-v2[1]; \
43 : dest[2]=v1[2]-v2[2];
44 :
45 : /* sort so that a<=b */
46 : #define SORT(a,b) \
47 : if(a>b) \
48 : { \
49 : number c; \
50 : c=a; \
51 : a=b; \
52 : b=c; \
53 : }
54 :
55 : #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
56 : isect0=VV0+(VV1-VV0)*D0/(D0-D1); \
57 : isect1=VV0+(VV2-VV0)*D0/(D0-D2);
58 :
59 :
60 : #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
61 : if(D0D1>0.0f) \
62 : { \
63 : /* here we know that D0D2<=0.0 */ \
64 : /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
65 : ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
66 : } \
67 : else if(D0D2>0.0f) \
68 : { \
69 : /* here we know that d0d1<=0.0 */ \
70 : ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
71 : } \
72 : else if(D1*D2>0.0f || D0!=0.0f) \
73 : { \
74 : /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
75 : ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
76 : } \
77 : else if(D1!=0.0f) \
78 : { \
79 : ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
80 : } \
81 : else if(D2!=0.0f) \
82 : { \
83 : ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
84 : } \
85 : else \
86 : { \
87 : /* triangles are coplanar */ \
88 : return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
89 : }
90 :
91 : // VV0, VV1, VV2, isect0 and isect1 have to be arrays of 3 numbers.
92 : #define ISECT3(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
93 : isect0[0]=VV0[0]+(VV1[0]-VV0[0])*D0/(D0-D1); \
94 : isect0[1]=VV0[1]+(VV1[1]-VV0[1])*D0/(D0-D1); \
95 : isect0[2]=VV0[2]+(VV1[2]-VV0[2])*D0/(D0-D1); \
96 : isect1[0]=VV0[0]+(VV2[0]-VV0[0])*D0/(D0-D2); \
97 : isect1[1]=VV0[1]+(VV2[1]-VV0[1])*D0/(D0-D2); \
98 : isect1[2]=VV0[2]+(VV2[2]-VV0[2])*D0/(D0-D2);
99 :
100 : // VV0, VV1, VV2, isect0 and isect1 have to be arrays of 3 numbers.
101 : #define COMPUTE_INTERVAL_POINTS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
102 : if(D0D1>0.0f) \
103 : { \
104 : /* here we know that D0D2<=0.0 */ \
105 : /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
106 : ISECT3(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
107 : } \
108 : else if(D0D2>0.0f) \
109 : { \
110 : /* here we know that d0d1<=0.0 */ \
111 : ISECT3(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
112 : } \
113 : else if(D1*D2>0.0f || D0!=0.0f) \
114 : { \
115 : /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
116 : ISECT3(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
117 : } \
118 : else if(D1!=0.0f) \
119 : { \
120 : ISECT3(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
121 : } \
122 : else if(D2!=0.0f) \
123 : { \
124 : ISECT3(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
125 : } \
126 : else \
127 : { \
128 : /* triangles are coplanar */ \
129 : /* we ignore this case since it we already returned.*/ \
130 : }
131 :
132 :
133 :
134 : /* this edge to edge test is based on Franlin Antonio's gem:
135 : "Faster Line Segment Intersection", in Graphics Gems III,
136 : pp. 199-202 */
137 : #define EDGE_EDGE_TEST(V0,U0,U1) \
138 : Bx=U0[i0]-U1[i0]; \
139 : By=U0[i1]-U1[i1]; \
140 : Cx=V0[i0]-U0[i0]; \
141 : Cy=V0[i1]-U0[i1]; \
142 : f=Ay*Bx-Ax*By; \
143 : d=By*Cx-Bx*Cy; \
144 : if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
145 : { \
146 : e=Ax*Cy-Ay*Cx; \
147 : if(f>0) \
148 : { \
149 : if(e>=0 && e<=f) return 2; \
150 : } \
151 : else \
152 : { \
153 : if(e<=0 && e>=f) return 2; \
154 : } \
155 : }
156 :
157 : #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
158 : { \
159 : number Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
160 : Ax=V1[i0]-V0[i0]; \
161 : Ay=V1[i1]-V0[i1]; \
162 : /* test edge U0,U1 against V0,V1 */ \
163 : EDGE_EDGE_TEST(V0,U0,U1); \
164 : /* test edge U1,U2 against V0,V1 */ \
165 : EDGE_EDGE_TEST(V0,U1,U2); \
166 : /* test edge U2,U1 against V0,V1 */ \
167 : EDGE_EDGE_TEST(V0,U2,U0); \
168 : }
169 :
170 : #define POINT_IN_TRI(V0,U0,U1,U2) \
171 : { \
172 : number a,b,c,d0,d1,d2; \
173 : /* is T1 completly inside T2? */ \
174 : /* check if V0 is inside tri(U0,U1,U2) */ \
175 : a=U1[i1]-U0[i1]; \
176 : b=-(U1[i0]-U0[i0]); \
177 : c=-a*U0[i0]-b*U0[i1]; \
178 : d0=a*V0[i0]+b*V0[i1]+c; \
179 : \
180 : a=U2[i1]-U1[i1]; \
181 : b=-(U2[i0]-U1[i0]); \
182 : c=-a*U1[i0]-b*U1[i1]; \
183 : d1=a*V0[i0]+b*V0[i1]+c; \
184 : \
185 : a=U0[i1]-U2[i1]; \
186 : b=-(U0[i0]-U2[i0]); \
187 : c=-a*U2[i0]-b*U2[i1]; \
188 : d2=a*V0[i0]+b*V0[i1]+c; \
189 : if(d0*d1>0.0) \
190 : { \
191 : if(d0*d2>0.0) return 2; \
192 : } \
193 : }
194 :
195 0 : int coplanar_tri_tri(number N[3],number V0[3],number V1[3],number V2[3],
196 : number U0[3],number U1[3],number U2[3])
197 : {
198 : number A[3];
199 : short i0,i1;
200 : /* first project onto an axis-aligned plane, that maximizes the area */
201 : /* of the triangles, compute indices: i0,i1. */
202 : //#pragma warning( disable : 4244 )
203 0 : A[0]=fabs(N[0]);
204 0 : A[1]=fabs(N[1]);
205 0 : A[2]=fabs(N[2]);
206 : //#pragma warning( default : 4244 )
207 0 : if(A[0]>A[1])
208 : {
209 0 : if(A[0]>A[2])
210 : {
211 : i0=1; /* A[0] is greatest */
212 : i1=2;
213 : }
214 : else
215 : {
216 : i0=0; /* A[2] is greatest */
217 : i1=1;
218 : }
219 : }
220 : else /* A[0]<=A[1] */
221 : {
222 0 : if(A[2]>A[1])
223 : {
224 : i0=0; /* A[2] is greatest */
225 : i1=1;
226 : }
227 : else
228 : {
229 : i0=0; /* A[1] is greatest */
230 : i1=2;
231 : }
232 : }
233 :
234 : /* test all edges of triangle 1 against the edges of triangle 2 */
235 0 : EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
236 0 : EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
237 0 : EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
238 :
239 : /* finally, test if tri1 is totally contained in tri2 or vice versa */
240 0 : POINT_IN_TRI(V0,U0,U1,U2);
241 0 : POINT_IN_TRI(U0,V0,V1,V2);
242 :
243 : return 0;
244 : }
245 :
246 :
247 0 : int tri_tri_intersect(number V0[3],number V1[3],number V2[3],
248 : number U0[3],number U1[3],number U2[3],
249 : number* ip1Out, number* ip2Out, const number snapThreshold)
250 : {
251 : number E1[3],E2[3];
252 : number N1[3],N2[3],d1,d2;
253 : number du0,du1,du2,dv0,dv1,dv2;
254 : number D[3];
255 : number isect1[2], isect2[2];
256 : number du0du1,du0du2,dv0dv1,dv0dv2;
257 : short index;
258 : number vp0,vp1,vp2;
259 : number up0,up1,up2;
260 : number b,c,max;
261 :
262 : /* compute plane equation of triangle(V0,V1,V2) */
263 0 : SUB(E1,V1,V0);
264 0 : SUB(E2,V2,V0);
265 0 : CROSS(N1,E1,E2);
266 0 : d1=-DOT(N1,V0);
267 : /* plane equation 1: N1.X+d1=0 */
268 :
269 : /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
270 0 : du0=DOT(N1,U0)+d1;
271 0 : du1=DOT(N1,U1)+d1;
272 0 : du2=DOT(N1,U2)+d1;
273 :
274 : /* coplanarity robustness check */
275 : #if USE_EPSILON_TEST==TRUE
276 0 : if(fabs(du0)<snapThreshold) du0=0.0;
277 0 : if(fabs(du1)<snapThreshold) du1=0.0;
278 0 : if(fabs(du2)<snapThreshold) du2=0.0;
279 : #endif
280 0 : du0du1=du0*du1;
281 0 : du0du2=du0*du2;
282 0 : if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
283 : return 0; /* no intersection occurs */
284 :
285 : /* compute plane of triangle (U0,U1,U2) */
286 0 : SUB(E1,U1,U0);
287 0 : SUB(E2,U2,U0);
288 0 : CROSS(N2,E1,E2);
289 0 : d2=-DOT(N2,U0);
290 : /* plane equation 2: N2.X+d2=0 */
291 :
292 : /* put V0,V1,V2 into plane equation 2 */
293 0 : dv0=DOT(N2,V0)+d2;
294 0 : dv1=DOT(N2,V1)+d2;
295 0 : dv2=DOT(N2,V2)+d2;
296 :
297 : #if USE_EPSILON_TEST==TRUE
298 0 : if(fabs(dv0)<snapThreshold) dv0=0.0;
299 0 : if(fabs(dv1)<snapThreshold) dv1=0.0;
300 0 : if(fabs(dv2)<snapThreshold) dv2=0.0;
301 : #endif
302 :
303 0 : dv0dv1=dv0*dv1;
304 0 : dv0dv2=dv0*dv2;
305 0 : if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
306 : return 0; /* no intersection occurs */
307 :
308 : /* compute direction of intersection line */
309 0 : CROSS(D,N1,N2);
310 :
311 : /* compute and index to the largest component of D */
312 : //#pragma warning( disable : 4244 )
313 0 : max=fabs(D[0]);
314 : index=0;
315 0 : b=fabs(D[1]);
316 0 : c=fabs(D[2]);
317 : //#pragma warning( default : 4244 )
318 0 : if(b>max) max=b,index=1;
319 0 : if(c>max) index=2;
320 :
321 : /* this is the simplified projection onto L*/
322 0 : vp0=V0[index];
323 0 : vp1=V1[index];
324 0 : vp2=V2[index];
325 :
326 0 : up0=U0[index];
327 0 : up1=U1[index];
328 0 : up2=U2[index];
329 :
330 : /* compute interval for triangle 1 */
331 0 : COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
332 :
333 : /* compute interval for triangle 2 */
334 0 : COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
335 :
336 : // those indices are used to find the maximum and minimum later on
337 : int ind1 = 1;
338 : int ind2 = 1;
339 0 : if(isect1[0] > isect1[1]){
340 : std::swap(isect1[0], isect1[1]);
341 : ind1 = 0;
342 : }
343 :
344 0 : if(isect2[0] > isect2[1]){
345 : std::swap(isect2[0], isect2[1]);
346 : ind2 = 0;
347 : }
348 : // SORT(isect1[0],isect1[1]);
349 : // SORT(isect2[0],isect2[1]);
350 0 : if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
351 :
352 0 : if(ip1Out && ip2Out){
353 : // calculate the endpoints of the line segment that resembles the intersection.
354 0 : number tpa1[3]={0, 0, 0}, tpa2[3]={0, 0, 0}, tpb1[3]={0, 0, 0},
355 0 : tpb2[3]={0, 0, 0};
356 :
357 : /* compute interval-points for triangle 1 */
358 0 : COMPUTE_INTERVAL_POINTS(V0,V1,V2,dv0,dv1,dv2,dv0dv1,dv0dv2,tpa1,tpa2);
359 :
360 : /* compute interval-points for triangle 2 */
361 0 : COMPUTE_INTERVAL_POINTS(U0,U1,U2,du0,du1,du2,du0du1,du0du2,tpb1, tpb2);
362 :
363 : //choose the right ones and return
364 0 : if(isect1[0] > isect2[0])
365 0 : if(ind1 == 0)
366 : memcpy((char*)ip1Out, tpa2, sizeof(number)*3);
367 : else
368 : memcpy((char*)ip1Out, tpa1, sizeof(number)*3);
369 : else
370 0 : if(ind2 == 0)
371 : memcpy((char*)ip1Out, tpb2, sizeof(number)*3);
372 : else
373 : memcpy((char*)ip1Out, tpb1, sizeof(number)*3);
374 :
375 0 : if(isect1[1] < isect2[1])
376 0 : if(ind1 == 0)
377 : memcpy((char*)ip2Out, tpa1, sizeof(number)*3);
378 : else
379 : memcpy((char*)ip2Out, tpa2, sizeof(number)*3);
380 : else
381 0 : if(ind2 == 0)
382 : memcpy((char*)ip2Out, tpb1, sizeof(number)*3);
383 : else
384 : memcpy((char*)ip2Out, tpb2, sizeof(number)*3);
385 : }
386 : return 1;
387 : }
388 :
389 : namespace ug{
390 0 : bool TriangleTriangleIntersection(const MathVector<3>& p0, const MathVector<3>& p1,
391 : const MathVector<3>& p2, const MathVector<3>& q0,
392 : const MathVector<3>& q1, const MathVector<3>& q2,
393 : MathVector<3>* ip1Out, MathVector<3>* ip2Out,
394 : number snapThreshold)
395 : {
396 0 : return tri_tri_intersect((number*)&p0, (number*)&p1, (number*)&p2,
397 : (number*)&q0, (number*)&q1, (number*)&q2,
398 0 : (number*)ip1Out, (number*)ip2Out, snapThreshold) == 1;
399 : }
400 :
401 : }// end of namespace
|
