src/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 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
  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 = 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