DART 6.7.3
Loading...
Searching...
No Matches
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
123inline 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
135inline 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