dali-physics/third-party/bullet3/src/Bullet3Geometry/b3AabbUtil.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 B3_AABB_UTIL2
  16 #define B3_AABB_UTIL2
  17
  18 #include "Bullet3Common/b3Transform.h"
  19 #include "Bullet3Common/b3Vector3.h"
  20 #include "Bullet3Common/b3MinMax.h"
  21
  22 B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
  23                                                                   b3Vector3& aabbMax,
  24                                                                   const b3Vector3& expansionMin,
  25                                                                   const b3Vector3& expansionMax)
  26 {
  27         aabbMin = aabbMin + expansionMin;
  28         aabbMax = aabbMax + expansionMax;
  29 }
  30
  31 /// conservative test for overlap between two aabbs
  32 B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
  33                                                                                          const b3Vector3& 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 B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
  44                                                                                         const b3Vector3& aabbMin2, const b3Vector3& 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 B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
  55                                                                                                 const b3Vector3& aabbMin, const b3Vector3& aabbMax)
  56 {
  57         const b3Vector3& p1 = vertices[0];
  58         const b3Vector3& p2 = vertices[1];
  59         const b3Vector3& p3 = vertices[2];
  60
  61         if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
  62         if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
  63
  64         if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
  65         if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
  66
  67         if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
  68         if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
  69         return true;
  70 }
  71
  72 B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& 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 B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
  83                                                                 const b3Vector3& rayInvDirection,
  84                                                                 const unsigned int raySign[3],
  85                                                                 const b3Vector3 bounds[2],
  86                                                                 b3Scalar& tmin,
  87                                                                 b3Scalar lambda_min,
  88                                                                 b3Scalar lambda_max)
  89 {
  90         b3Scalar 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 B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
 118                                                            const b3Vector3& rayTo,
 119                                                            const b3Vector3& aabbMin,
 120                                                            const b3Vector3& aabbMax,
 121                                                            b3Scalar& param, b3Vector3& normal)
 122 {
 123         b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
 124         b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
 125         b3Vector3 source = rayFrom - aabbCenter;
 126         b3Vector3 target = rayTo - aabbCenter;
 127         int sourceOutcode = b3Outcode(source, aabbHalfExtent);
 128         int targetOutcode = b3Outcode(target, aabbHalfExtent);
 129         if ((sourceOutcode & targetOutcode) == 0x0)
 130         {
 131                 b3Scalar lambda_enter = b3Scalar(0.0);
 132                 b3Scalar lambda_exit = param;
 133                 b3Vector3 r = target - source;
 134                 int i;
 135                 b3Scalar normSign = 1;
 136                 b3Vector3 hitNormal = b3MakeVector3(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                                         b3Scalar 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                                         b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 156                                         b3SetMin(lambda_exit, lambda);
 157                                 }
 158                                 bit <<= 1;
 159                         }
 160                         normSign = b3Scalar(-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 B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
 173 {
 174         b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
 175         b3Matrix3x3 abs_b = t.getBasis().absolute();
 176         b3Vector3 center = t.getOrigin();
 177         b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
 178         aabbMinOut = center - extent;
 179         aabbMaxOut = center + extent;
 180 }
 181
 182 B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
 183 {
 184         //b3Assert(localAabbMin.getX() <= localAabbMax.getX());
 185         //b3Assert(localAabbMin.getY() <= localAabbMax.getY());
 186         //b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
 187         b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
 188         localHalfExtents += b3MakeVector3(margin, margin, margin);
 189
 190         b3Vector3 localCenter = b3Scalar(0.5) * (localAabbMax + localAabbMin);
 191         b3Matrix3x3 abs_b = trans.getBasis().absolute();
 192         b3Vector3 center = trans(localCenter);
 193         b3Vector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 194         aabbMinOut = center - extent;
 195         aabbMaxOut = center + extent;
 196 }
 197
 198 #define B3_USE_BANCHLESS 1
 199 #ifdef B3_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 B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(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>(b3Select((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 B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(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  //B3_USE_BANCHLESS
 216
 217 #endif  //B3_AABB_UTIL2