OPCODE/OPC_BoxBoxOverlap.h

   1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   2 /**
   3  *      OBB-OBB overlap test using the separating axis theorem.
   4  *      - original code by Gomez / Gamasutra (similar to Gottschalk's one in RAPID)
   5  *      - optimized for AABB trees by computing the rotation matrix once (SOLID-fashion)
   6  *      - the fabs matrix is precomputed as well and epsilon-tweaked (RAPID-style, we found this almost mandatory)
   7  *      - Class III axes can be disabled... (SOLID & Intel fashion)
   8  *      - ...or enabled to perform some profiling
   9  *      - CPU comparisons used when appropriate
  10  *      - lazy evaluation sometimes saves some work in case of early exits (unlike SOLID)
  11  *
  12  *      \param          ea      [in] extents from box A
  13  *      \param          ca      [in] center from box A
  14  *      \param          eb      [in] extents from box B
  15  *      \param          cb      [in] center from box B
  16  *      \return         true if boxes overlap
  17  */
  18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  19 inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb)
  20 {
  21         // Stats
  22         mNbBVBVTests++;
  23
  24         float t,t2;
  25
  26         // Class I : A's basis vectors
  27         float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x;
  28         t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0];
  29         if(GREATER(Tx, t))      return FALSE;
  30
  31         float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y;
  32         t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1];
  33         if(GREATER(Ty, t))      return FALSE;
  34
  35         float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z;
  36         t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2];
  37         if(GREATER(Tz, t))      return FALSE;
  38
  39         // Class II : B's basis vectors
  40         t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2];  t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x;
  41         if(GREATER(t, t2))      return FALSE;
  42
  43         t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2];  t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y;
  44         if(GREATER(t, t2))      return FALSE;
  45
  46         t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2];  t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z;
  47         if(GREATER(t, t2))      return FALSE;
  48
  49         // Class III : 9 cross products
  50         // Cool trick: always perform the full test for first level, regardless of settings.
  51         // That way pathological cases (such as the pencils scene) are quickly rejected anyway !
  52         if(mFullBoxBoxTest || mNbBVBVTests==1)
  53         {
  54                 t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2];      t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; if(GREATER(t, t2))      return FALSE;   // L = A0 x B0
  55                 t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2];      t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; if(GREATER(t, t2))      return FALSE;   // L = A0 x B1
  56                 t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2];      t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; if(GREATER(t, t2))      return FALSE;   // L = A0 x B2
  57                 t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0];      t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; if(GREATER(t, t2))      return FALSE;   // L = A1 x B0
  58                 t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0];      t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; if(GREATER(t, t2))      return FALSE;   // L = A1 x B1
  59                 t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0];      t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; if(GREATER(t, t2))      return FALSE;   // L = A1 x B2
  60                 t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1];      t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; if(GREATER(t, t2))      return FALSE;   // L = A2 x B0
  61                 t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1];      t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; if(GREATER(t, t2))      return FALSE;   // L = A2 x B1
  62                 t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1];      t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; if(GREATER(t, t2))      return FALSE;   // L = A2 x B2
  63         }
  64         return TRUE;
  65 }
  66
  67 //! A dedicated version when one box is constant
  68 inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center)
  69 {
  70         // Stats
  71         mNbVolumeBVTests++;
  72
  73         float t,t2;
  74
  75         // Class I : A's basis vectors
  76         float Tx = mTBoxToModel.x - center.x;   t = extents.x + mBBx1;  if(GREATER(Tx, t))      return FALSE;
  77         float Ty = mTBoxToModel.y - center.y;   t = extents.y + mBBy1;  if(GREATER(Ty, t))      return FALSE;
  78         float Tz = mTBoxToModel.z - center.z;   t = extents.z + mBBz1;  if(GREATER(Tz, t))      return FALSE;
  79
  80         // Class II : B's basis vectors
  81         t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2];
  82         t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x;
  83         if(GREATER(t, t2))      return FALSE;
  84
  85         t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2];
  86         t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y;
  87         if(GREATER(t, t2))      return FALSE;
  88
  89         t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2];
  90         t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z;
  91         if(GREATER(t, t2))      return FALSE;
  92
  93         // Class III : 9 cross products
  94         // Cool trick: always perform the full test for first level, regardless of settings.
  95         // That way pathological cases (such as the pencils scene) are quickly rejected anyway !
  96         if(mFullBoxBoxTest || mNbVolumeBVTests==1)
  97         {
  98                 t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2];  t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1;     if(GREATER(t, t2))      return FALSE;   // L = A0 x B0
  99                 t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2];  t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2;     if(GREATER(t, t2))      return FALSE;   // L = A0 x B1
 100                 t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2];  t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3;     if(GREATER(t, t2))      return FALSE;   // L = A0 x B2
 101                 t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0];  t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4;     if(GREATER(t, t2))      return FALSE;   // L = A1 x B0
 102                 t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0];  t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5;     if(GREATER(t, t2))      return FALSE;   // L = A1 x B1
 103                 t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0];  t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6;     if(GREATER(t, t2))      return FALSE;   // L = A1 x B2
 104                 t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1];  t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7;     if(GREATER(t, t2))      return FALSE;   // L = A2 x B0
 105                 t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1];  t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8;     if(GREATER(t, t2))      return FALSE;   // L = A2 x B1
 106                 t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1];  t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9;     if(GREATER(t, t2))      return FALSE;   // L = A2 x B2
 107         }
 108         return TRUE;
 109 }
 110
 111 //! A special version for 2 axis-aligned boxes
 112 inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center)
 113 {
 114         // Stats
 115         mNbVolumeBVTests++;
 116
 117         float tx = mBox.mCenter.x - center.x;   float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex))     return FALSE;
 118         float ty = mBox.mCenter.y - center.y;   float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey))     return FALSE;
 119         float tz = mBox.mCenter.z - center.z;   float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez))     return FALSE;
 120
 121         return TRUE;
 122 }