dali-physics/third-party/bullet3/src/LinearMath/btAabbUtil2.h

   1 /*
   2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  https://bulletphysics.org
   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 #ifndef BT_AABB_UTIL2
  16 #define BT_AABB_UTIL2
  17
  18 #include "btTransform.h"
  19 #include "btVector3.h"
  20 #include "btMinMax.h"
  21
  22 SIMD_FORCE_INLINE void AabbExpand(btVector3& aabbMin,
  23                                                                   btVector3& aabbMax,
  24                                                                   const btVector3& expansionMin,
  25                                                                   const btVector3& expansionMax)
  26 {
  27         aabbMin = aabbMin + expansionMin;
  28         aabbMax = aabbMax + expansionMax;
  29 }
  30
  31 /// conservative test for overlap between two aabbs
  32 SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
  33                                                                                          const btVector3& point)
  34 {
  35         bool overlap = true;
  36         overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
  37         overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
  38         overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
  39         return overlap;
  40 }
  41
  42 /// conservative test for overlap between two aabbs
  43 SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
  44                                                                                         const btVector3& aabbMin2, const btVector3& aabbMax2)
  45 {
  46         bool overlap = true;
  47         overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
  48         overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
  49         overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
  50         return overlap;
  51 }
  52
  53 /// conservative test for overlap between triangle and aabb
  54 SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3* vertices,
  55                                                                                                 const btVector3& aabbMin, const btVector3& aabbMax)
  56 {
  57         const btVector3& p1 = vertices[0];
  58         const btVector3& p2 = vertices[1];
  59         const btVector3& p3 = vertices[2];
  60
  61         if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
  62         if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
  63
  64         if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
  65         if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
  66
  67         if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
  68         if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
  69         return true;
  70 }
  71
  72 SIMD_FORCE_INLINE int btOutcode(const btVector3& p, const btVector3& halfExtent)
  73 {
  74         return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
  75                    (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
  76                    (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
  77                    (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
  78                    (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
  79                    (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
  80 }
  81
  82 SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
  83                                                                   const btVector3& rayInvDirection,
  84                                                                   const unsigned int raySign[3],
  85                                                                   const btVector3 bounds[2],
  86                                                                   btScalar& tmin,
  87                                                                   btScalar lambda_min,
  88                                                                   btScalar lambda_max)
  89 {
  90         btScalar tmax, tymin, tymax, tzmin, tzmax;
  91         tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
  92         tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
  93         tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
  94         tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
  95
  96         if ((tmin > tymax) || (tymin > tmax))
  97                 return false;
  98
  99         if (tymin > tmin)
 100                 tmin = tymin;
 101
 102         if (tymax < tmax)
 103                 tmax = tymax;
 104
 105         tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
 106         tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
 107
 108         if ((tmin > tzmax) || (tzmin > tmax))
 109                 return false;
 110         if (tzmin > tmin)
 111                 tmin = tzmin;
 112         if (tzmax < tmax)
 113                 tmax = tzmax;
 114         return ((tmin < lambda_max) && (tmax > lambda_min));
 115 }
 116
 117 SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
 118                                                                  const btVector3& rayTo,
 119                                                                  const btVector3& aabbMin,
 120                                                                  const btVector3& aabbMax,
 121                                                                  btScalar& param, btVector3& normal)
 122 {
 123         btVector3 aabbHalfExtent = (aabbMax - aabbMin) * btScalar(0.5);
 124         btVector3 aabbCenter = (aabbMax + aabbMin) * btScalar(0.5);
 125         btVector3 source = rayFrom - aabbCenter;
 126         btVector3 target = rayTo - aabbCenter;
 127         int sourceOutcode = btOutcode(source, aabbHalfExtent);
 128         int targetOutcode = btOutcode(target, aabbHalfExtent);
 129         if ((sourceOutcode & targetOutcode) == 0x0)
 130         {
 131                 btScalar lambda_enter = btScalar(0.0);
 132                 btScalar lambda_exit = param;
 133                 btVector3 r = target - source;
 134                 int i;
 135                 btScalar normSign = 1;
 136                 btVector3 hitNormal(0, 0, 0);
 137                 int bit = 1;
 138
 139                 for (int j = 0; j < 2; j++)
 140                 {
 141                         for (i = 0; i != 3; ++i)
 142                         {
 143                                 if (sourceOutcode & bit)
 144                                 {
 145                                         btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 146                                         if (lambda_enter <= lambda)
 147                                         {
 148                                                 lambda_enter = lambda;
 149                                                 hitNormal.setValue(0, 0, 0);
 150                                                 hitNormal[i] = normSign;
 151                                         }
 152                                 }
 153                                 else if (targetOutcode & bit)
 154                                 {
 155                                         btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 156                                         btSetMin(lambda_exit, lambda);
 157                                 }
 158                                 bit <<= 1;
 159                         }
 160                         normSign = btScalar(-1.);
 161                 }
 162                 if (lambda_enter <= lambda_exit)
 163                 {
 164                         param = lambda_enter;
 165                         normal = hitNormal;
 166                         return true;
 167                 }
 168         }
 169         return false;
 170 }
 171
 172 SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin, const btTransform& t, btVector3& aabbMinOut, btVector3& aabbMaxOut)
 173 {
 174         btVector3 halfExtentsWithMargin = halfExtents + btVector3(margin, margin, margin);
 175         btMatrix3x3 abs_b = t.getBasis().absolute();
 176         btVector3 center = t.getOrigin();
 177         btVector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
 178         aabbMinOut = center - extent;
 179         aabbMaxOut = center + extent;
 180 }
 181
 182 SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin, const btVector3& localAabbMax, btScalar margin, const btTransform& trans, btVector3& aabbMinOut, btVector3& aabbMaxOut)
 183 {
 184         btAssert(localAabbMin.getX() <= localAabbMax.getX());
 185         btAssert(localAabbMin.getY() <= localAabbMax.getY());
 186         btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
 187         btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin);
 188         localHalfExtents += btVector3(margin, margin, margin);
 189
 190         btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin);
 191         btMatrix3x3 abs_b = trans.getBasis().absolute();
 192         btVector3 center = trans(localCenter);
 193         btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 194         aabbMinOut = center - extent;
 195         aabbMaxOut = center + extent;
 196 }
 197
 198 #define USE_BANCHLESS 1
 199 #ifdef USE_BANCHLESS
 200 //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)
 201 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)
 202 {
 203         return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
 204                                                                                           1, 0));
 205 }
 206 #else
 207 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)
 208 {
 209         bool overlap = true;
 210         overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
 211         overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
 212         overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
 213         return overlap;
 214 }
 215 #endif  //USE_BANCHLESS
 216
 217 #endif  //BT_AABB_UTIL2