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