OPC_BoxBoxOverlap.h
Go to the documentation of this file.
1 
18 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 // Modified!
67 // Only AABBTreeCollider
68 #if 0
69 inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center)
71 {
72  // Stats
73  mNbVolumeBVTests++;
74 
75  float t,t2;
76 
77  // Class I : A's basis vectors
78  float Tx = mTBoxToModel.x - center.x; t = extents.x + mBBx1; if(GREATER(Tx, t)) return FALSE;
79  float Ty = mTBoxToModel.y - center.y; t = extents.y + mBBy1; if(GREATER(Ty, t)) return FALSE;
80  float Tz = mTBoxToModel.z - center.z; t = extents.z + mBBz1; if(GREATER(Tz, t)) return FALSE;
81 
82  // Class II : B's basis vectors
83  t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2];
84  t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x;
85  if(GREATER(t, t2)) return FALSE;
86 
87  t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2];
88  t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y;
89  if(GREATER(t, t2)) return FALSE;
90 
91  t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2];
92  t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z;
93  if(GREATER(t, t2)) return FALSE;
94 
95  // Class III : 9 cross products
96  // Cool trick: always perform the full test for first level, regardless of settings.
97  // That way pathological cases (such as the pencils scene) are quickly rejected anyway !
98  if(mFullBoxBoxTest || mNbVolumeBVTests==1)
99  {
100  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
101  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
102  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
103  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
104  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
105  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
106  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
107  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
108  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
109  }
110  return TRUE;
111 }
112 
114 inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center)
115 {
116  // Stats
117  mNbVolumeBVTests++;
118 
119  float tx = mBox.mCenter.x - center.x; float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex)) return FALSE;
120  float ty = mBox.mCenter.y - center.y; float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey)) return FALSE;
121  float tz = mBox.mCenter.z - center.z; float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez)) return FALSE;
122 
123  return TRUE;
124 }
125 #endif
FALSE
#define FALSE
Definition: OPC_IceHook.h:9
Matrix3x3::m
float m[3][3]
Definition: IceMatrix3x3.h:492
TRUE
#define TRUE
Definition: OPC_IceHook.h:13
AABBTreeCollider::mFullBoxBoxTest
bool mFullBoxBoxTest
Perform full BV-BV tests (true) or SAT-lite tests (false)
Definition: OPC_TreeCollider.h:218
AABBTreeCollider::mNbBVBVTests
udword mNbBVBVTests
Number of BV-BV tests.
Definition: OPC_TreeCollider.h:194
inline_
#define inline_
Definition: IcePreprocessor.h:103
Point::z
float z
Definition: IcePoint.h:524
Point
Definition: IcePoint.h:25
AABBTreeCollider::mAR
Matrix3x3 mAR
Absolute rotation matrix.
Definition: OPC_TreeCollider.h:198
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
Point::x
float x
Definition: IcePoint.h:524
t
t
Point::y
float y
Definition: IcePoint.h:524
AABBTreeCollider::mT1to0
Point mT1to0
Translation from object1 to object0.
Definition: OPC_TreeCollider.h:202
GREATER
#define GREATER(x, y)
Definition: Opcode.h:28
AABBTreeCollider::mR1to0
Matrix3x3 mR1to0
Rotation from object1 to object0.
Definition: OPC_TreeCollider.h:200
AABBTreeCollider::BoxBoxOverlap
inline_ BOOL BoxBoxOverlap(const Point &ea, const Point &ca, const Point &eb, const Point &cb)
Definition: OPC_BoxBoxOverlap.h:19

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04