00001 #ifndef GIM_BOX_COLLISION_H_INCLUDED
00002 #define GIM_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
00028
00029
00030
00031
00032
00033
00034
00035 #include "gim_basic_geometry_operations.h"
00036 #include "LinearMath/btTransform.h"
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
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 #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
00101 {\
00102 const btScalar dir0 = -edge[i_dir_0];\
00103 const btScalar dir1 = edge[i_dir_1];\
00104 btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
00105 btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
00106 if(pmin>pmax)\
00107 {\
00108 GIM_SWAP_NUMBERS(pmin,pmax); \
00109 }\
00110 const btScalar abs_dir0 = absolute_edge[i_dir_0];\
00111 const btScalar abs_dir1 = absolute_edge[i_dir_1];\
00112 const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
00113 if(pmin>rad || -rad>pmax) return false;\
00114 }\
00115
00116
00117 #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00118 {\
00119 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
00120 }\
00121
00122 #define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00123 {\
00124 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
00125 }\
00126
00127 #define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
00128 {\
00129 TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
00130 }\
00131
00132
00133
00135 class GIM_BOX_BOX_TRANSFORM_CACHE
00136 {
00137 public:
00138 btVector3 m_T1to0;
00139 btMatrix3x3 m_R1to0;
00140 btMatrix3x3 m_AR;
00141
00142 SIMD_FORCE_INLINE void calc_absolute_matrix()
00143 {
00144 static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
00145 m_AR[0] = vepsi + m_R1to0[0].absolute();
00146 m_AR[1] = vepsi + m_R1to0[1].absolute();
00147 m_AR[2] = vepsi + m_R1to0[2].absolute();
00148 }
00149
00150 GIM_BOX_BOX_TRANSFORM_CACHE()
00151 {
00152 }
00153
00154
00155 GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0)
00156 {
00157 COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
00158 MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
00159 calc_absolute_matrix();
00160 }
00161
00163 SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
00164 {
00165
00166 m_R1to0 = trans0.getBasis().transpose();
00167 m_T1to0 = m_R1to0 * (-trans0.getOrigin());
00168
00169 m_T1to0 += m_R1to0*trans1.getOrigin();
00170 m_R1to0 *= trans1.getBasis();
00171
00172 calc_absolute_matrix();
00173 }
00174
00176 SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
00177 {
00178 m_R1to0 = trans0.getBasis().inverse();
00179 m_T1to0 = m_R1to0 * (-trans0.getOrigin());
00180
00181 m_T1to0 += m_R1to0*trans1.getOrigin();
00182 m_R1to0 *= trans1.getBasis();
00183
00184 calc_absolute_matrix();
00185 }
00186
00187 SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
00188 {
00189 return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
00190 }
00191 };
00192
00193
00194 #define BOX_PLANE_EPSILON 0.000001f
00195
00197 class GIM_AABB
00198 {
00199 public:
00200 btVector3 m_min;
00201 btVector3 m_max;
00202
00203 GIM_AABB()
00204 {}
00205
00206
00207 GIM_AABB(const btVector3 & V1,
00208 const btVector3 & V2,
00209 const btVector3 & V3)
00210 {
00211 m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
00212 m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
00213 m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
00214
00215 m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
00216 m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
00217 m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
00218 }
00219
00220 GIM_AABB(const btVector3 & V1,
00221 const btVector3 & V2,
00222 const btVector3 & V3,
00223 GREAL margin)
00224 {
00225 m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
00226 m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
00227 m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
00228
00229 m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
00230 m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
00231 m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
00232
00233 m_min[0] -= margin;
00234 m_min[1] -= margin;
00235 m_min[2] -= margin;
00236 m_max[0] += margin;
00237 m_max[1] += margin;
00238 m_max[2] += margin;
00239 }
00240
00241 GIM_AABB(const GIM_AABB &other):
00242 m_min(other.m_min),m_max(other.m_max)
00243 {
00244 }
00245
00246 GIM_AABB(const GIM_AABB &other,btScalar margin ):
00247 m_min(other.m_min),m_max(other.m_max)
00248 {
00249 m_min[0] -= margin;
00250 m_min[1] -= margin;
00251 m_min[2] -= margin;
00252 m_max[0] += margin;
00253 m_max[1] += margin;
00254 m_max[2] += margin;
00255 }
00256
00257 SIMD_FORCE_INLINE void invalidate()
00258 {
00259 m_min[0] = G_REAL_INFINITY;
00260 m_min[1] = G_REAL_INFINITY;
00261 m_min[2] = G_REAL_INFINITY;
00262 m_max[0] = -G_REAL_INFINITY;
00263 m_max[1] = -G_REAL_INFINITY;
00264 m_max[2] = -G_REAL_INFINITY;
00265 }
00266
00267 SIMD_FORCE_INLINE void increment_margin(btScalar margin)
00268 {
00269 m_min[0] -= margin;
00270 m_min[1] -= margin;
00271 m_min[2] -= margin;
00272 m_max[0] += margin;
00273 m_max[1] += margin;
00274 m_max[2] += margin;
00275 }
00276
00277 SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
00278 {
00279 m_min[0] = other.m_min[0] - margin;
00280 m_min[1] = other.m_min[1] - margin;
00281 m_min[2] = other.m_min[2] - margin;
00282
00283 m_max[0] = other.m_max[0] + margin;
00284 m_max[1] = other.m_max[1] + margin;
00285 m_max[2] = other.m_max[2] + margin;
00286 }
00287
00288 template<typename CLASS_POINT>
00289 SIMD_FORCE_INLINE void calc_from_triangle(
00290 const CLASS_POINT & V1,
00291 const CLASS_POINT & V2,
00292 const CLASS_POINT & V3)
00293 {
00294 m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
00295 m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
00296 m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
00297
00298 m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
00299 m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
00300 m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
00301 }
00302
00303 template<typename CLASS_POINT>
00304 SIMD_FORCE_INLINE void calc_from_triangle_margin(
00305 const CLASS_POINT & V1,
00306 const CLASS_POINT & V2,
00307 const CLASS_POINT & V3, btScalar margin)
00308 {
00309 m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
00310 m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
00311 m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
00312
00313 m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
00314 m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
00315 m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
00316
00317 m_min[0] -= margin;
00318 m_min[1] -= margin;
00319 m_min[2] -= margin;
00320 m_max[0] += margin;
00321 m_max[1] += margin;
00322 m_max[2] += margin;
00323 }
00324
00326 SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
00327 {
00328 btVector3 center = (m_max+m_min)*0.5f;
00329 btVector3 extends = m_max - center;
00330
00331 center = trans(center);
00332
00333 btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
00334 trans.getBasis().getRow(1).absolute(),
00335 trans.getBasis().getRow(2).absolute());
00336
00337 m_min = center - textends;
00338 m_max = center + textends;
00339 }
00340
00342 SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
00343 {
00344 m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
00345 m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
00346 m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
00347
00348 m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
00349 m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
00350 m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
00351 }
00352
00354 template<typename CLASS_POINT>
00355 SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
00356 {
00357 m_min[0] = GIM_MIN(m_min[0],point[0]);
00358 m_min[1] = GIM_MIN(m_min[1],point[1]);
00359 m_min[2] = GIM_MIN(m_min[2],point[2]);
00360
00361 m_max[0] = GIM_MAX(m_max[0],point[0]);
00362 m_max[1] = GIM_MAX(m_max[1],point[1]);
00363 m_max[2] = GIM_MAX(m_max[2],point[2]);
00364 }
00365
00367 SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const
00368 {
00369 center = (m_max+m_min)*0.5f;
00370 extend = m_max - center;
00371 }
00372
00374 SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection) const
00375 {
00376 intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
00377 intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
00378 intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
00379
00380 intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
00381 intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
00382 intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
00383 }
00384
00385
00386 SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
00387 {
00388 if(m_min[0] > other.m_max[0] ||
00389 m_max[0] < other.m_min[0] ||
00390 m_min[1] > other.m_max[1] ||
00391 m_max[1] < other.m_min[1] ||
00392 m_min[2] > other.m_max[2] ||
00393 m_max[2] < other.m_min[2])
00394 {
00395 return false;
00396 }
00397 return true;
00398 }
00399
00405 SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
00406 {
00407 btVector3 extents,center;
00408 this->get_center_extend(center,extents);;
00409
00410 btScalar Dx = vorigin[0] - center[0];
00411 if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false;
00412 btScalar Dy = vorigin[1] - center[1];
00413 if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false;
00414 btScalar Dz = vorigin[2] - center[2];
00415 if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false;
00416
00417
00418 btScalar f = vdir[1] * Dz - vdir[2] * Dy;
00419 if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
00420 f = vdir[2] * Dx - vdir[0] * Dz;
00421 if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
00422 f = vdir[0] * Dy - vdir[1] * Dx;
00423 if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
00424 return true;
00425 }
00426
00427
00428 SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
00429 {
00430 btVector3 center = (m_max+m_min)*0.5f;
00431 btVector3 extend = m_max-center;
00432
00433 btScalar _fOrigin = direction.dot(center);
00434 btScalar _fMaximumExtent = extend.dot(direction.absolute());
00435 vmin = _fOrigin - _fMaximumExtent;
00436 vmax = _fOrigin + _fMaximumExtent;
00437 }
00438
00439 SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
00440 {
00441 btScalar _fmin,_fmax;
00442 this->projection_interval(plane,_fmin,_fmax);
00443
00444 if(plane[3] > _fmax + BOX_PLANE_EPSILON)
00445 {
00446 return G_BACK_PLANE;
00447 }
00448
00449 if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
00450 {
00451 return G_COLLIDE_PLANE;
00452 }
00453 return G_FRONT_PLANE;
00454 }
00455
00456 SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
00457 {
00458 GIM_AABB tbox = box;
00459 tbox.appy_transform(trans1_to_0);
00460 return has_collision(tbox);
00461 }
00462
00464 SIMD_FORCE_INLINE bool overlapping_trans_cache(
00465 const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
00466 {
00467
00468
00469 btVector3 ea,eb;
00470 btVector3 ca,cb;
00471 get_center_extend(ca,ea);
00472 box.get_center_extend(cb,eb);
00473
00474
00475 btVector3 T;
00476 btScalar t,t2;
00477 int i;
00478
00479
00480 for(i=0;i<3;i++)
00481 {
00482 T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
00483 t = transcache.m_AR[i].dot(eb) + ea[i];
00484 if(GIM_GREATER(T[i], t)) return false;
00485 }
00486
00487 for(i=0;i<3;i++)
00488 {
00489 t = MAT_DOT_COL(transcache.m_R1to0,T,i);
00490 t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
00491 if(GIM_GREATER(t,t2)) return false;
00492 }
00493
00494 if(fulltest)
00495 {
00496 int j,m,n,o,p,q,r;
00497 for(i=0;i<3;i++)
00498 {
00499 m = (i+1)%3;
00500 n = (i+2)%3;
00501 o = i==0?1:0;
00502 p = i==2?1:2;
00503 for(j=0;j<3;j++)
00504 {
00505 q = j==2?1:2;
00506 r = j==0?1:0;
00507 t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
00508 t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
00509 eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
00510 if(GIM_GREATER(t,t2)) return false;
00511 }
00512 }
00513 }
00514 return true;
00515 }
00516
00518 SIMD_FORCE_INLINE bool collide_plane(
00519 const btVector4 & plane)
00520 {
00521 ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
00522 return (classify == G_COLLIDE_PLANE);
00523 }
00524
00526 SIMD_FORCE_INLINE bool collide_triangle_exact(
00527 const btVector3 & p1,
00528 const btVector3 & p2,
00529 const btVector3 & p3,
00530 const btVector4 & triangle_plane)
00531 {
00532 if(!collide_plane(triangle_plane)) return false;
00533
00534 btVector3 center,extends;
00535 this->get_center_extend(center,extends);
00536
00537 const btVector3 v1(p1 - center);
00538 const btVector3 v2(p2 - center);
00539 const btVector3 v3(p3 - center);
00540
00541
00542 btVector3 diff(v2 - v1);
00543 btVector3 abs_diff = diff.absolute();
00544
00545 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00546
00547 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00548
00549 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00550
00551
00552 diff = v3 - v2;
00553 abs_diff = diff.absolute();
00554
00555 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00556
00557 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00558
00559 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00560
00561 diff = v1 - v3;
00562 abs_diff = diff.absolute();
00563
00564 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00565
00566 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00567
00568 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00569
00570 return true;
00571 }
00572 };
00573
00574
00576 SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
00577 {
00578 if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
00579
00580 if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
00581 if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
00582 if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
00583 return true;
00584 }
00585
00586
00587
00588 #endif // GIM_BOX_COLLISION_H_INCLUDED