Bullet Collision Detection & Physics Library: btAabbUtil2.h Source File

Bullet Collision Detection & Physics Library

btAabbUtil2.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9
10 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16
17 #ifndef BT_AABB_UTIL2
18 #define BT_AABB_UTIL2
19
20 #include "btTransform.h"
21 #include "btVector3.h"
22 #include "btMinMax.h"
23
24
25
26 SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
27 btVector3& aabbMax,
28 const btVector3& expansionMin,
29 const btVector3& expansionMax)
30 {
31 aabbMin = aabbMin + expansionMin;
32 aabbMax = aabbMax + expansionMax;
33 }
34
36 SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
37 const btVector3 &point)
38 {
39 bool overlap = true;
40 overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
41 overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
42 overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
43 return overlap;
44 }
45
46
48 SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
49 const btVector3 &aabbMin2, const btVector3 &aabbMax2)
50 {
51 bool overlap = true;
52 overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
53 overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
54 overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
55 return overlap;
56 }
57
59 SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
60 const btVector3 &aabbMin, const btVector3 &aabbMax)
61 {
62 const btVector3 &p1 = vertices[0];
63 const btVector3 &p2 = vertices[1];
64 const btVector3 &p3 = vertices[2];
65
66 if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
67 if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
68
69 if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
70 if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
71
72 if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
73 if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
74 return true;
75 }
76
77
78 SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
79 {
80 return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
81 (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
82 (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
83 (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
84 (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
85 (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
86 }
87
88
89
90 SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
91 const btVector3& rayInvDirection,
92 const unsigned int raySign[3],
93 const btVector3 bounds[2],
94 btScalar& tmin,
95 btScalar lambda_min,
96 btScalar lambda_max)
97 {
98 btScalar tmax, tymin, tymax, tzmin, tzmax;
99 tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
100 tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
101 tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
102 tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
103
104 if ( (tmin > tymax) || (tymin > tmax) )
105 return false;
106
107 if (tymin > tmin)
108 tmin = tymin;
109
110 if (tymax < tmax)
111 tmax = tymax;
112
113 tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
114 tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
115
116 if ( (tmin > tzmax) || (tzmin > tmax) )
117 return false;
118 if (tzmin > tmin)
119 tmin = tzmin;
120 if (tzmax < tmax)
121 tmax = tzmax;
122 return ( (tmin < lambda_max) && (tmax > lambda_min) );
123 }
124
125 SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
126 const btVector3& rayTo,
127 const btVector3& aabbMin,
128 const btVector3& aabbMax,
129 btScalar& param, btVector3& normal)
130 {
131 btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
132 btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
133 btVector3 source = rayFrom - aabbCenter;
134 btVector3 target = rayTo - aabbCenter;
135 int sourceOutcode = btOutcode(source,aabbHalfExtent);
136 int targetOutcode = btOutcode(target,aabbHalfExtent);
137 if ((sourceOutcode & targetOutcode) == 0x0)
138 {
139 btScalar lambda_enter = btScalar(0.0);
140 btScalar lambda_exit = param;
141 btVector3 r = target - source;
142 int i;
143 btScalar normSign = 1;
144 btVector3 hitNormal(0,0,0);
145 int bit=1;
146
147 for (int j=0;j<2;j++)
148 {
149 for (i = 0; i != 3; ++i)
150 {
151 if (sourceOutcode & bit)
152 {
153 btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
154 if (lambda_enter <= lambda)
155 {
156 lambda_enter = lambda;
157 hitNormal.setValue(0,0,0);
158 hitNormal[i] = normSign;
159 }
160 }
161 else if (targetOutcode & bit)
162 {
163 btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
164 btSetMin(lambda_exit, lambda);
165 }
166 bit<<=1;
167 }
168 normSign = btScalar(-1.);
169 }
170 if (lambda_enter <= lambda_exit)
171 {
172 param = lambda_enter;
173 normal = hitNormal;
174 return true;
175 }
176 }
177 return false;
178 }
179
180
181
182 SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
183 {
184 btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
185 btMatrix3x3 abs_b = t.getBasis().absolute();
186 btVector3 center = t.getOrigin();
187 btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
188 aabbMinOut = center - extent;
189 aabbMaxOut = center + extent;
190 }
191
192
193 SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
194 {
195 btAssert(localAabbMin.getX() <= localAabbMax.getX());
196 btAssert(localAabbMin.getY() <= localAabbMax.getY());
197 btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
198 btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
199 localHalfExtents+=btVector3(margin,margin,margin);
200
201 btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
202 btMatrix3x3 abs_b = trans.getBasis().absolute();
203 btVector3 center = trans(localCenter);
204 btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
205 aabbMinOut = center-extent;
206 aabbMaxOut = center+extent;
207 }
208
209 #define USE_BANCHLESS 1
210 #ifdef USE_BANCHLESS
211 //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
212 SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
213 {
214 return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
215 & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
216 & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
217 1, 0));
218 }
219 #else
220 SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
221 {
222 bool overlap = true;
223 overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
224 overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
225 overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
226 return overlap;
227 }
228 #endif //USE_BANCHLESS
229
230 #endif //BT_AABB_UTIL2
231
232
btSelect
unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBo...
Definition: btScalar.h:595
btOutcode
int btOutcode(const btVector3 &p, const btVector3 &halfExtent)
Definition: btAabbUtil2.h:78
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btVector3.h
btVector3::dot3
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:731
btAssert
#define btAssert(x)
Definition: btScalar.h:131
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
btTransform.h
TestAabbAgainstAabb2
bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition: btAabbUtil2.h:48
btSetMin
void btSetMin(T &a, const T &b)
Definition: btMinMax.h:41
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
AabbExpand
void AabbExpand(btVector3 &aabbMin, btVector3 &aabbMax, const btVector3 &expansionMin, const btVector3 &expansionMax)
Definition: btAabbUtil2.h:26
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
testQuantizedAabbAgainstQuantizedAabb
unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int *aabbMin1, const unsigned short int *aabbMax1, const unsigned short int *aabbMin2, const unsigned short int *aabbMax2)
Definition: btAabbUtil2.h:212
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btMinMax.h
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
TestPointAgainstAabb2
bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &point)
conservative test for overlap between two aabbs
Definition: btAabbUtil2.h:36
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3::absolute
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:937
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
bounds
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:284
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btTransformAabb
void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
Definition: btAabbUtil2.h:182
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
btRayAabb2
bool btRayAabb2(const btVector3 &rayFrom, const btVector3 &rayInvDirection, const unsigned int raySign[3], const btVector3 bounds[2], btScalar &tmin, btScalar lambda_min, btScalar lambda_max)
Definition: btAabbUtil2.h:90
btRayAabb
bool btRayAabb(const btVector3 &rayFrom, const btVector3 &rayTo, const btVector3 &aabbMin, const btVector3 &aabbMax, btScalar &param, btVector3 &normal)
Definition: btAabbUtil2.h:125
TestTriangleAgainstAabb2
bool TestTriangleAgainstAabb2(const btVector3 *vertices, const btVector3 &aabbMin, const btVector3 &aabbMax)
conservative test for overlap between triangle and aabb
Definition: btAabbUtil2.h:59
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292