| 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 |
|---|
| 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 | |
|---|
| 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 118 | 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 | |
|---|
| 197 | //! A dedicated version where the box is constant |
|---|
| 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 | |
|---|
| 258 | //! ...and another one, jeez |
|---|
| 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 | } |
|---|
