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