DART  6.10.1
tri_tri_intersection_test.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
34 #define DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
35 
36 #include <math.h>
37 
38 #define FABS(x) ((float)fabs(x)) /* implement as is fastest on your machine */
39 
40 /* if USE_EPSILON_TEST is true then we do a check:
41  if |dv|<EPSILON then dv=0.0;
42  else no check is done (which is less robust)
43 */
44 #define USE_EPSILON_TEST TRUE
45 #define EPSILON 0.000001
46 
47 #define NO_CONTACT 0
48 #define COPLANAR_CONTACT -1
49 #define INTERIAL_CONTACT 1
50 
51 /* some macros */
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];
56 
57 #define DOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2])
58 
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];
63 
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];
68 
69 #define MULT(dest, v, factor) \
70  dest[0] = factor * v[0]; \
71  dest[1] = factor * v[1]; \
72  dest[2] = factor * v[2];
73 
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];
78 
79 #define SET(dest, src) \
80  dest[0] = src[0]; \
81  dest[1] = src[1]; \
82  dest[2] = src[2];
83 
84 /* sort so that a<=b */
85 #define SORT(a, b) \
86  if (a > b) \
87  { \
88  float c; \
89  c = a; \
90  a = b; \
91  b = c; \
92  }
93 
94 #define SWAP(a, b) \
95  { \
96  float c; \
97  c = a; \
98  a = b; \
99  b = c; \
100  }
101 
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);
105 
106 #define COMPUTE_INTERVALS( \
107  VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, isect1) \
108  if (D0D1 > 0.0f) \
109  { \
110  /* here we know that D0D2<=0.0 */ \
111  /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
112  ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
113  } \
114  else if (D0D2 > 0.0f) \
115  { \
116  /* here we know that d0d1<=0.0 */ \
117  ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
118  } \
119  else if (D1 * D2 > 0.0f || D0 != 0.0f) \
120  { \
121  /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
122  ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1); \
123  } \
124  else if (D1 != 0.0f) \
125  { \
126  ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
127  } \
128  else if (D2 != 0.0f) \
129  { \
130  ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
131  } \
132  else \
133  { \
134  /* triangles are coplanar */ \
135  return COPLANAR_CONTACT; \
136  }
137 
138 inline void edge_tri_intersect(
139  float V0[3], float V1[3], float DV0, float DV1, float V[3])
140 {
141  float VV0[3], VV1[3];
142  MULT(VV0, V1, DV0);
143  MULT(VV1, V0, DV1);
144  float U[3], D;
145  SUB(U, VV0, VV1);
146  D = DV0 - DV1;
147  MULT(V, U, 1.0 / D);
148 }
149 
150 inline int tri_tri_intersect(
151  float V0[3],
152  float V1[3],
153  float V2[3],
154  float U0[3],
155  float U1[3],
156  float U2[3],
157  float res1[3],
158  float res2[3])
159 {
160  float E1[3], E2[3];
161  float N1[3], N2[3], d1, d2;
162  float du0, du1, du2, dv0, dv1, dv2;
163  float D[3];
164  float isect1[2], isect2[2];
165  float du0du1, du0du2, dv0dv1, dv0dv2, du1du2, dv1dv2;
166  short index;
167  float vp0, vp1, vp2;
168  float up0, up1, up2;
169  float b, c, max;
170 
171  /* compute plane equation of triangle(V0,V1,V2) */
172  SUB(E1, V1, V0);
173  SUB(E2, V2, V0);
174  CROSS(N1, E1, E2);
175  d1 = -DOT(N1, V0);
176  /* plane equation 1: N1.X+d1=0 */
177 
178  /* put U0,U1,U2 into plane equation 1 to compute signed distances to the
179  * plane*/
180  du0 = DOT(N1, U0) + d1;
181  du1 = DOT(N1, U1) + d1;
182  du2 = DOT(N1, U2) + d1;
183 
184  /* coplanarity robustness check */
185 #if USE_EPSILON_TEST == TRUE
186  if (fabs(du0) < EPSILON)
187  du0 = 0.0;
188  if (fabs(du1) < EPSILON)
189  du1 = 0.0;
190  if (fabs(du2) < EPSILON)
191  du2 = 0.0;
192  if (du1 == 0 && du2 == 0 && fabs(du0) < 1e-4)
193  du0 = 0.0;
194  if (du0 == 0 && du2 == 0 && fabs(du1) < 1e-4)
195  du1 = 0.0;
196  if (du0 == 0 && du1 == 0 && fabs(du2) < 1e-4)
197  du2 = 0.0;
198 #endif
199  du0du1 = du0 * du1;
200  du0du2 = du0 * du2;
201  du1du2 = du1 * du2;
202 
203  if (du0du1 > 0.0f && du0du2 > 0.0f)
204  { /* same sign on all of them + not equal 0 ? */
205  return NO_CONTACT; /* no intersection occurs */
206  }
207  /* compute plane of triangle (U0,U1,U2) */
208  SUB(E1, U1, U0);
209  SUB(E2, U2, U0);
210  CROSS(N2, E1, E2);
211  d2 = -DOT(N2, U0);
212  /* plane equation 2: N2.X+d2=0 */
213 
214  /* put V0,V1,V2 into plane equation 2 */
215  dv0 = DOT(N2, V0) + d2;
216  dv1 = DOT(N2, V1) + d2;
217  dv2 = DOT(N2, V2) + d2;
218 
219 #if USE_EPSILON_TEST == TRUE
220  if (fabs(dv0) < EPSILON)
221  dv0 = 0.0;
222  if (fabs(dv1) < EPSILON)
223  dv1 = 0.0;
224  if (fabs(dv2) < EPSILON)
225  dv2 = 0.0;
226  if (dv1 == 0 && dv2 == 0 && fabs(dv0) < 1e-5)
227  dv0 = 0.0;
228  if (dv0 == 0 && dv2 == 0 && fabs(dv1) < 1e-5)
229  dv1 = 0.0;
230  if (dv0 == 0 && dv1 == 0 && fabs(dv2) < 1e-5)
231  dv2 = 0.0;
232 #endif
233  dv0dv1 = dv0 * dv1;
234  dv0dv2 = dv0 * dv2;
235  dv1dv2 = dv1 * dv2;
236 
237  if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
238  { /* same sign on all of them + not equal 0 ? */
239  return NO_CONTACT; /* no intersection occurs */
240  }
241  /* compute direction of intersection line */
242  CROSS(D, N1, N2);
243 
244  /* compute and index to the largest component of D */
245  max = fabs(D[0]);
246  index = 0;
247  b = fabs(D[1]);
248  c = fabs(D[2]);
249  if (b > max)
250  max = b, index = 1;
251  if (c > max)
252  max = c, index = 2;
253 
254  /* this is the simplified projection onto L*/
255  vp0 = V0[index];
256  vp1 = V1[index];
257  vp2 = V2[index];
258 
259  up0 = U0[index];
260  up1 = U1[index];
261  up2 = U2[index];
262 
263  /* compute interval for triangle 1 */
265  vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, isect1[0], isect1[1]);
266 
267  /* compute interval for triangle 2 */
269  up0, up1, up2, du0, du1, du2, du0du1, du0du2, isect2[0], isect2[1]);
270 
271  SORT(isect1[0], isect1[1]);
272  SORT(isect2[0], isect2[1]);
273 
274  // if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return NO_CONTACT;
275 
276  float res[4][3];
277  if (du0du1 > 0)
278  {
279  edge_tri_intersect(U2, U0, du2, du0, res[0]);
280  edge_tri_intersect(U2, U1, du2, du1, res[1]);
281  }
282  else if (du0du2 > 0)
283  {
284  edge_tri_intersect(U1, U0, du1, du0, res[0]);
285  edge_tri_intersect(U1, U2, du1, du2, res[1]);
286  }
287  else if (du1du2 > 0)
288  {
289  edge_tri_intersect(U0, U1, du0, du1, res[0]);
290  edge_tri_intersect(U0, U2, du0, du2, res[1]);
291  }
292  else if (du0 == 0)
293  {
294  SET(res[0], U0);
295  edge_tri_intersect(U1, U2, du1, du2, res[1]);
296  }
297  else if (du1 == 0)
298  {
299  SET(res[0], U1);
300  edge_tri_intersect(U0, U2, du0, du2, res[1]);
301  }
302  else if (du2 == 0)
303  {
304  SET(res[0], U2);
305  edge_tri_intersect(U0, U1, du0, du1, res[1]);
306  }
307  else
308  {
309  std::cerr << "contact error" << std::endl;
310  }
311 
312  if (dv0dv1 > 0)
313  {
314  edge_tri_intersect(V2, V0, dv2, dv0, res[2]);
315  edge_tri_intersect(V2, V1, dv2, dv1, res[3]);
316  }
317  else if (dv0dv2 > 0)
318  {
319  edge_tri_intersect(V1, V0, dv1, dv0, res[2]);
320  edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
321  }
322  else if (dv1dv2 > 0)
323  {
324  edge_tri_intersect(V0, V1, dv0, dv1, res[2]);
325  edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
326  }
327  else if (dv0 == 0)
328  {
329  SET(res[2], V0);
330  edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
331  }
332  else if (dv1 == 0)
333  {
334  SET(res[2], V1);
335  edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
336  }
337  else if (dv2 == 0)
338  {
339  SET(res[2], V2);
340  edge_tri_intersect(V0, V1, dv0, dv1, res[3]);
341  }
342  else
343  {
344  std::cerr << "contact error" << std::endl;
345  }
346 
347  for (int i = 3; i > 0; i--)
348  for (int j = 0; j < i; j++)
349  {
350  if (res[j][index] > res[j + 1][index])
351  {
352  for (int k = 0; k < 3; k++)
353  SWAP(res[j][k], res[j + 1][k]);
354  }
355  }
356  SET(res1, res[1]);
357  SET(res2, res[2]);
358 
359  return 1;
360 }
361 
362 #endif // DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
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