← Back to branch summary

~jtaylor/ubuntu/oneiric/soya/fix-780305

« back to all changes in this revision

Viewing changes to ode-0.5/OPCODE/OPC_BoxBoxOverlap.h

  • Committer: Bazaar Package Importer
  • Author(s): Marc Dequènes (Duck)
  • Date: 2005-01-30 09:55:06 UTC
  • mfrom: (1.2.1 upstream) (2.1.1 hoary)
  • Revision ID: james.westby@ubuntu.com-20050130095506-f21p6v6cgaobhn5j
Tags: 0.9.2-1
New upstream release.

Show diffs side-by-side

added added

removed removed

Lines of Context:
ode-0.5/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
}