aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h
parentadding ode source to /libraries (diff)
downloadopensim-SC-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h339
1 files changed, 339 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h b/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h
new file mode 100644
index 0000000..b3a9bde
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h
@@ -0,0 +1,339 @@
1
2//! This macro quickly finds the min & max values among 3 variables
3#define FINDMINMAX(x0, x1, x2, min, max) \
4 min = max = x0; \
5 if(x1<min) min=x1; \
6 if(x1>max) max=x1; \
7 if(x2<min) min=x2; \
8 if(x2>max) max=x2;
9
10//! TO BE DOCUMENTED
11inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
12{
13 Point vmin, vmax;
14 for(udword q=0;q<=2;q++)
15 {
16 if(normal[q]>0.0f) { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; }
17 else { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; }
18 }
19 if((normal|vmin)+d>0.0f) return FALSE;
20 if((normal|vmax)+d>=0.0f) return TRUE;
21
22 return FALSE;
23}
24
25//! TO BE DOCUMENTED
26#define AXISTEST_X01(a, b, fa, fb) \
27 min = a*v0.y - b*v0.z; \
28 max = a*v2.y - b*v2.z; \
29 if(min>max) {const float tmp=max; max=min; min=tmp; } \
30 rad = fa * extents.y + fb * extents.z; \
31 if(min>rad || max<-rad) return FALSE;
32
33//! TO BE DOCUMENTED
34#define AXISTEST_X2(a, b, fa, fb) \
35 min = a*v0.y - b*v0.z; \
36 max = a*v1.y - b*v1.z; \
37 if(min>max) {const float tmp=max; max=min; min=tmp; } \
38 rad = fa * extents.y + fb * extents.z; \
39 if(min>rad || max<-rad) return FALSE;
40
41//! TO BE DOCUMENTED
42#define AXISTEST_Y02(a, b, fa, fb) \
43 min = b*v0.z - a*v0.x; \
44 max = b*v2.z - a*v2.x; \
45 if(min>max) {const float tmp=max; max=min; min=tmp; } \
46 rad = fa * extents.x + fb * extents.z; \
47 if(min>rad || max<-rad) return FALSE;
48
49//! TO BE DOCUMENTED
50#define AXISTEST_Y1(a, b, fa, fb) \
51 min = b*v0.z - a*v0.x; \
52 max = b*v1.z - a*v1.x; \
53 if(min>max) {const float tmp=max; max=min; min=tmp; } \
54 rad = fa * extents.x + fb * extents.z; \
55 if(min>rad || max<-rad) return FALSE;
56
57//! TO BE DOCUMENTED
58#define AXISTEST_Z12(a, b, fa, fb) \
59 min = a*v1.x - b*v1.y; \
60 max = a*v2.x - b*v2.y; \
61 if(min>max) {const float tmp=max; max=min; min=tmp; } \
62 rad = fa * extents.x + fb * extents.y; \
63 if(min>rad || max<-rad) return FALSE;
64
65//! TO BE DOCUMENTED
66#define AXISTEST_Z0(a, b, fa, fb) \
67 min = a*v0.x - b*v0.y; \
68 max = a*v1.x - b*v1.y; \
69 if(min>max) {const float tmp=max; max=min; min=tmp; } \
70 rad = fa * extents.x + fb * extents.y; \
71 if(min>rad || max<-rad) return FALSE;
72
73// compute triangle edges
74// - edges lazy evaluated to take advantage of early exits
75// - fabs precomputed (half less work, possible since extents are always >0)
76// - customized macros to take advantage of the null component
77// - axis vector discarded, possibly saves useless movs
78#define IMPLEMENT_CLASS3_TESTS \
79 float rad; \
80 float min, max; \
81 \
82 const float fey0 = fabsf(e0.y); \
83 const float fez0 = fabsf(e0.z); \
84 AXISTEST_X01(e0.z, e0.y, fez0, fey0); \
85 const float fex0 = fabsf(e0.x); \
86 AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \
87 AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \
88 \
89 const float fey1 = fabsf(e1.y); \
90 const float fez1 = fabsf(e1.z); \
91 AXISTEST_X01(e1.z, e1.y, fez1, fey1); \
92 const float fex1 = fabsf(e1.x); \
93 AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \
94 AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \
95 \
96 const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
97 const float fey2 = fabsf(e2.y); \
98 const float fez2 = fabsf(e2.z); \
99 AXISTEST_X2(e2.z, e2.y, fez2, fey2); \
100 const float fex2 = fabsf(e2.x); \
101 AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \
102 AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
103
104///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
105/**
106 * Triangle-Box overlap test using the separating axis theorem.
107 * This is the code from Tomas Möller, a bit optimized:
108 * - with some more lazy evaluation (faster path on PC)
109 * - with a tiny bit of assembly
110 * - with "SAT-lite" applied if needed
111 * - and perhaps with some more minor modifs...
112 *
113 * \param center [in] box center
114 * \param extents [in] box extents
115 * \return true if triangle & box overlap
116 */
117///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
118inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
119{
120 // Stats
121 mNbBVPrimTests++;
122
123 // use separating axis theorem to test overlap between triangle and box
124 // need to test for overlap in these directions:
125 // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
126 // we do not even need to test these)
127 // 2) normal of the triangle
128 // 3) crossproduct(edge from tri, {x,y,z}-directin)
129 // this gives 3x3=9 more tests
130
131 // move everything so that the boxcenter is in (0,0,0)
132 Point v0, v1, v2;
133 v0.x = mLeafVerts[0].x - center.x;
134 v1.x = mLeafVerts[1].x - center.x;
135 v2.x = mLeafVerts[2].x - center.x;
136
137 // First, test overlap in the {x,y,z}-directions
138#ifdef OPC_USE_FCOMI
139 // find min, max of the triangle in x-direction, and test for overlap in X
140 if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE;
141 if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
142
143 // same for Y
144 v0.y = mLeafVerts[0].y - center.y;
145 v1.y = mLeafVerts[1].y - center.y;
146 v2.y = mLeafVerts[2].y - center.y;
147
148 if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE;
149 if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
150
151 // same for Z
152 v0.z = mLeafVerts[0].z - center.z;
153 v1.z = mLeafVerts[1].z - center.z;
154 v2.z = mLeafVerts[2].z - center.z;
155
156 if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE;
157 if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
158#else
159 float min,max;
160 // Find min, max of the triangle in x-direction, and test for overlap in X
161 FINDMINMAX(v0.x, v1.x, v2.x, min, max);
162 if(min>extents.x || max<-extents.x) return FALSE;
163
164 // Same for Y
165 v0.y = mLeafVerts[0].y - center.y;
166 v1.y = mLeafVerts[1].y - center.y;
167 v2.y = mLeafVerts[2].y - center.y;
168
169 FINDMINMAX(v0.y, v1.y, v2.y, min, max);
170 if(min>extents.y || max<-extents.y) return FALSE;
171
172 // Same for Z
173 v0.z = mLeafVerts[0].z - center.z;
174 v1.z = mLeafVerts[1].z - center.z;
175 v2.z = mLeafVerts[2].z - center.z;
176
177 FINDMINMAX(v0.z, v1.z, v2.z, min, max);
178 if(min>extents.z || max<-extents.z) return FALSE;
179#endif
180 // 2) Test if the box intersects the plane of the triangle
181 // compute plane equation of triangle: normal*x+d=0
182 // ### could be precomputed since we use the same leaf triangle several times
183 const Point e0 = v1 - v0;
184 const Point e1 = v2 - v1;
185 const Point normal = e0 ^ e1;
186 const float d = -normal|v0;
187 if(!planeBoxOverlap(normal, d, extents)) return FALSE;
188
189 // 3) "Class III" tests
190 if(mFullPrimBoxTest)
191 {
192 IMPLEMENT_CLASS3_TESTS
193 }
194 return TRUE;
195}
196
197//! A dedicated version where the box is constant
198inline_ BOOL OBBCollider::TriBoxOverlap()
199{
200 // Stats
201 mNbVolumePrimTests++;
202
203 // Hook
204 const Point& extents = mBoxExtents;
205 const Point& v0 = mLeafVerts[0];
206 const Point& v1 = mLeafVerts[1];
207 const Point& v2 = mLeafVerts[2];
208
209 // use separating axis theorem to test overlap between triangle and box
210 // need to test for overlap in these directions:
211 // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
212 // we do not even need to test these)
213 // 2) normal of the triangle
214 // 3) crossproduct(edge from tri, {x,y,z}-directin)
215 // this gives 3x3=9 more tests
216
217 // Box center is already in (0,0,0)
218
219 // First, test overlap in the {x,y,z}-directions
220#ifdef OPC_USE_FCOMI
221 // find min, max of the triangle in x-direction, and test for overlap in X
222 if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x) return FALSE;
223 if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x) return FALSE;
224
225 if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y) return FALSE;
226 if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y) return FALSE;
227
228 if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z) return FALSE;
229 if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z) return FALSE;
230#else
231 float min,max;
232 // Find min, max of the triangle in x-direction, and test for overlap in X
233 FINDMINMAX(v0.x, v1.x, v2.x, min, max);
234 if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE;
235
236 FINDMINMAX(v0.y, v1.y, v2.y, min, max);
237 if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE;
238
239 FINDMINMAX(v0.z, v1.z, v2.z, min, max);
240 if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE;
241#endif
242 // 2) Test if the box intersects the plane of the triangle
243 // compute plane equation of triangle: normal*x+d=0
244 // ### could be precomputed since we use the same leaf triangle several times
245 const Point e0 = v1 - v0;
246 const Point e1 = v2 - v1;
247 const Point normal = e0 ^ e1;
248 const float d = -normal|v0;
249 if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
250
251 // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
252 {
253 IMPLEMENT_CLASS3_TESTS
254 }
255 return TRUE;
256}
257
258//! ...and another one, jeez
259inline_ BOOL AABBCollider::TriBoxOverlap()
260{
261 // Stats
262 mNbVolumePrimTests++;
263
264 // Hook
265 const Point& center = mBox.mCenter;
266 const Point& extents = mBox.mExtents;
267
268 // use separating axis theorem to test overlap between triangle and box
269 // need to test for overlap in these directions:
270 // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
271 // we do not even need to test these)
272 // 2) normal of the triangle
273 // 3) crossproduct(edge from tri, {x,y,z}-directin)
274 // this gives 3x3=9 more tests
275
276 // move everything so that the boxcenter is in (0,0,0)
277 Point v0, v1, v2;
278 v0.x = mLeafVerts[0].x - center.x;
279 v1.x = mLeafVerts[1].x - center.x;
280 v2.x = mLeafVerts[2].x - center.x;
281
282 // First, test overlap in the {x,y,z}-directions
283#ifdef OPC_USE_FCOMI
284 // find min, max of the triangle in x-direction, and test for overlap in X
285 if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE;
286 if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
287
288 // same for Y
289 v0.y = mLeafVerts[0].y - center.y;
290 v1.y = mLeafVerts[1].y - center.y;
291 v2.y = mLeafVerts[2].y - center.y;
292
293 if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE;
294 if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
295
296 // same for Z
297 v0.z = mLeafVerts[0].z - center.z;
298 v1.z = mLeafVerts[1].z - center.z;
299 v2.z = mLeafVerts[2].z - center.z;
300
301 if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE;
302 if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
303#else
304 float min,max;
305 // Find min, max of the triangle in x-direction, and test for overlap in X
306 FINDMINMAX(v0.x, v1.x, v2.x, min, max);
307 if(min>extents.x || max<-extents.x) return FALSE;
308
309 // Same for Y
310 v0.y = mLeafVerts[0].y - center.y;
311 v1.y = mLeafVerts[1].y - center.y;
312 v2.y = mLeafVerts[2].y - center.y;
313
314 FINDMINMAX(v0.y, v1.y, v2.y, min, max);
315 if(min>extents.y || max<-extents.y) return FALSE;
316
317 // Same for Z
318 v0.z = mLeafVerts[0].z - center.z;
319 v1.z = mLeafVerts[1].z - center.z;
320 v2.z = mLeafVerts[2].z - center.z;
321
322 FINDMINMAX(v0.z, v1.z, v2.z, min, max);
323 if(min>extents.z || max<-extents.z) return FALSE;
324#endif
325 // 2) Test if the box intersects the plane of the triangle
326 // compute plane equation of triangle: normal*x+d=0
327 // ### could be precomputed since we use the same leaf triangle several times
328 const Point e0 = v1 - v0;
329 const Point e1 = v2 - v1;
330 const Point normal = e0 ^ e1;
331 const float d = -normal|v0;
332 if(!planeBoxOverlap(normal, d, extents)) return FALSE;
333
334 // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
335 {
336 IMPLEMENT_CLASS3_TESTS
337 }
338 return TRUE;
339}
generated by cgit v1.1 at 2024-12-25 22:24:08 +0000