Blender V2.61 - r43446

btAabbUtil2.h

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 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.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 
00017 #ifndef AABB_UTIL2
00018 #define AABB_UTIL2
00019 
00020 #include "btTransform.h"
00021 #include "btVector3.h"
00022 #include "btMinMax.h"
00023 
00024 
00025 
00026 SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
00027                                    btVector3& aabbMax,
00028                                    const btVector3& expansionMin,
00029                                    const btVector3& expansionMax)
00030 {
00031     aabbMin = aabbMin + expansionMin;
00032     aabbMax = aabbMax + expansionMax;
00033 }
00034 
00036 SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
00037                                 const btVector3 &point)
00038 {
00039     bool overlap = true;
00040     overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
00041     overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
00042     overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
00043     return overlap;
00044 }
00045 
00046 
00048 SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
00049                                 const btVector3 &aabbMin2, const btVector3 &aabbMax2)
00050 {
00051     bool overlap = true;
00052     overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
00053     overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
00054     overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
00055     return overlap;
00056 }
00057 
00059 SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
00060                                     const btVector3 &aabbMin, const btVector3 &aabbMax)
00061 {
00062     const btVector3 &p1 = vertices[0];
00063     const btVector3 &p2 = vertices[1];
00064     const btVector3 &p3 = vertices[2];
00065 
00066     if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
00067     if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
00068 
00069     if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
00070     if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
00071   
00072     if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
00073     if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
00074     return true;
00075 }
00076 
00077 
00078 SIMD_FORCE_INLINE int   btOutcode(const btVector3& p,const btVector3& halfExtent) 
00079 {
00080     return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
00081            (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
00082            (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
00083            (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
00084            (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
00085            (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
00086 }
00087 
00088 
00089 
00090 SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
00091                                   const btVector3& rayInvDirection,
00092                                   const unsigned int raySign[3],
00093                                   const btVector3 bounds[2],
00094                                   btScalar& tmin,
00095                                   btScalar lambda_min,
00096                                   btScalar lambda_max)
00097 {
00098     btScalar tmax, tymin, tymax, tzmin, tzmax;
00099     tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
00100     tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
00101     tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
00102     tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
00103 
00104     if ( (tmin > tymax) || (tymin > tmax) )
00105         return false;
00106 
00107     if (tymin > tmin)
00108         tmin = tymin;
00109 
00110     if (tymax < tmax)
00111         tmax = tymax;
00112 
00113     tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
00114     tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
00115 
00116     if ( (tmin > tzmax) || (tzmin > tmax) )
00117         return false;
00118     if (tzmin > tmin)
00119         tmin = tzmin;
00120     if (tzmax < tmax)
00121         tmax = tzmax;
00122     return ( (tmin < lambda_max) && (tmax > lambda_min) );
00123 }
00124 
00125 SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
00126                                  const btVector3& rayTo, 
00127                                  const btVector3& aabbMin, 
00128                                  const btVector3& aabbMax,
00129                       btScalar& param, btVector3& normal) 
00130 {
00131     btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
00132     btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
00133     btVector3   source = rayFrom - aabbCenter;
00134     btVector3   target = rayTo - aabbCenter;
00135     int sourceOutcode = btOutcode(source,aabbHalfExtent);
00136     int targetOutcode = btOutcode(target,aabbHalfExtent);
00137     if ((sourceOutcode & targetOutcode) == 0x0)
00138     {
00139         btScalar lambda_enter = btScalar(0.0);
00140         btScalar lambda_exit  = param;
00141         btVector3 r = target - source;
00142         int i;
00143         btScalar    normSign = 1;
00144         btVector3   hitNormal(0,0,0);
00145         int bit=1;
00146 
00147         for (int j=0;j<2;j++)
00148         {
00149             for (i = 0; i != 3; ++i)
00150             {
00151                 if (sourceOutcode & bit)
00152                 {
00153                     btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
00154                     if (lambda_enter <= lambda)
00155                     {
00156                         lambda_enter = lambda;
00157                         hitNormal.setValue(0,0,0);
00158                         hitNormal[i] = normSign;
00159                     }
00160                 }
00161                 else if (targetOutcode & bit) 
00162                 {
00163                     btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
00164                     btSetMin(lambda_exit, lambda);
00165                 }
00166                 bit<<=1;
00167             }
00168             normSign = btScalar(-1.);
00169         }
00170         if (lambda_enter <= lambda_exit)
00171         {
00172             param = lambda_enter;
00173             normal = hitNormal;
00174             return true;
00175         }
00176     }
00177     return false;
00178 }
00179 
00180 
00181 
00182 SIMD_FORCE_INLINE   void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
00183 {
00184     btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
00185     btMatrix3x3 abs_b = t.getBasis().absolute();  
00186     btVector3 center = t.getOrigin();
00187     btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
00188            abs_b[1].dot(halfExtentsWithMargin),
00189           abs_b[2].dot(halfExtentsWithMargin));
00190     aabbMinOut = center - extent;
00191     aabbMaxOut = center + extent;
00192 }
00193 
00194 
00195 SIMD_FORCE_INLINE   void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
00196 {
00197         btAssert(localAabbMin.getX() <= localAabbMax.getX());
00198         btAssert(localAabbMin.getY() <= localAabbMax.getY());
00199         btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
00200         btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
00201         localHalfExtents+=btVector3(margin,margin,margin);
00202 
00203         btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
00204         btMatrix3x3 abs_b = trans.getBasis().absolute();  
00205         btVector3 center = trans(localCenter);
00206         btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
00207                abs_b[1].dot(localHalfExtents),
00208               abs_b[2].dot(localHalfExtents));
00209         aabbMinOut = center-extent;
00210         aabbMaxOut = center+extent;
00211 }
00212 
00213 #define USE_BANCHLESS 1
00214 #ifdef USE_BANCHLESS
00215     //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)
00216     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)
00217     {       
00218         return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
00219             & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
00220             & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
00221             1, 0));
00222     }
00223 #else
00224     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)
00225     {
00226         bool overlap = true;
00227         overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
00228         overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
00229         overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
00230         return overlap;
00231     }
00232 #endif //USE_BANCHLESS
00233 
00234 #endif
00235 
00236