Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
945b299e56e089ea7cfbbddfc50ff9dc4dd54e1e
bullet3/src/LinearMath/btAabbUtil2.h
erwin.coumans 945b299e56 some rayTest improvements in btDbvt::rayTestInternal, it avoids/reduces memory allocations during stack allocation (by sharing a persistent m_stack) 
and rayTestInternal re-uses precomputed invRayDirection/signs.
also did some performance comparison with different ray-AABB test, from 
http://jgt.akpeters.com/papers/EisemannEtAl07/

In short: it is faster, but it is not clear how to cull ray segments using ray slopes: when rays starts inside the AABB, we get a negative t value, but negatives also get false t-values...
2008-10-16 20:01:02 +00:00

844 lines
19 KiB
C
Raw Blame History

/*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef AABB_UTIL2
#define AABB_UTIL2
#include "btTransform.h"
#include "btVector3.h"
#include "btMinMax.h"
#define TEST_RAY_SLOPES 1
SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
btVector3& aabbMax,
const btVector3& expansionMin,
const btVector3& expansionMax)
{
aabbMin = aabbMin + expansionMin;
aabbMax = aabbMax + expansionMax;
}
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &point)
{
bool overlap = true;
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
return overlap;
}
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &aabbMin2, const btVector3 &aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
return overlap;
}
/// conservative test for overlap between triangle and aabb
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
const btVector3 &aabbMin, const btVector3 &aabbMax)
{
const btVector3 &p1 = vertices[0];
const btVector3 &p2 = vertices[1];
const btVector3 &p3 = vertices[2];
if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
return true;
}
SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
}
/// http://jgt.akpeters.com/papers/EisemannEtAl07/
/// See test case in btDbvt::rayTestInternal on dynamic AABB tree, Bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h
enum CLASSIFICATION
{ MMM, MMP, MPM, MPP, PMM, PMP, PPM, PPP, POO, MOO, OPO, OMO, OOP, OOM,
OMM,OMP,OPM,OPP,MOM,MOP,POM,POP,MMO,MPO,PMO,PPO};
struct btRaySlope
{
//common variables
float x, y, z; // ray origin
float i, j, k; // ray direction
float ii, ij, ik; // inverses of direction components
// ray slope
int classification;
float ibyj, jbyi, kbyj, jbyk, ibyk, kbyi; //slope
float c_xy, c_xz, c_yx, c_yz, c_zx, c_zy;
};
struct btAaboxSlope
{
float x0, y0, z0, x1, y1, z1;
};
SIMD_FORCE_INLINE void btMakeRaySlope(float x, float y, float z, float i, float j, float k, btRaySlope *r)
{
//common variables
r->x = x;
r->y = y;
r->z = z;
r->i = i;
r->j = j;
r->k = k;
r->ii = 1.0f/i;
r->ij = 1.0f/j;
r->ik = 1.0f/k;
//ray slope
r->ibyj = r->i * r->ij;
r->jbyi = r->j * r->ii;
r->jbyk = r->j * r->ik;
r->kbyj = r->k * r->ij;
r->ibyk = r->i * r->ik;
r->kbyi = r->k * r->ii;
r->c_xy = r->y - r->jbyi * r->x;
r->c_xz = r->z - r->kbyi * r->x;
r->c_yx = r->x - r->ibyj * r->y;
r->c_yz = r->z - r->kbyj * r->y;
r->c_zx = r->x - r->ibyk * r->z;
r->c_zy = r->y - r->jbyk * r->z;
//ray slope classification
if(i < 0)
{
if(j < 0)
{
if(k < 0)
{
r->classification = MMM;
}
else if(k > 0){
r->classification = MMP;
}
else//(k >= 0)
{
r->classification = MMO;
}
}
else//(j >= 0)
{
if(k < 0)
{
r->classification = MPM;
if(j==0)
r->classification = MOM;
}
else//(k >= 0)
{
if((j==0) && (k==0))
r->classification = MOO;
else if(k==0)
r->classification = MPO;
else if(j==0)
r->classification = MOP;
else
r->classification = MPP;
}
}
}
else//(i >= 0)
{
if(j < 0)
{
if(k < 0)
{
r->classification = PMM;
if(i==0)
r->classification = OMM;
}
else//(k >= 0)
{
if((i==0) && (k==0))
r->classification = OMO;
else if(k==0)
r->classification = PMO;
else if(i==0)
r->classification = OMP;
else
r->classification = PMP;
}
}
else//(j >= 0)
{
if(k < 0)
{
if((i==0) && (j==0))
r->classification = OOM;
else if(i==0)
r->classification = OPM;
else if(j==0)
r->classification = POM;
else
r->classification = PPM;
}
else//(k > 0)
{
if(i==0)
{
if(j==0)
r->classification = OOP;
else if(k==0)
r->classification = OPO;
else
r->classification = OPP;
}
else
{
if((j==0) && (k==0))
r->classification = POO;
else if(j==0)
r->classification = POP;
else if(k==0)
r->classification = PPO;
else
r->classification = PPP;
}
}
}
}
}
//SIMD_FORCE_INLINE bool slopeint_div(const btRaySlope* r, const btAaboxSlope* b, float *t)
SIMD_FORCE_INLINE bool btRaySlopeAabb(const btRaySlope* r, const btAaboxSlope* b, float *t)
{
switch (r->classification)
{
case MMM:
{
if ((r->x < b->x0) || (r->y < b->y0) || (r->z < b->z0)
|| (r->jbyi * b->x0 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x1 + r->c_yx > 0)
|| (r->jbyk * b->z0 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z1 + r->c_yz > 0)
|| (r->kbyi * b->x0 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MMP:
{
if ((r->x < b->x0) || (r->y < b->y0) || (r->z > b->z1)
|| (r->jbyi * b->x0 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x1 + r->c_yx > 0)
|| (r->jbyk * b->z1 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z0 + r->c_yz < 0)
|| (r->kbyi * b->x0 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MPM:
{
if ((r->x < b->x0) || (r->y > b->y1) || (r->z < b->z0)
|| (r->jbyi * b->x0 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x1 + r->c_yx > 0)
|| (r->jbyk * b->z0 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z1 + r->c_yz > 0)
|| (r->kbyi * b->x0 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MPP:
{
if ((r->x < b->x0) || (r->y > b->y1) || (r->z > b->z1)
|| (r->jbyi * b->x0 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x1 + r->c_yx > 0)
|| (r->jbyk * b->z1 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z0 + r->c_yz < 0)
|| (r->kbyi * b->x0 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case PMM:
{
if ((r->x > b->x1) || (r->y < b->y0) || (r->z < b->z0)
|| (r->jbyi * b->x1 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x0 + r->c_yx < 0)
|| (r->jbyk * b->z0 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z1 + r->c_yz > 0)
|| (r->kbyi * b->x1 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case PMP:
{
if ((r->x > b->x1) || (r->y < b->y0) || (r->z > b->z1)
|| (r->jbyi * b->x1 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x0 + r->c_yx < 0)
|| (r->jbyk * b->z1 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z0 + r->c_yz < 0)
|| (r->kbyi * b->x1 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case PPM:
{
if ((r->x > b->x1) || (r->y > b->y1) || (r->z < b->z0)
|| (r->jbyi * b->x1 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x0 + r->c_yx < 0)
|| (r->jbyk * b->z0 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z1 + r->c_yz > 0)
|| (r->kbyi * b->x1 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case PPP:
{
if ((r->x > b->x1) || (r->y > b->y1) || (r->z > b->z1)
|| (r->jbyi * b->x1 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x0 + r->c_yx < 0)
|| (r->jbyk * b->z1 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z0 + r->c_yz < 0)
|| (r->kbyi * b->x1 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case OMM:
{
if((r->x < b->x0) || (r->x > b->x1)
|| (r->y < b->y0) || (r->z < b->z0)
|| (r->jbyk * b->z0 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z1 + r->c_yz > 0)
)
return false;
*t = (b->y1 - r->y) / r->j;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case OMP:
{
if((r->x < b->x0) || (r->x > b->x1)
|| (r->y < b->y0) || (r->z > b->z1)
|| (r->jbyk * b->z1 - b->y1 + r->c_zy > 0)
|| (r->kbyj * b->y0 - b->z0 + r->c_yz < 0)
)
return false;
*t = (b->y1 - r->y) / r->j;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case OPM:
{
if((r->x < b->x0) || (r->x > b->x1)
|| (r->y > b->y1) || (r->z < b->z0)
|| (r->jbyk * b->z0 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z1 + r->c_yz > 0)
)
return false;
*t = (b->y0 - r->y) / r->j;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case OPP:
{
if((r->x < b->x0) || (r->x > b->x1)
|| (r->y > b->y1) || (r->z > b->z1)
|| (r->jbyk * b->z1 - b->y0 + r->c_zy < 0)
|| (r->kbyj * b->y1 - b->z0 + r->c_yz < 0)
)
return false;
*t = (b->y0 - r->y) / r->j;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MOM:
{
if((r->y < b->y0) || (r->y > b->y1)
|| (r->x < b->x0) || (r->z < b->z0)
|| (r->kbyi * b->x0 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MOP:
{
if((r->y < b->y0) || (r->y > b->y1)
|| (r->x < b->x0) || (r->z > b->z1)
|| (r->kbyi * b->x0 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x1 + r->c_zx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case POM:
{
if((r->y < b->y0) || (r->y > b->y1)
|| (r->x > b->x1) || (r->z < b->z0)
|| (r->kbyi * b->x1 - b->z1 + r->c_xz > 0)
|| (r->ibyk * b->z0 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t2 = (b->z1 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case POP:
{
if((r->y < b->y0) || (r->y > b->y1)
|| (r->x > b->x1) || (r->z > b->z1)
|| (r->kbyi * b->x1 - b->z0 + r->c_xz < 0)
|| (r->ibyk * b->z1 - b->x0 + r->c_zx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t2 = (b->z0 - r->z) / r->k;
if(t2 > *t)
*t = t2;
return true;
}
case MMO:
{
if((r->z < b->z0) || (r->z > b->z1)
|| (r->x < b->x0) || (r->y < b->y0)
|| (r->jbyi * b->x0 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x1 + r->c_yx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
return true;
}
case MPO:
{
if((r->z < b->z0) || (r->z > b->z1)
|| (r->x < b->x0) || (r->y > b->y1)
|| (r->jbyi * b->x0 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x1 + r->c_yx > 0)
)
return false;
*t = (b->x1 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
return true;
}
case PMO:
{
if((r->z < b->z0) || (r->z > b->z1)
|| (r->x > b->x1) || (r->y < b->y0)
|| (r->jbyi * b->x1 - b->y1 + r->c_xy > 0)
|| (r->ibyj * b->y0 - b->x0 + r->c_yx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y1 - r->y) / r->j;
if(t1 > *t)
*t = t1;
return true;
}
case PPO:
{
if((r->z < b->z0) || (r->z > b->z1)
|| (r->x > b->x1) || (r->y > b->y1)
|| (r->jbyi * b->x1 - b->y0 + r->c_xy < 0)
|| (r->ibyj * b->y1 - b->x0 + r->c_yx < 0)
)
return false;
*t = (b->x0 - r->x) / r->i;
float t1 = (b->y0 - r->y) / r->j;
if(t1 > *t)
*t = t1;
return true;
}
case MOO:
{
if((r->x < b->x0)
|| (r->y < b->y0) || (r->y > b->y1)
|| (r->z < b->z0) || (r->z > b->z1)
)
return false;
*t = (b->x1 - r->x) / r->i;
return true;
}
case POO:
{
if((r->x > b->x1)
|| (r->y < b->y0) || (r->y > b->y1)
|| (r->z < b->z0) || (r->z > b->z1)
)
return false;
*t = (b->x0 - r->x) / r->i;
return true;
}
case OMO:
{
if((r->y < b->y0)
|| (r->x < b->x0) || (r->x > b->x1)
|| (r->z < b->z0) || (r->z > b->z1)
)
return false;
*t = (b->y1 - r->y) / r->j;
return true;
}
case OPO:
{
if((r->y > b->y1)
|| (r->x < b->x0) || (r->x > b->x1)
|| (r->z < b->z0) || (r->z > b->z1)
)
return false;
*t = (b->y0 - r->y) / r->j;
return true;
}
case OOM:
{
if((r->z < b->z0)
|| (r->x < b->x0) || (r->x > b->x1)
|| (r->y < b->y0) || (r->y > b->y1)
)
return false;
*t = (b->z1 - r->z) / r->k;
return true;
}
case OOP:
{
if((r->z > b->z1)
|| (r->x < b->x0) || (r->x > b->x1)
|| (r->y < b->y0) || (r->y > b->y1)
)
return false;
*t = (b->z0 - r->z) / r->k;
return true;
}
}
return false;
}
SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
const btVector3& rayInvDirection,
const unsigned int raySign[3],
const btVector3 bounds[2],
btScalar& tmin,
btScalar lambda_min,
btScalar lambda_max)
{
btScalar tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
if ( (tmin > tymax) || (tymin > tmax) )
return false;
if (tymin > tmin)
tmin = tymin;
if (tymax < tmax)
tmax = tymax;
tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
if ( (tmin > tzmax) || (tzmin > tmax) )
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return ( (tmin < lambda_max) && (tmax > lambda_min) );
}
SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
const btVector3& rayTo,
const btVector3& aabbMin,
const btVector3& aabbMax,
btScalar& param, btVector3& normal)
{
btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
btVector3 source = rayFrom - aabbCenter;
btVector3 target = rayTo - aabbCenter;
int sourceOutcode = btOutcode(source,aabbHalfExtent);
int targetOutcode = btOutcode(target,aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
btScalar lambda_enter = btScalar(0.0);
btScalar lambda_exit = param;
btVector3 r = target - source;
int i;
btScalar normSign = 1;
btVector3 hitNormal(0,0,0);
int bit=1;
for (int j=0;j<2;j++)
{
for (i = 0; i != 3; ++i)
{
if (sourceOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
if (lambda_enter <= lambda)
{
lambda_enter = lambda;
hitNormal.setValue(0,0,0);
hitNormal[i] = normSign;
}
}
else if (targetOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
btSetMin(lambda_exit, lambda);
}
bit<<=1;
}
normSign = btScalar(-1.);
}
if (lambda_enter <= lambda_exit)
{
param = lambda_enter;
normal = hitNormal;
return true;
}
}
return false;
}
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
{
btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
abs_b[1].dot(halfExtentsWithMargin),
abs_b[2].dot(halfExtentsWithMargin));
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
{
btAssert(localAabbMin.getX() <= localAabbMax.getX());
btAssert(localAabbMin.getY() <= localAabbMax.getY());
btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=btVector3(margin,margin,margin);
btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
abs_b[1].dot(localHalfExtents),
abs_b[2].dot(localHalfExtents));
aabbMinOut = center-extent;
aabbMaxOut = center+extent;
}
#endif
Reference in New Issue View Git Blame Copy Permalink