== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / LinearMath / btAabbUtil2.h
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 AABB_UTIL2
18 #define 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
35 /// conservative test for overlap between two aabbs
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
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
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 = btVector3(abs_b[0].dot(halfExtentsWithMargin),
188                    abs_b[1].dot(halfExtentsWithMargin),
189                   abs_b[2].dot(halfExtentsWithMargin));
190         aabbMinOut = center - extent;
191         aabbMaxOut = center + extent;
192 }
193
194
195 SIMD_FORCE_INLINE       void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
196 {
197                 btAssert(localAabbMin.getX() <= localAabbMax.getX());
198                 btAssert(localAabbMin.getY() <= localAabbMax.getY());
199                 btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
200                 btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
201                 localHalfExtents+=btVector3(margin,margin,margin);
202
203                 btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
204                 btMatrix3x3 abs_b = trans.getBasis().absolute();  
205                 btVector3 center = trans(localCenter);
206                 btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
207                            abs_b[1].dot(localHalfExtents),
208                           abs_b[2].dot(localHalfExtents));
209                 aabbMinOut = center-extent;
210                 aabbMaxOut = center+extent;
211 }
212
213 #define USE_BANCHLESS 1
214 #ifdef USE_BANCHLESS
215         //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)
216         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)
217         {               
218                 return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
219                         & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
220                         & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
221                         1, 0));
222         }
223 #else
224         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)
225         {
226                 bool overlap = true;
227                 overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
228                 overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
229                 overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
230                 return overlap;
231         }
232 #endif //USE_BANCHLESS
233
234 #endif
235
236