src/BulletCollision/Gimpact/btBoxCollision.h

   1 #ifndef BT_BOX_COLLISION_H_INCLUDED
   2 #define BT_BOX_COLLISION_H_INCLUDED
   3
   4 /*! \file gim_box_collision.h
   5 \author Francisco Leon Najera
   6 */
   7 /*
   8 This source file is part of GIMPACT Library.
   9
  10 For the latest info, see http://gimpact.sourceforge.net/
  11
  12 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
  13 email: projectileman@yahoo.com
  14
  15
  16 This software is provided 'as-is', without any express or implied warranty.
  17 In no event will the authors be held liable for any damages arising from the use of this software.
  18 Permission is granted to anyone to use this software for any purpose,
  19 including commercial applications, and to alter it and redistribute it freely,
  20 subject to the following restrictions:
  21
  22 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.
  23 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  24 3. This notice may not be removed or altered from any source distribution.
  25 */
  26
  27 #include "LinearMath/btTransform.h"
  28
  29
  30 ///Swap numbers
  31 #define BT_SWAP_NUMBERS(a,b){ \
  32     a = a+b; \
  33     b = a-b; \
  34     a = a-b; \
  35 }\
  36
  37
  38 #define BT_MAX(a,b) (a<b?b:a)
  39 #define BT_MIN(a,b) (a>b?b:a)
  40
  41 #define BT_GREATER(x, y)        btFabs(x) > (y)
  42
  43 #define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
  44 #define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
  45
  46
  47
  48
  49
  50
  51 enum eBT_PLANE_INTERSECTION_TYPE
  52 {
  53         BT_CONST_BACK_PLANE = 0,
  54         BT_CONST_COLLIDE_PLANE,
  55         BT_CONST_FRONT_PLANE
  56 };
  57
  58 //SIMD_FORCE_INLINE bool test_cross_edge_box(
  59 //      const btVector3 & edge,
  60 //      const btVector3 & absolute_edge,
  61 //      const btVector3 & pointa,
  62 //      const btVector3 & pointb, const btVector3 & extend,
  63 //      int dir_index0,
  64 //      int dir_index1
  65 //      int component_index0,
  66 //      int component_index1)
  67 //{
  68 //      // dir coords are -z and y
  69 //
  70 //      const btScalar dir0 = -edge[dir_index0];
  71 //      const btScalar dir1 = edge[dir_index1];
  72 //      btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
  73 //      btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
  74 //      //find minmax
  75 //      if(pmin>pmax)
  76 //      {
  77 //              BT_SWAP_NUMBERS(pmin,pmax);
  78 //      }
  79 //      //find extends
  80 //      const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
  81 //                                      extend[component_index1] * absolute_edge[dir_index1];
  82 //
  83 //      if(pmin>rad || -rad>pmax) return false;
  84 //      return true;
  85 //}
  86 //
  87 //SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
  88 //      const btVector3 & edge,
  89 //      const btVector3 & absolute_edge,
  90 //      const btVector3 & pointa,
  91 //      const btVector3 & pointb, btVector3 & extend)
  92 //{
  93 //
  94 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
  95 //}
  96 //
  97 //
  98 //SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
  99 //      const btVector3 & edge,
 100 //      const btVector3 & absolute_edge,
 101 //      const btVector3 & pointa,
 102 //      const btVector3 & pointb, btVector3 & extend)
 103 //{
 104 //
 105 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
 106 //}
 107 //
 108 //SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
 109 //      const btVector3 & edge,
 110 //      const btVector3 & absolute_edge,
 111 //      const btVector3 & pointa,
 112 //      const btVector3 & pointb, btVector3 & extend)
 113 //{
 114 //
 115 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
 116 //}
 117
 118
 119 #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
 120 {\
 121         const btScalar dir0 = -edge[i_dir_0];\
 122         const btScalar dir1 = edge[i_dir_1];\
 123         btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
 124         btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
 125         if(pmin>pmax)\
 126         {\
 127                 BT_SWAP_NUMBERS(pmin,pmax); \
 128         }\
 129         const btScalar abs_dir0 = absolute_edge[i_dir_0];\
 130         const btScalar abs_dir1 = absolute_edge[i_dir_1];\
 131         const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
 132         if(pmin>rad || -rad>pmax) return false;\
 133 }\
 134
 135
 136 #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 137 {\
 138         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
 139 }\
 140
 141 #define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 142 {\
 143         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
 144 }\
 145
 146 #define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 147 {\
 148         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
 149 }\
 150
 151
 152 //! Returns the dot product between a vec3f and the col of a matrix
 153 SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
 154 const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
 155 {
 156         return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
 157 }
 158
 159
 160 //!  Class for transforming a model1 to the space of model0
 161 ATTRIBUTE_ALIGNED16     (class) BT_BOX_BOX_TRANSFORM_CACHE
 162 {
 163 public:
 164     btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
 165         btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
 166         btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
 167
 168         SIMD_FORCE_INLINE void calc_absolute_matrix()
 169         {
 170 //              static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
 171 //              m_AR[0] = vepsi + m_R1to0[0].absolute();
 172 //              m_AR[1] = vepsi + m_R1to0[1].absolute();
 173 //              m_AR[2] = vepsi + m_R1to0[2].absolute();
 174
 175                 int i,j;
 176
 177         for(i=0;i<3;i++)
 178         {
 179             for(j=0;j<3;j++ )
 180             {
 181                 m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
 182             }
 183         }
 184
 185         }
 186
 187         BT_BOX_BOX_TRANSFORM_CACHE()
 188         {
 189         }
 190
 191
 192
 193         //! Calc the transformation relative  1 to 0. Inverts matrics by transposing
 194         SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
 195         {
 196
 197                 btTransform temp_trans = trans0.inverse();
 198                 temp_trans = temp_trans * trans1;
 199
 200                 m_T1to0 = temp_trans.getOrigin();
 201                 m_R1to0 = temp_trans.getBasis();
 202
 203
 204                 calc_absolute_matrix();
 205         }
 206
 207         //! Calcs the full invertion of the matrices. Useful for scaling matrices
 208         SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
 209         {
 210                 m_R1to0 = trans0.getBasis().inverse();
 211                 m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 212
 213                 m_T1to0 += m_R1to0*trans1.getOrigin();
 214                 m_R1to0 *= trans1.getBasis();
 215
 216                 calc_absolute_matrix();
 217         }
 218
 219         SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
 220         {
 221         return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0;
 222         }
 223 };
 224
 225
 226 #define BOX_PLANE_EPSILON 0.000001f
 227
 228 //! Axis aligned box
 229 ATTRIBUTE_ALIGNED16     (class) btAABB
 230 {
 231 public:
 232         btVector3 m_min;
 233         btVector3 m_max;
 234
 235         btAABB()
 236         {}
 237
 238
 239         btAABB(const btVector3 & V1,
 240                          const btVector3 & V2,
 241                          const btVector3 & V3)
 242         {
 243                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
 244                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
 245                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
 246
 247                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
 248                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
 249                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
 250         }
 251
 252         btAABB(const btVector3 & V1,
 253                          const btVector3 & V2,
 254                          const btVector3 & V3,
 255                          btScalar margin)
 256         {
 257                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
 258                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
 259                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
 260
 261                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
 262                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
 263                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
 264
 265                 m_min[0] -= margin;
 266                 m_min[1] -= margin;
 267                 m_min[2] -= margin;
 268                 m_max[0] += margin;
 269                 m_max[1] += margin;
 270                 m_max[2] += margin;
 271         }
 272
 273         btAABB(const btAABB &other):
 274                 m_min(other.m_min),m_max(other.m_max)
 275         {
 276         }
 277
 278         btAABB(const btAABB &other,btScalar margin ):
 279                 m_min(other.m_min),m_max(other.m_max)
 280         {
 281                 m_min[0] -= margin;
 282                 m_min[1] -= margin;
 283                 m_min[2] -= margin;
 284                 m_max[0] += margin;
 285                 m_max[1] += margin;
 286                 m_max[2] += margin;
 287         }
 288
 289         SIMD_FORCE_INLINE void invalidate()
 290         {
 291                 m_min[0] = SIMD_INFINITY;
 292                 m_min[1] = SIMD_INFINITY;
 293                 m_min[2] = SIMD_INFINITY;
 294                 m_max[0] = -SIMD_INFINITY;
 295                 m_max[1] = -SIMD_INFINITY;
 296                 m_max[2] = -SIMD_INFINITY;
 297         }
 298
 299         SIMD_FORCE_INLINE void increment_margin(btScalar margin)
 300         {
 301                 m_min[0] -= margin;
 302                 m_min[1] -= margin;
 303                 m_min[2] -= margin;
 304                 m_max[0] += margin;
 305                 m_max[1] += margin;
 306                 m_max[2] += margin;
 307         }
 308
 309         SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
 310         {
 311                 m_min[0] = other.m_min[0] - margin;
 312                 m_min[1] = other.m_min[1] - margin;
 313                 m_min[2] = other.m_min[2] - margin;
 314
 315                 m_max[0] = other.m_max[0] + margin;
 316                 m_max[1] = other.m_max[1] + margin;
 317                 m_max[2] = other.m_max[2] + margin;
 318         }
 319
 320         template<typename CLASS_POINT>
 321         SIMD_FORCE_INLINE void calc_from_triangle(
 322                                                         const CLASS_POINT & V1,
 323                                                         const CLASS_POINT & V2,
 324                                                         const CLASS_POINT & V3)
 325         {
 326                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
 327                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
 328                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
 329
 330                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
 331                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
 332                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
 333         }
 334
 335         template<typename CLASS_POINT>
 336         SIMD_FORCE_INLINE void calc_from_triangle_margin(
 337                                                         const CLASS_POINT & V1,
 338                                                         const CLASS_POINT & V2,
 339                                                         const CLASS_POINT & V3, btScalar margin)
 340         {
 341                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
 342                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
 343                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
 344
 345                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
 346                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
 347                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
 348
 349                 m_min[0] -= margin;
 350                 m_min[1] -= margin;
 351                 m_min[2] -= margin;
 352                 m_max[0] += margin;
 353                 m_max[1] += margin;
 354                 m_max[2] += margin;
 355         }
 356
 357         //! Apply a transform to an AABB
 358         SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
 359         {
 360                 btVector3 center = (m_max+m_min)*0.5f;
 361                 btVector3 extends = m_max - center;
 362                 // Compute new center
 363                 center = trans(center);
 364
 365         btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
 366                                           trans.getBasis().getRow(1).absolute(),
 367                                           trans.getBasis().getRow(2).absolute());
 368
 369                 m_min = center - textends;
 370                 m_max = center + textends;
 371         }
 372
 373
 374         //! Apply a transform to an AABB
 375         SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
 376         {
 377                 btVector3 center = (m_max+m_min)*0.5f;
 378                 btVector3 extends = m_max - center;
 379                 // Compute new center
 380                 center = trans.transform(center);
 381
 382         btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(),
 383                                           trans.m_R1to0.getRow(1).absolute(),
 384                                           trans.m_R1to0.getRow(2).absolute());
 385
 386                 m_min = center - textends;
 387                 m_max = center + textends;
 388         }
 389
 390         //! Merges a Box
 391         SIMD_FORCE_INLINE void merge(const btAABB & box)
 392         {
 393                 m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
 394                 m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
 395                 m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
 396
 397                 m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
 398                 m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
 399                 m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
 400         }
 401
 402         //! Merges a point
 403         template<typename CLASS_POINT>
 404         SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
 405         {
 406                 m_min[0] = BT_MIN(m_min[0],point[0]);
 407                 m_min[1] = BT_MIN(m_min[1],point[1]);
 408                 m_min[2] = BT_MIN(m_min[2],point[2]);
 409
 410                 m_max[0] = BT_MAX(m_max[0],point[0]);
 411                 m_max[1] = BT_MAX(m_max[1],point[1]);
 412                 m_max[2] = BT_MAX(m_max[2],point[2]);
 413         }
 414
 415         //! Gets the extend and center
 416         SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
 417         {
 418                 center = (m_max+m_min)*0.5f;
 419                 extend = m_max - center;
 420         }
 421
 422         //! Finds the intersecting box between this box and the other.
 423         SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
 424         {
 425                 intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
 426                 intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
 427                 intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
 428
 429                 intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
 430                 intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
 431                 intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
 432         }
 433
 434
 435         SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
 436         {
 437                 if(m_min[0] > other.m_max[0] ||
 438                    m_max[0] < other.m_min[0] ||
 439                    m_min[1] > other.m_max[1] ||
 440                    m_max[1] < other.m_min[1] ||
 441                    m_min[2] > other.m_max[2] ||
 442                    m_max[2] < other.m_min[2])
 443                 {
 444                         return false;
 445                 }
 446                 return true;
 447         }
 448
 449         /*! \brief Finds the Ray intersection parameter.
 450         \param aabb Aligned box
 451         \param vorigin A vec3f with the origin of the ray
 452         \param vdir A vec3f with the direction of the ray
 453         */
 454         SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
 455         {
 456                 btVector3 extents,center;
 457                 this->get_center_extend(center,extents);;
 458
 459                 btScalar Dx = vorigin[0] - center[0];
 460                 if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)      return false;
 461                 btScalar Dy = vorigin[1] - center[1];
 462                 if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)      return false;
 463                 btScalar Dz = vorigin[2] - center[2];
 464                 if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)      return false;
 465
 466
 467                 btScalar f = vdir[1] * Dz - vdir[2] * Dy;
 468                 if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
 469                 f = vdir[2] * Dx - vdir[0] * Dz;
 470                 if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
 471                 f = vdir[0] * Dy - vdir[1] * Dx;
 472                 if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
 473                 return true;
 474         }
 475
 476
 477         SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
 478         {
 479                 btVector3 center = (m_max+m_min)*0.5f;
 480                 btVector3 extend = m_max-center;
 481
 482                 btScalar _fOrigin =  direction.dot(center);
 483                 btScalar _fMaximumExtent = extend.dot(direction.absolute());
 484                 vmin = _fOrigin - _fMaximumExtent;
 485                 vmax = _fOrigin + _fMaximumExtent;
 486         }
 487
 488         SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
 489         {
 490                 btScalar _fmin,_fmax;
 491                 this->projection_interval(plane,_fmin,_fmax);
 492
 493                 if(plane[3] > _fmax + BOX_PLANE_EPSILON)
 494                 {
 495                         return BT_CONST_BACK_PLANE; // 0
 496                 }
 497
 498                 if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
 499                 {
 500                         return BT_CONST_COLLIDE_PLANE; //1
 501                 }
 502                 return BT_CONST_FRONT_PLANE;//2
 503         }
 504
 505         SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
 506         {
 507                 btAABB tbox = box;
 508                 tbox.appy_transform(trans1_to_0);
 509                 return has_collision(tbox);
 510         }
 511
 512         SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
 513                 const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
 514         {
 515                 btAABB tbox = box;
 516                 tbox.appy_transform_trans_cache(trans1_to_0);
 517                 return has_collision(tbox);
 518         }
 519
 520         //! transcache is the transformation cache from box to this AABB
 521         SIMD_FORCE_INLINE bool overlapping_trans_cache(
 522                 const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
 523         {
 524
 525                 //Taken from OPCODE
 526                 btVector3 ea,eb;//extends
 527                 btVector3 ca,cb;//extends
 528                 get_center_extend(ca,ea);
 529                 box.get_center_extend(cb,eb);
 530
 531
 532                 btVector3 T;
 533                 btScalar t,t2;
 534                 int i;
 535
 536                 // Class I : A's basis vectors
 537                 for(i=0;i<3;i++)
 538                 {
 539                         T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
 540                         t = transcache.m_AR[i].dot(eb) + ea[i];
 541                         if(BT_GREATER(T[i], t)) return false;
 542                 }
 543                 // Class II : B's basis vectors
 544                 for(i=0;i<3;i++)
 545                 {
 546                         t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
 547                         t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
 548                         if(BT_GREATER(t,t2))    return false;
 549                 }
 550                 // Class III : 9 cross products
 551                 if(fulltest)
 552                 {
 553                         int j,m,n,o,p,q,r;
 554                         for(i=0;i<3;i++)
 555                         {
 556                                 m = (i+1)%3;
 557                                 n = (i+2)%3;
 558                                 o = i==0?1:0;
 559                                 p = i==2?1:2;
 560                                 for(j=0;j<3;j++)
 561                                 {
 562                                         q = j==2?1:2;
 563                                         r = j==0?1:0;
 564                                         t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
 565                                         t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
 566                                                 eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
 567                                         if(BT_GREATER(t,t2))    return false;
 568                                 }
 569                         }
 570                 }
 571                 return true;
 572         }
 573
 574         //! Simple test for planes.
 575         SIMD_FORCE_INLINE bool collide_plane(
 576                 const btVector4 & plane) const
 577         {
 578                 eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
 579                 return (classify == BT_CONST_COLLIDE_PLANE);
 580         }
 581
 582         //! test for a triangle, with edges
 583         SIMD_FORCE_INLINE bool collide_triangle_exact(
 584                 const btVector3 & p1,
 585                 const btVector3 & p2,
 586                 const btVector3 & p3,
 587                 const btVector4 & triangle_plane) const
 588         {
 589                 if(!collide_plane(triangle_plane)) return false;
 590
 591                 btVector3 center,extends;
 592                 this->get_center_extend(center,extends);
 593
 594                 const btVector3 v1(p1 - center);
 595                 const btVector3 v2(p2 - center);
 596                 const btVector3 v3(p3 - center);
 597
 598                 //First axis
 599                 btVector3 diff(v2 - v1);
 600                 btVector3 abs_diff = diff.absolute();
 601                 //Test With X axis
 602                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 603                 //Test With Y axis
 604                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 605                 //Test With Z axis
 606                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 607
 608
 609                 diff = v3 - v2;
 610                 abs_diff = diff.absolute();
 611                 //Test With X axis
 612                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 613                 //Test With Y axis
 614                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 615                 //Test With Z axis
 616                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 617
 618                 diff = v1 - v3;
 619                 abs_diff = diff.absolute();
 620                 //Test With X axis
 621                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 622                 //Test With Y axis
 623                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 624                 //Test With Z axis
 625                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 626
 627                 return true;
 628         }
 629 };
 630
 631
 632 //! Compairison of transformation objects
 633 SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
 634 {
 635         if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
 636
 637         if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
 638         if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
 639         if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
 640         return true;
 641 }
 642
 643
 644
 645 #endif // GIM_BOX_COLLISION_H_INCLUDED