DART  6.7.3
tri_tri_intersection_test.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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) dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2];
60 
61 #define ADD(dest,v1,v2) dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2];
62 
63 #define MULT(dest,v,factor) dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2];
64 
65 #define DIV(dest,v1,v2) dest[0]=v1[0]/v2[0]; dest[1]=v1[1]/2[1]; dest[2]=v1[2]/v2[2];
66 
67 #define SET(dest,src) dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2];
68 
69 /* sort so that a<=b */
70 #define SORT(a,b) \
71  if(a>b) \
72  { \
73  float c; \
74  c=a; \
75  a=b; \
76  b=c; \
77  }
78 
79 #define SWAP(a,b) \
80  { \
81  float c; \
82  c=a; \
83  a=b; \
84  b=c; \
85  }
86 
87 #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
88  isect0=VV0+(VV1-VV0)*D0/(D0-D1); \
89  isect1=VV0+(VV2-VV0)*D0/(D0-D2);
90 
91 
92 #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
93  if(D0D1>0.0f) \
94  { \
95  /* here we know that D0D2<=0.0 */ \
96  /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
97  ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
98  } \
99  else if(D0D2>0.0f) \
100  { \
101  /* here we know that d0d1<=0.0 */ \
102  ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
103  } \
104  else if(D1*D2>0.0f || D0!=0.0f) \
105  { \
106  /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
107  ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
108  } \
109  else if(D1!=0.0f) \
110  { \
111  ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
112  } \
113  else if(D2!=0.0f) \
114  { \
115  ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
116  } \
117  else \
118  { \
119  /* triangles are coplanar */ \
120  return COPLANAR_CONTACT; \
121  }
122 
123 inline void edge_tri_intersect(float V0[3], float V1[3], float DV0, float DV1, float V[3])
124 {
125  float VV0[3], VV1[3];
126  MULT(VV0, V1, DV0);
127  MULT(VV1, V0, DV1);
128  float U[3],D;
129  SUB(U, VV0, VV1);
130  D = DV0 - DV1;
131  MULT(V, U, 1.0/D);
132 
133 }
134 
135 inline int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
136  float U0[3],float U1[3],float U2[3], float res1[3], float res2[3])
137 {
138  float E1[3],E2[3];
139  float N1[3],N2[3],d1,d2;
140  float du0,du1,du2,dv0,dv1,dv2;
141  float D[3];
142  float isect1[2], isect2[2];
143  float du0du1,du0du2,dv0dv1,dv0dv2,du1du2,dv1dv2;
144  short index;
145  float vp0,vp1,vp2;
146  float up0,up1,up2;
147  float b,c,max;
148 
149  /* compute plane equation of triangle(V0,V1,V2) */
150  SUB(E1,V1,V0);
151  SUB(E2,V2,V0);
152  CROSS(N1,E1,E2);
153  d1=-DOT(N1,V0);
154  /* plane equation 1: N1.X+d1=0 */
155 
156  /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
157  du0=DOT(N1,U0)+d1;
158  du1=DOT(N1,U1)+d1;
159  du2=DOT(N1,U2)+d1;
160 
161  /* coplanarity robustness check */
162 #if USE_EPSILON_TEST==TRUE
163  if(fabs(du0)<EPSILON) du0=0.0;
164  if(fabs(du1)<EPSILON) du1=0.0;
165  if(fabs(du2)<EPSILON) du2=0.0;
166  if (du1 == 0 && du2 == 0 && fabs(du0) < 1e-4)
167  du0 = 0.0;
168  if (du0 == 0 && du2 == 0 && fabs(du1) < 1e-4)
169  du1 = 0.0;
170  if (du0 == 0 && du1 == 0 && fabs(du2) < 1e-4)
171  du2 = 0.0;
172 #endif
173  du0du1=du0*du1;
174  du0du2=du0*du2;
175  du1du2=du1*du2;
176 
177  if(du0du1>0.0f && du0du2>0.0f) { /* same sign on all of them + not equal 0 ? */
178  return NO_CONTACT; /* no intersection occurs */
179  }
180  /* compute plane of triangle (U0,U1,U2) */
181  SUB(E1,U1,U0);
182  SUB(E2,U2,U0);
183  CROSS(N2,E1,E2);
184  d2=-DOT(N2,U0);
185  /* plane equation 2: N2.X+d2=0 */
186 
187  /* put V0,V1,V2 into plane equation 2 */
188  dv0=DOT(N2,V0)+d2;
189  dv1=DOT(N2,V1)+d2;
190  dv2=DOT(N2,V2)+d2;
191 
192 #if USE_EPSILON_TEST==TRUE
193  if(fabs(dv0)<EPSILON) dv0=0.0;
194  if(fabs(dv1)<EPSILON) dv1=0.0;
195  if(fabs(dv2)<EPSILON) dv2=0.0;
196  if (dv1 == 0 && dv2 == 0 && fabs(dv0) < 1e-5)
197  dv0 = 0.0;
198  if (dv0 == 0 && dv2 == 0 && fabs(dv1) < 1e-5)
199  dv1 = 0.0;
200  if (dv0 == 0 && dv1 == 0 && fabs(dv2) < 1e-5)
201  dv2 = 0.0;
202 #endif
203  dv0dv1=dv0*dv1;
204  dv0dv2=dv0*dv2;
205  dv1dv2=dv1*dv2;
206 
207  if(dv0dv1>0.0f && dv0dv2>0.0f) { /* same sign on all of them + not equal 0 ? */
208  return NO_CONTACT; /* no intersection occurs */
209  }
210  /* compute direction of intersection line */
211  CROSS(D,N1,N2);
212 
213  /* compute and index to the largest component of D */
214  max=fabs(D[0]);
215  index=0;
216  b=fabs(D[1]);
217  c=fabs(D[2]);
218  if(b>max) max=b,index=1;
219  if(c>max) max=c,index=2;
220 
221  /* this is the simplified projection onto L*/
222  vp0=V0[index];
223  vp1=V1[index];
224  vp2=V2[index];
225 
226  up0=U0[index];
227  up1=U1[index];
228  up2=U2[index];
229 
230  /* compute interval for triangle 1 */
231  COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
232 
233  /* compute interval for triangle 2 */
234  COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
235 
236  SORT(isect1[0],isect1[1]);
237  SORT(isect2[0],isect2[1]);
238 
239  //if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return NO_CONTACT;
240 
241  float res[4][3];
242  if(du0du1>0)
243  {
244  edge_tri_intersect(U2, U0, du2, du0, res[0]);
245  edge_tri_intersect(U2, U1, du2, du1, res[1]);
246  }
247  else if(du0du2>0)
248  {
249  edge_tri_intersect(U1, U0, du1, du0, res[0]);
250  edge_tri_intersect(U1, U2, du1, du2, res[1]);
251  }
252  else if(du1du2>0)
253  {
254  edge_tri_intersect(U0, U1, du0, du1, res[0]);
255  edge_tri_intersect(U0, U2, du0, du2, res[1]);
256  }
257  else if(du0==0)
258  {
259  SET(res[0], U0);
260  edge_tri_intersect(U1, U2, du1, du2, res[1]);
261  }
262  else if(du1==0)
263  {
264  SET(res[0], U1);
265  edge_tri_intersect(U0, U2, du0, du2, res[1]);
266  }
267  else if(du2==0)
268  {
269  SET(res[0], U2);
270  edge_tri_intersect(U0, U1, du0, du1, res[1]);
271  }
272  else
273  {
274  std::cerr << "contact error" << std::endl;
275  }
276 
277  if(dv0dv1>0)
278  {
279  edge_tri_intersect(V2, V0, dv2, dv0, res[2]);
280  edge_tri_intersect(V2, V1, dv2, dv1, res[3]);
281  }
282  else if(dv0dv2>0)
283  {
284  edge_tri_intersect(V1, V0, dv1, dv0, res[2]);
285  edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
286  }
287  else if(dv1dv2>0)
288  {
289  edge_tri_intersect(V0, V1, dv0, dv1, res[2]);
290  edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
291  }
292  else if(dv0==0)
293  {
294  SET(res[2], V0);
295  edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
296  }
297  else if(dv1==0)
298  {
299  SET(res[2], V1);
300  edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
301  }
302  else if(dv2==0)
303  {
304  SET(res[2], V2);
305  edge_tri_intersect(V0, V1, dv0, dv1, res[3]);
306  }
307  else
308  {
309  std::cerr << "contact error" << std::endl;
310  }
311 
312  for (int i=3;i>0;i--)
313  for (int j=0;j<i;j++)
314  {
315  if(res[j][index]>res[j+1][index])
316  {
317  for (int k=0;k<3;k++)
318  SWAP(res[j][k], res[j+1][k]);
319  }
320  }
321  SET(res1, res[1]);
322  SET(res2, res[2]);
323 
324  return 1;
325 }
326 
327 #endif // DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
std::size_t index
Definition: SkelParser.cpp:1617
#define EPSILON
Definition: tri_tri_intersection_test.hpp:45
#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:123
#define DOT(v1, v2)
Definition: tri_tri_intersection_test.hpp:57
#define SET(dest, src)
Definition: tri_tri_intersection_test.hpp:67
#define SWAP(a, b)
Definition: tri_tri_intersection_test.hpp:79
#define COMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, isect1)
Definition: tri_tri_intersection_test.hpp:92
#define SORT(a, b)
Definition: tri_tri_intersection_test.hpp:70
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:135
#define MULT(dest, v, factor)
Definition: tri_tri_intersection_test.hpp:63