OPC_TriBoxOverlap.h
Go to the documentation of this file.
1 
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 
11 inline_ 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 
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 
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 
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 
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 
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 
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 
105 
117 inline_ 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 
198 inline_ 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 
259 inline_ 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 }
FALSE
#define FALSE
Definition: OPC_IceHook.h:9
AABBTreeCollider::mLeafVerts
Point mLeafVerts[3]
Triangle vertices.
Definition: OPC_TreeCollider.h:215
hrp::min
static int min(int a, int b)
Definition: MatrixSolvers.cpp:55
planeBoxOverlap
inline_ BOOL planeBoxOverlap(const Point &normal, const float d, const Point &maxbox)
TO BE DOCUMENTED.
Definition: OPC_TriBoxOverlap.h:11
AABBTreeCollider::TriBoxOverlap
inline_ BOOL TriBoxOverlap(const Point &center, const Point &extents)
Definition: OPC_TriBoxOverlap.h:118
AABBTreeCollider::mFullPrimBoxTest
bool mFullPrimBoxTest
Perform full Primitive-BV tests (true) or SAT-lite tests (false)
Definition: OPC_TreeCollider.h:219
FINDMINMAX
#define FINDMINMAX(x0, x1, x2, min, max)
This macro quickly finds the min & max values among 3 variables.
Definition: OPC_TriBoxOverlap.h:3
TRUE
#define TRUE
Definition: OPC_IceHook.h:13
inline_
#define inline_
Definition: IcePreprocessor.h:103
Point::z
float z
Definition: IcePoint.h:524
Point
Definition: IcePoint.h:25
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
Point::x
float x
Definition: IcePoint.h:524
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
AABBTreeCollider::mNbBVPrimTests
udword mNbBVPrimTests
Number of BV-Primitive tests.
Definition: OPC_TreeCollider.h:196
Point::y
float y
Definition: IcePoint.h:524
FCMin3
inline_ float FCMin3(float a, float b, float c)
A global function to find MIN(a,b,c) using FCOMI/FCMOV.
Definition: IceFPU.h:281
IMPLEMENT_CLASS3_TESTS
#define IMPLEMENT_CLASS3_TESTS
Definition: OPC_TriBoxOverlap.h:78
FCMax3
inline_ float FCMax3(float a, float b, float c)
A global function to find MAX(a,b,c) using FCOMI/FCMOV.
Definition: IceFPU.h:261
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04