OPC_BoxBoxOverlap.h
Go to the documentation of this file.
00001 
00002 
00018 
00019 inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb)
00020 {
00021         // Stats
00022         mNbBVBVTests++;
00023 
00024         float t,t2;
00025 
00026         // Class I : A's basis vectors
00027         float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x;
00028         t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0];
00029         if(GREATER(Tx, t))      return FALSE;
00030 
00031         float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y;
00032         t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1];
00033         if(GREATER(Ty, t))      return FALSE;
00034 
00035         float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z;
00036         t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2];
00037         if(GREATER(Tz, t))      return FALSE;
00038 
00039         // Class II : B's basis vectors
00040         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;
00041         if(GREATER(t, t2))      return FALSE;
00042 
00043         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;
00044         if(GREATER(t, t2))      return FALSE;
00045 
00046         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;
00047         if(GREATER(t, t2))      return FALSE;
00048 
00049         // Class III : 9 cross products
00050         // Cool trick: always perform the full test for first level, regardless of settings.
00051         // That way pathological cases (such as the pencils scene) are quickly rejected anyway !
00052         if(mFullBoxBoxTest || mNbBVBVTests==1)
00053         {
00054                 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
00055                 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
00056                 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
00057                 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
00058                 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
00059                 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
00060                 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
00061                 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
00062                 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
00063         }
00064         return TRUE;
00065 }
00066 // Modified!
00067 // Only AABBTreeCollider
00068 #if 0
00069 
00070 inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center)
00071 {
00072         // Stats
00073         mNbVolumeBVTests++;
00074 
00075         float t,t2;
00076 
00077         // Class I : A's basis vectors
00078         float Tx = mTBoxToModel.x - center.x;   t = extents.x + mBBx1;  if(GREATER(Tx, t))      return FALSE;
00079         float Ty = mTBoxToModel.y - center.y;   t = extents.y + mBBy1;  if(GREATER(Ty, t))      return FALSE;
00080         float Tz = mTBoxToModel.z - center.z;   t = extents.z + mBBz1;  if(GREATER(Tz, t))      return FALSE;
00081 
00082         // Class II : B's basis vectors
00083         t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2];
00084         t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x;
00085         if(GREATER(t, t2))      return FALSE;
00086 
00087         t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2];
00088         t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y;
00089         if(GREATER(t, t2))      return FALSE;
00090 
00091         t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2];
00092         t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z;
00093         if(GREATER(t, t2))      return FALSE;
00094 
00095         // Class III : 9 cross products
00096         // Cool trick: always perform the full test for first level, regardless of settings.
00097         // That way pathological cases (such as the pencils scene) are quickly rejected anyway !
00098         if(mFullBoxBoxTest || mNbVolumeBVTests==1)
00099         {
00100                 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
00101                 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
00102                 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
00103                 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
00104                 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
00105                 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
00106                 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
00107                 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
00108                 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
00109         }
00110         return TRUE;
00111 }
00112 
00114 inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center)
00115 {
00116         // Stats
00117         mNbVolumeBVTests++;
00118 
00119         float tx = mBox.mCenter.x - center.x;   float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex))     return FALSE;
00120         float ty = mBox.mCenter.y - center.y;   float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey))     return FALSE;
00121         float tz = mBox.mCenter.z - center.z;   float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez))     return FALSE;
00122 
00123         return TRUE;
00124 }
00125 #endif 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55