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
AabbExpand(btVector3 & aabbMin,btVector3 & aabbMax,const btVector3 & expansionMin,const btVector3 & expansionMax)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
35 /// conservative test for overlap between two aabbs
TestPointAgainstAabb2(const btVector3 & aabbMin1,const btVector3 & aabbMax1,const btVector3 & point)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
47 /// conservative test for overlap between two aabbs
TestAabbAgainstAabb2(const btVector3 & aabbMin1,const btVector3 & aabbMax1,const btVector3 & aabbMin2,const btVector3 & aabbMax2)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
58 /// conservative test for overlap between triangle and aabb
TestTriangleAgainstAabb2(const btVector3 * vertices,const btVector3 & aabbMin,const btVector3 & aabbMax)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
btOutcode(const btVector3 & p,const btVector3 & halfExtent)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
btRayAabb2(const btVector3 & rayFrom,const btVector3 & rayInvDirection,const unsigned int raySign[3],const btVector3 bounds[2],btScalar & tmin,btScalar lambda_min,btScalar lambda_max)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
btRayAabb(const btVector3 & rayFrom,const btVector3 & rayTo,const btVector3 & aabbMin,const btVector3 & aabbMax,btScalar & param,btVector3 & normal)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
btTransformAabb(const btVector3 & halfExtents,btScalar margin,const btTransform & t,btVector3 & aabbMinOut,btVector3 & aabbMaxOut)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
btTransformAabb(const btVector3 & localAabbMin,const btVector3 & localAabbMax,btScalar margin,const btTransform & trans,btVector3 & aabbMinOut,btVector3 & aabbMaxOut)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)
testQuantizedAabbAgainstQuantizedAabb(const unsigned short int * aabbMin1,const unsigned short int * aabbMax1,const unsigned short int * aabbMin2,const unsigned short int * aabbMax2)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
testQuantizedAabbAgainstQuantizedAabb(const unsigned short int * aabbMin1,const unsigned short int * aabbMax1,const unsigned short int * aabbMin2,const unsigned short int * aabbMax2)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
233