00001 #ifndef BT_BOX_COLLISION_H_INCLUDED
00002 #define BT_BOX_COLLISION_H_INCLUDED
00003
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "LinearMath/btTransform.h"
00028
00029
00031 #define BT_SWAP_NUMBERS(a,b){ \
00032 a = a+b; \
00033 b = a-b; \
00034 a = a-b; \
00035 }\
00036
00037
00038 #define BT_MAX(a,b) (a<b?b:a)
00039 #define BT_MIN(a,b) (a>b?b:a)
00040
00041 #define BT_GREATER(x, y) btFabs(x) > (y)
00042
00043 #define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
00044 #define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
00045
00046
00047
00048
00049
00050
00051 enum eBT_PLANE_INTERSECTION_TYPE
00052 {
00053 BT_CONST_BACK_PLANE = 0,
00054 BT_CONST_COLLIDE_PLANE,
00055 BT_CONST_FRONT_PLANE
00056 };
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119 #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
00120 {\
00121 const btScalar dir0 = -edge[i_dir_0];\
00122 const btScalar dir1 = edge[i_dir_1];\
00123 btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
00124 btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
00125 if(pmin>pmax)\
00126 {\
00127 BT_SWAP_NUMBERS(pmin,pmax); \
00128 }\
00129 const btScalar abs_dir0 = absolute_edge[i_dir_0];\
00130 const btScalar abs_dir1 = absolute_edge[i_dir_1];\
00131 const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
00132 if(pmin>rad || -rad>pmax) return false;\
00133 }\
00134
00135
00136 #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00137 {\
00138 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
00139 }\
00140
00141 #define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00142 {\
00143 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
00144 }\
00145
00146 #define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00147 {\
00148 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
00149 }\
00150
00151
00153 SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
00154 const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
00155 {
00156 return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
00157 }
00158
00159
00161 ATTRIBUTE_ALIGNED16 (class) BT_BOX_BOX_TRANSFORM_CACHE
00162 {
00163 public:
00164 btVector3 m_T1to0;
00165 btMatrix3x3 m_R1to0;
00166 btMatrix3x3 m_AR;
00167
00168 SIMD_FORCE_INLINE void calc_absolute_matrix()
00169 {
00170
00171
00172
00173
00174
00175 int i,j;
00176
00177 for(i=0;i<3;i++)
00178 {
00179 for(j=0;j<3;j++ )
00180 {
00181 m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
00182 }
00183 }
00184
00185 }
00186
00187 BT_BOX_BOX_TRANSFORM_CACHE()
00188 {
00189 }
00190
00191
00192
00194 SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
00195 {
00196
00197 btTransform temp_trans = trans0.inverse();
00198 temp_trans = temp_trans * trans1;
00199
00200 m_T1to0 = temp_trans.getOrigin();
00201 m_R1to0 = temp_trans.getBasis();
00202
00203
00204 calc_absolute_matrix();
00205 }
00206
00208 SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
00209 {
00210 m_R1to0 = trans0.getBasis().inverse();
00211 m_T1to0 = m_R1to0 * (-trans0.getOrigin());
00212
00213 m_T1to0 += m_R1to0*trans1.getOrigin();
00214 m_R1to0 *= trans1.getBasis();
00215
00216 calc_absolute_matrix();
00217 }
00218
00219 SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
00220 {
00221 return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0;
00222 }
00223 };
00224
00225
00226 #define BOX_PLANE_EPSILON 0.000001f
00227
00229 ATTRIBUTE_ALIGNED16 (class) btAABB
00230 {
00231 public:
00232 btVector3 m_min;
00233 btVector3 m_max;
00234
00235 btAABB()
00236 {}
00237
00238
00239 btAABB(const btVector3 & V1,
00240 const btVector3 & V2,
00241 const btVector3 & V3)
00242 {
00243 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
00244 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
00245 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
00246
00247 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
00248 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
00249 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
00250 }
00251
00252 btAABB(const btVector3 & V1,
00253 const btVector3 & V2,
00254 const btVector3 & V3,
00255 btScalar margin)
00256 {
00257 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
00258 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
00259 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
00260
00261 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
00262 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
00263 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
00264
00265 m_min[0] -= margin;
00266 m_min[1] -= margin;
00267 m_min[2] -= margin;
00268 m_max[0] += margin;
00269 m_max[1] += margin;
00270 m_max[2] += margin;
00271 }
00272
00273 btAABB(const btAABB &other):
00274 m_min(other.m_min),m_max(other.m_max)
00275 {
00276 }
00277
00278 btAABB(const btAABB &other,btScalar margin ):
00279 m_min(other.m_min),m_max(other.m_max)
00280 {
00281 m_min[0] -= margin;
00282 m_min[1] -= margin;
00283 m_min[2] -= margin;
00284 m_max[0] += margin;
00285 m_max[1] += margin;
00286 m_max[2] += margin;
00287 }
00288
00289 SIMD_FORCE_INLINE void invalidate()
00290 {
00291 m_min[0] = SIMD_INFINITY;
00292 m_min[1] = SIMD_INFINITY;
00293 m_min[2] = SIMD_INFINITY;
00294 m_max[0] = -SIMD_INFINITY;
00295 m_max[1] = -SIMD_INFINITY;
00296 m_max[2] = -SIMD_INFINITY;
00297 }
00298
00299 SIMD_FORCE_INLINE void increment_margin(btScalar margin)
00300 {
00301 m_min[0] -= margin;
00302 m_min[1] -= margin;
00303 m_min[2] -= margin;
00304 m_max[0] += margin;
00305 m_max[1] += margin;
00306 m_max[2] += margin;
00307 }
00308
00309 SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
00310 {
00311 m_min[0] = other.m_min[0] - margin;
00312 m_min[1] = other.m_min[1] - margin;
00313 m_min[2] = other.m_min[2] - margin;
00314
00315 m_max[0] = other.m_max[0] + margin;
00316 m_max[1] = other.m_max[1] + margin;
00317 m_max[2] = other.m_max[2] + margin;
00318 }
00319
00320 template<typename CLASS_POINT>
00321 SIMD_FORCE_INLINE void calc_from_triangle(
00322 const CLASS_POINT & V1,
00323 const CLASS_POINT & V2,
00324 const CLASS_POINT & V3)
00325 {
00326 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
00327 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
00328 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
00329
00330 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
00331 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
00332 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
00333 }
00334
00335 template<typename CLASS_POINT>
00336 SIMD_FORCE_INLINE void calc_from_triangle_margin(
00337 const CLASS_POINT & V1,
00338 const CLASS_POINT & V2,
00339 const CLASS_POINT & V3, btScalar margin)
00340 {
00341 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
00342 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
00343 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
00344
00345 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
00346 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
00347 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
00348
00349 m_min[0] -= margin;
00350 m_min[1] -= margin;
00351 m_min[2] -= margin;
00352 m_max[0] += margin;
00353 m_max[1] += margin;
00354 m_max[2] += margin;
00355 }
00356
00358 SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
00359 {
00360 btVector3 center = (m_max+m_min)*0.5f;
00361 btVector3 extends = m_max - center;
00362
00363 center = trans(center);
00364
00365 btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
00366 trans.getBasis().getRow(1).absolute(),
00367 trans.getBasis().getRow(2).absolute());
00368
00369 m_min = center - textends;
00370 m_max = center + textends;
00371 }
00372
00373
00375 SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
00376 {
00377 btVector3 center = (m_max+m_min)*0.5f;
00378 btVector3 extends = m_max - center;
00379
00380 center = trans.transform(center);
00381
00382 btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(),
00383 trans.m_R1to0.getRow(1).absolute(),
00384 trans.m_R1to0.getRow(2).absolute());
00385
00386 m_min = center - textends;
00387 m_max = center + textends;
00388 }
00389
00391 SIMD_FORCE_INLINE void merge(const btAABB & box)
00392 {
00393 m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
00394 m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
00395 m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
00396
00397 m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
00398 m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
00399 m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
00400 }
00401
00403 template<typename CLASS_POINT>
00404 SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
00405 {
00406 m_min[0] = BT_MIN(m_min[0],point[0]);
00407 m_min[1] = BT_MIN(m_min[1],point[1]);
00408 m_min[2] = BT_MIN(m_min[2],point[2]);
00409
00410 m_max[0] = BT_MAX(m_max[0],point[0]);
00411 m_max[1] = BT_MAX(m_max[1],point[1]);
00412 m_max[2] = BT_MAX(m_max[2],point[2]);
00413 }
00414
00416 SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const
00417 {
00418 center = (m_max+m_min)*0.5f;
00419 extend = m_max - center;
00420 }
00421
00423 SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection) const
00424 {
00425 intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
00426 intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
00427 intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
00428
00429 intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
00430 intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
00431 intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
00432 }
00433
00434
00435 SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
00436 {
00437 if(m_min[0] > other.m_max[0] ||
00438 m_max[0] < other.m_min[0] ||
00439 m_min[1] > other.m_max[1] ||
00440 m_max[1] < other.m_min[1] ||
00441 m_min[2] > other.m_max[2] ||
00442 m_max[2] < other.m_min[2])
00443 {
00444 return false;
00445 }
00446 return true;
00447 }
00448
00454 SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) const
00455 {
00456 btVector3 extents,center;
00457 this->get_center_extend(center,extents);;
00458
00459 btScalar Dx = vorigin[0] - center[0];
00460 if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false;
00461 btScalar Dy = vorigin[1] - center[1];
00462 if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false;
00463 btScalar Dz = vorigin[2] - center[2];
00464 if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false;
00465
00466
00467 btScalar f = vdir[1] * Dz - vdir[2] * Dy;
00468 if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
00469 f = vdir[2] * Dx - vdir[0] * Dz;
00470 if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
00471 f = vdir[0] * Dy - vdir[1] * Dx;
00472 if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
00473 return true;
00474 }
00475
00476
00477 SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
00478 {
00479 btVector3 center = (m_max+m_min)*0.5f;
00480 btVector3 extend = m_max-center;
00481
00482 btScalar _fOrigin = direction.dot(center);
00483 btScalar _fMaximumExtent = extend.dot(direction.absolute());
00484 vmin = _fOrigin - _fMaximumExtent;
00485 vmax = _fOrigin + _fMaximumExtent;
00486 }
00487
00488 SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
00489 {
00490 btScalar _fmin,_fmax;
00491 this->projection_interval(plane,_fmin,_fmax);
00492
00493 if(plane[3] > _fmax + BOX_PLANE_EPSILON)
00494 {
00495 return BT_CONST_BACK_PLANE;
00496 }
00497
00498 if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
00499 {
00500 return BT_CONST_COLLIDE_PLANE;
00501 }
00502 return BT_CONST_FRONT_PLANE;
00503 }
00504
00505 SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
00506 {
00507 btAABB tbox = box;
00508 tbox.appy_transform(trans1_to_0);
00509 return has_collision(tbox);
00510 }
00511
00512 SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
00513 const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
00514 {
00515 btAABB tbox = box;
00516 tbox.appy_transform_trans_cache(trans1_to_0);
00517 return has_collision(tbox);
00518 }
00519
00521 SIMD_FORCE_INLINE bool overlapping_trans_cache(
00522 const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
00523 {
00524
00525
00526 btVector3 ea,eb;
00527 btVector3 ca,cb;
00528 get_center_extend(ca,ea);
00529 box.get_center_extend(cb,eb);
00530
00531
00532 btVector3 T;
00533 btScalar t,t2;
00534 int i;
00535
00536
00537 for(i=0;i<3;i++)
00538 {
00539 T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
00540 t = transcache.m_AR[i].dot(eb) + ea[i];
00541 if(BT_GREATER(T[i], t)) return false;
00542 }
00543
00544 for(i=0;i<3;i++)
00545 {
00546 t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
00547 t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
00548 if(BT_GREATER(t,t2)) return false;
00549 }
00550
00551 if(fulltest)
00552 {
00553 int j,m,n,o,p,q,r;
00554 for(i=0;i<3;i++)
00555 {
00556 m = (i+1)%3;
00557 n = (i+2)%3;
00558 o = i==0?1:0;
00559 p = i==2?1:2;
00560 for(j=0;j<3;j++)
00561 {
00562 q = j==2?1:2;
00563 r = j==0?1:0;
00564 t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
00565 t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
00566 eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
00567 if(BT_GREATER(t,t2)) return false;
00568 }
00569 }
00570 }
00571 return true;
00572 }
00573
00575 SIMD_FORCE_INLINE bool collide_plane(
00576 const btVector4 & plane) const
00577 {
00578 eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
00579 return (classify == BT_CONST_COLLIDE_PLANE);
00580 }
00581
00583 SIMD_FORCE_INLINE bool collide_triangle_exact(
00584 const btVector3 & p1,
00585 const btVector3 & p2,
00586 const btVector3 & p3,
00587 const btVector4 & triangle_plane) const
00588 {
00589 if(!collide_plane(triangle_plane)) return false;
00590
00591 btVector3 center,extends;
00592 this->get_center_extend(center,extends);
00593
00594 const btVector3 v1(p1 - center);
00595 const btVector3 v2(p2 - center);
00596 const btVector3 v3(p3 - center);
00597
00598
00599 btVector3 diff(v2 - v1);
00600 btVector3 abs_diff = diff.absolute();
00601
00602 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00603
00604 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00605
00606 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00607
00608
00609 diff = v3 - v2;
00610 abs_diff = diff.absolute();
00611
00612 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00613
00614 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00615
00616 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00617
00618 diff = v1 - v3;
00619 abs_diff = diff.absolute();
00620
00621 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00622
00623 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00624
00625 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00626
00627 return true;
00628 }
00629 };
00630
00631
00633 SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
00634 {
00635 if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
00636
00637 if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
00638 if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
00639 if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
00640 return true;
00641 }
00642
00643
00644
00645 #endif // GIM_BOX_COLLISION_H_INCLUDED