btBoxCollision.h

Go to the documentation of this file.
00001 #ifndef BT_BOX_COLLISION_H_INCLUDED
00002 #define BT_BOX_COLLISION_H_INCLUDED
00003 
00007 /*
00008 This source file is part of GIMPACT Library.
00009 
00010 For the latest info, see http://gimpact.sourceforge.net/
00011 
00012 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00013 email: projectileman@yahoo.com
00014 
00015 
00016 This software is provided 'as-is', without any express or implied warranty.
00017 In no event will the authors be held liable for any damages arising from the use of this software.
00018 Permission is granted to anyone to use this software for any purpose,
00019 including commercial applications, and to alter it and redistribute it freely,
00020 subject to the following restrictions:
00021 
00022 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00023 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00024 3. This notice may not be removed or altered from any source distribution.
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 //SIMD_FORCE_INLINE bool test_cross_edge_box(
00059 //      const btVector3 & edge,
00060 //      const btVector3 & absolute_edge,
00061 //      const btVector3 & pointa,
00062 //      const btVector3 & pointb, const btVector3 & extend,
00063 //      int dir_index0,
00064 //      int dir_index1
00065 //      int component_index0,
00066 //      int component_index1)
00067 //{
00068 //      // dir coords are -z and y
00069 //
00070 //      const btScalar dir0 = -edge[dir_index0];
00071 //      const btScalar dir1 = edge[dir_index1];
00072 //      btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
00073 //      btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
00074 //      //find minmax
00075 //      if(pmin>pmax)
00076 //      {
00077 //              BT_SWAP_NUMBERS(pmin,pmax);
00078 //      }
00079 //      //find extends
00080 //      const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
00081 //                                      extend[component_index1] * absolute_edge[dir_index1];
00082 //
00083 //      if(pmin>rad || -rad>pmax) return false;
00084 //      return true;
00085 //}
00086 //
00087 //SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
00088 //      const btVector3 & edge,
00089 //      const btVector3 & absolute_edge,
00090 //      const btVector3 & pointa,
00091 //      const btVector3 & pointb, btVector3 & extend)
00092 //{
00093 //
00094 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
00095 //}
00096 //
00097 //
00098 //SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
00099 //      const btVector3 & edge,
00100 //      const btVector3 & absolute_edge,
00101 //      const btVector3 & pointa,
00102 //      const btVector3 & pointb, btVector3 & extend)
00103 //{
00104 //
00105 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
00106 //}
00107 //
00108 //SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
00109 //      const btVector3 & edge,
00110 //      const btVector3 & absolute_edge,
00111 //      const btVector3 & pointa,
00112 //      const btVector3 & pointb, btVector3 & extend)
00113 //{
00114 //
00115 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
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 //              static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
00171 //              m_AR[0] = vepsi + m_R1to0[0].absolute();
00172 //              m_AR[1] = vepsi + m_R1to0[1].absolute();
00173 //              m_AR[2] = vepsi + m_R1to0[2].absolute();
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                 // Compute new center
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                 // Compute new center
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; // 0
00496                 }
00497 
00498                 if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
00499                 {
00500                         return BT_CONST_COLLIDE_PLANE; //1
00501                 }
00502                 return BT_CONST_FRONT_PLANE;//2
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                 //Taken from OPCODE
00526                 btVector3 ea,eb;//extends
00527                 btVector3 ca,cb;//extends
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                 // Class I : A's basis vectors
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                 // Class II : B's basis vectors
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                 // Class III : 9 cross products
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                 //First axis
00599                 btVector3 diff(v2 - v1);
00600                 btVector3 abs_diff = diff.absolute();
00601                 //Test With X axis
00602                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00603                 //Test With Y axis
00604                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
00605                 //Test With Z axis
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                 //Test With X axis
00612                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00613                 //Test With Y axis
00614                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
00615                 //Test With Z axis
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                 //Test With X axis
00621                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00622                 //Test With Y axis
00623                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
00624                 //Test With Z axis
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