448 lines
14 KiB
C++
448 lines
14 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "foundation/PxVec3.h"
|
|
#include "foundation/PxMathIntrinsics.h"
|
|
#include "foundation/PxFPU.h"
|
|
#include "GuIntersectionRayBox.h"
|
|
|
|
using namespace physx;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
/**
|
|
* Computes a ray-AABB intersection.
|
|
* Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
|
|
* Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
|
|
* Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
|
|
*
|
|
* Hence this version is faster as well as more robust than the original one.
|
|
*
|
|
* Should work provided:
|
|
* 1) the integer representation of 0.0f is 0x00000000
|
|
* 2) the sign bit of the float is the most significant one
|
|
*
|
|
* Report bugs: p.terdiman@codercorner.com
|
|
*
|
|
* \param aabb [in] the axis-aligned bounding box
|
|
* \param origin [in] ray origin
|
|
* \param dir [in] ray direction
|
|
* \param coord [out] impact coordinates
|
|
* \return true if ray intersects AABB
|
|
*/
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
#define RAYAABB_EPSILON 0.00001f
|
|
bool Gu::rayAABBIntersect(const PxVec3& minimum, const PxVec3& maximum, const PxVec3& origin, const PxVec3& _dir, PxVec3& coord)
|
|
{
|
|
PxIntBool Inside = PxIntTrue;
|
|
PxVec3 MaxT(-1.0f, -1.0f, -1.0f);
|
|
const PxReal* dir = &_dir.x;
|
|
const PxU32* idir = reinterpret_cast<const PxU32*>(dir);
|
|
// Find candidate planes.
|
|
for(PxU32 i=0;i<3;i++)
|
|
{
|
|
if(origin[i] < minimum[i])
|
|
{
|
|
coord[i] = minimum[i];
|
|
Inside = PxIntFalse;
|
|
|
|
// Calculate T distances to candidate planes
|
|
if(idir[i])
|
|
// if(PX_IR(dir[i]))
|
|
MaxT[i] = (minimum[i] - origin[i]) / dir[i];
|
|
}
|
|
else if(origin[i] > maximum[i])
|
|
{
|
|
coord[i] = maximum[i];
|
|
Inside = PxIntFalse;
|
|
|
|
// Calculate T distances to candidate planes
|
|
if(idir[i])
|
|
// if(PX_IR(dir[i]))
|
|
MaxT[i] = (maximum[i] - origin[i]) / dir[i];
|
|
}
|
|
}
|
|
|
|
// Ray origin inside bounding box
|
|
if(Inside)
|
|
{
|
|
coord = origin;
|
|
return true;
|
|
}
|
|
|
|
// Get largest of the maxT's for final choice of intersection
|
|
PxU32 WhichPlane = 0;
|
|
if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1;
|
|
if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2;
|
|
|
|
// Check final candidate actually inside box
|
|
const PxU32* tmp = reinterpret_cast<const PxU32*>(&MaxT[WhichPlane]);
|
|
if((*tmp)&PX_SIGN_BITMASK)
|
|
// if(PX_IR(MaxT[WhichPlane])&PX_SIGN_BITMASK)
|
|
return false;
|
|
|
|
for(PxU32 i=0;i<3;i++)
|
|
{
|
|
if(i!=WhichPlane)
|
|
{
|
|
coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
|
|
#ifdef RAYAABB_EPSILON
|
|
if(coord[i] < minimum[i] - RAYAABB_EPSILON || coord[i] > maximum[i] + RAYAABB_EPSILON)
|
|
#else
|
|
if(coord[i] < minimum[i] || coord[i] > maximum[i])
|
|
#endif
|
|
return false;
|
|
}
|
|
}
|
|
return true; // ray hits box
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Computes a ray-AABB intersection.
|
|
* Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
|
|
* Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
|
|
* Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
|
|
* Return of intersected face code and parameter by Adam! Also modified behavior for ray starts inside AABB. 2004 :-p
|
|
*
|
|
* Hence this version is faster as well as more robust than the original one.
|
|
*
|
|
* Should work provided:
|
|
* 1) the integer representation of 0.0f is 0x00000000
|
|
* 2) the sign bit of the float is the most significant one
|
|
*
|
|
* Report bugs: p.terdiman@codercorner.com
|
|
*
|
|
* \param minimum [in] the smaller corner of the bounding box
|
|
* \param maximum [in] the larger corner of the bounding box
|
|
* \param origin [in] ray origin
|
|
* \param _dir [in] ray direction
|
|
* \param coord [out] impact coordinates
|
|
* \param t [out] t such that coord = origin + dir * t
|
|
* \return false if ray does not intersect AABB, or ray origin is inside AABB. Else:
|
|
1 + coordinate index of box axis that was hit
|
|
|
|
Note: sign bit that determines if the minimum (0) or maximum (1) of the axis was hit is equal to sign(coord[returnVal-1]).
|
|
*/
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
PxU32 Gu::rayAABBIntersect2(const PxVec3& minimum, const PxVec3& maximum, const PxVec3& origin, const PxVec3& _dir, PxVec3& coord, PxReal & t)
|
|
{
|
|
PxIntBool Inside = PxIntTrue;
|
|
PxVec3 MaxT(-1.0f, -1.0f, -1.0f);
|
|
const PxReal* dir = &_dir.x;
|
|
const PxU32* idir = reinterpret_cast<const PxU32*>(dir);
|
|
// Find candidate planes.
|
|
for(PxU32 i=0;i<3;i++)
|
|
{
|
|
if(origin[i] < minimum[i])
|
|
{
|
|
coord[i] = minimum[i];
|
|
Inside = PxIntFalse;
|
|
|
|
// Calculate T distances to candidate planes
|
|
if(idir[i])
|
|
// if(PX_IR(dir[i]))
|
|
MaxT[i] = (minimum[i] - origin[i]) / dir[i];
|
|
}
|
|
else if(origin[i] > maximum[i])
|
|
{
|
|
coord[i] = maximum[i];
|
|
Inside = PxIntFalse;
|
|
|
|
// Calculate T distances to candidate planes
|
|
if(idir[i])
|
|
// if(PX_IR(dir[i]))
|
|
MaxT[i] = (maximum[i] - origin[i]) / dir[i];
|
|
}
|
|
}
|
|
|
|
// Ray origin inside bounding box
|
|
if(Inside)
|
|
{
|
|
coord = origin;
|
|
t = 0;
|
|
return 1;
|
|
}
|
|
|
|
// Get largest of the maxT's for final choice of intersection
|
|
PxU32 WhichPlane = 0;
|
|
if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1;
|
|
if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2;
|
|
|
|
// Check final candidate actually inside box
|
|
const PxU32* tmp = reinterpret_cast<const PxU32*>(&MaxT[WhichPlane]);
|
|
if((*tmp)&PX_SIGN_BITMASK)
|
|
// if(PX_IR(MaxT[WhichPlane])&PX_SIGN_BITMASK)
|
|
return 0;
|
|
|
|
for(PxU32 i=0;i<3;i++)
|
|
{
|
|
if(i!=WhichPlane)
|
|
{
|
|
coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
|
|
#ifdef RAYAABB_EPSILON
|
|
if(coord[i] < minimum[i] - RAYAABB_EPSILON || coord[i] > maximum[i] + RAYAABB_EPSILON) return 0;
|
|
#else
|
|
if(coord[i] < minimum[i] || coord[i] > maximum[i]) return 0;
|
|
#endif
|
|
}
|
|
}
|
|
t = MaxT[WhichPlane];
|
|
return 1 + WhichPlane; // ray hits box
|
|
}
|
|
|
|
// Collide ray defined by ray origin (ro) and ray direction (rd)
|
|
// with the bounding box. Returns -1 on no collision and the face index
|
|
// for first intersection if a collision is found together with
|
|
// the distance to the collision points (tnear and tfar)
|
|
|
|
// ptchernev:
|
|
// Should we use an enum, or should we keep the anonymous ints?
|
|
// Should we increment the return code by one (return 0 for non intersection)?
|
|
|
|
int Gu::intersectRayAABB(const PxVec3& minimum, const PxVec3& maximum, const PxVec3& ro, const PxVec3& rd, float& tnear, float& tfar)
|
|
{
|
|
// Refactor
|
|
int ret=-1;
|
|
|
|
tnear = -PX_MAX_F32;
|
|
tfar = PX_MAX_F32;
|
|
// PT: why did we change the initial epsilon value?
|
|
#define LOCAL_EPSILON PX_EPS_F32
|
|
//#define LOCAL_EPSILON 0.0001f
|
|
|
|
for(unsigned int a=0;a<3;a++)
|
|
{
|
|
if(rd[a]>-LOCAL_EPSILON && rd[a]<LOCAL_EPSILON)
|
|
{
|
|
if(ro[a]<minimum[a] || ro[a]>maximum[a])
|
|
return -1;
|
|
}
|
|
else
|
|
{
|
|
const PxReal OneOverDir = 1.0f / rd[a];
|
|
PxReal t1 = (minimum[a]-ro[a]) * OneOverDir;
|
|
PxReal t2 = (maximum[a]-ro[a]) * OneOverDir;
|
|
|
|
unsigned int b = a;
|
|
if(t1>t2)
|
|
{
|
|
PxReal t=t1;
|
|
t1=t2;
|
|
t2=t;
|
|
b += 3;
|
|
}
|
|
|
|
if(t1>tnear)
|
|
{
|
|
tnear = t1;
|
|
ret = int(b);
|
|
}
|
|
if(t2<tfar)
|
|
tfar=t2;
|
|
if(tnear>tfar || tfar<LOCAL_EPSILON)
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
if(tnear>tfar || tfar<LOCAL_EPSILON)
|
|
return -1;
|
|
|
|
return ret;
|
|
}
|
|
|
|
// PT: specialized version where oneOverDir is available
|
|
int Gu::intersectRayAABB(const PxVec3& minimum, const PxVec3& maximum, const PxVec3& ro, const PxVec3& rd, const PxVec3& oneOverDir, float& tnear, float& tfar)
|
|
{
|
|
// PT: why did we change the initial epsilon value?
|
|
#define LOCAL_EPSILON PX_EPS_F32
|
|
//#define LOCAL_EPSILON 0.0001f
|
|
|
|
if(physx::intrinsics::abs(rd.x)<LOCAL_EPSILON)
|
|
// if(rd.x>-LOCAL_EPSILON && rd.x<LOCAL_EPSILON)
|
|
if(ro.x<minimum.x || ro.x>maximum.x)
|
|
return -1;
|
|
if(physx::intrinsics::abs(rd.y)<LOCAL_EPSILON)
|
|
// if(rd.y>-LOCAL_EPSILON && rd.y<LOCAL_EPSILON)
|
|
if(ro.y<minimum.y || ro.y>maximum.y)
|
|
return -1;
|
|
if(physx::intrinsics::abs(rd.z)<LOCAL_EPSILON)
|
|
// if(rd.z>-LOCAL_EPSILON && rd.z<LOCAL_EPSILON)
|
|
if(ro.z<minimum.z || ro.z>maximum.z)
|
|
return -1;
|
|
|
|
PxReal t1x = (minimum.x - ro.x) * oneOverDir.x;
|
|
PxReal t2x = (maximum.x - ro.x) * oneOverDir.x;
|
|
PxReal t1y = (minimum.y - ro.y) * oneOverDir.y;
|
|
PxReal t2y = (maximum.y - ro.y) * oneOverDir.y;
|
|
PxReal t1z = (minimum.z - ro.z) * oneOverDir.z;
|
|
PxReal t2z = (maximum.z - ro.z) * oneOverDir.z;
|
|
|
|
int bx;
|
|
int by;
|
|
int bz;
|
|
|
|
if(t1x>t2x)
|
|
{
|
|
PxReal t=t1x; t1x=t2x; t2x=t;
|
|
bx = 3;
|
|
}
|
|
else
|
|
{
|
|
bx = 0;
|
|
}
|
|
|
|
if(t1y>t2y)
|
|
{
|
|
PxReal t=t1y; t1y=t2y; t2y=t;
|
|
by = 4;
|
|
}
|
|
else
|
|
{
|
|
by = 1;
|
|
}
|
|
|
|
if(t1z>t2z)
|
|
{
|
|
PxReal t=t1z; t1z=t2z; t2z=t;
|
|
bz = 5;
|
|
}
|
|
else
|
|
{
|
|
bz = 2;
|
|
}
|
|
|
|
int ret;
|
|
// if(t1x>tnear) // PT: no need to test for the first value
|
|
{
|
|
tnear = t1x;
|
|
ret = bx;
|
|
}
|
|
// tfar = PxMin(tfar, t2x);
|
|
tfar = t2x; // PT: no need to test for the first value
|
|
|
|
if(t1y>tnear)
|
|
{
|
|
tnear = t1y;
|
|
ret = by;
|
|
}
|
|
tfar = PxMin(tfar, t2y);
|
|
|
|
if(t1z>tnear)
|
|
{
|
|
tnear = t1z;
|
|
ret = bz;
|
|
}
|
|
tfar = PxMin(tfar, t2z);
|
|
|
|
if(tnear>tfar || tfar<LOCAL_EPSILON)
|
|
return -1;
|
|
|
|
return ret;
|
|
}
|
|
|
|
bool Gu::intersectRayAABB2(
|
|
const PxVec3& minimum, const PxVec3& maximum, const PxVec3& ro, const PxVec3& rd, float maxDist, float& tnear, float& tfar)
|
|
{
|
|
PX_ASSERT(maximum.x-minimum.x >= GU_MIN_AABB_EXTENT*0.5f);
|
|
PX_ASSERT(maximum.y-minimum.y >= GU_MIN_AABB_EXTENT*0.5f);
|
|
PX_ASSERT(maximum.z-minimum.z >= GU_MIN_AABB_EXTENT*0.5f);
|
|
// not using vector math due to vector to integer pipeline penalties. TODO: verify that it's indeed faster
|
|
namespace i = physx::intrinsics;
|
|
|
|
// P+tD=a; t=(a-P)/D
|
|
// t=(a - p.x)*1/d.x = a/d.x +(- p.x/d.x)
|
|
const PxF32 dEpsilon = 1e-9f;
|
|
// using recipFast fails height field unit tests case where a ray cast from y=10000 to 0 gets clipped to 0.27 in y
|
|
PxF32 invDx = i::recip(i::selectMax(i::abs(rd.x), dEpsilon) * i::sign(rd.x));
|
|
#ifdef RAYAABB_EPSILON
|
|
PxF32 tx0 = (minimum.x - RAYAABB_EPSILON - ro.x) * invDx;
|
|
PxF32 tx1 = (maximum.x + RAYAABB_EPSILON - ro.x) * invDx;
|
|
#else
|
|
PxF32 tx0 = (minimum.x - ro.x) * invDx;
|
|
PxF32 tx1 = (maximum.x - ro.x) * invDx;
|
|
#endif
|
|
PxF32 txMin = i::selectMin(tx0, tx1);
|
|
PxF32 txMax = i::selectMax(tx0, tx1);
|
|
|
|
PxF32 invDy = i::recip(i::selectMax(i::abs(rd.y), dEpsilon) * i::sign(rd.y));
|
|
#ifdef RAYAABB_EPSILON
|
|
PxF32 ty0 = (minimum.y - RAYAABB_EPSILON - ro.y) * invDy;
|
|
PxF32 ty1 = (maximum.y + RAYAABB_EPSILON - ro.y) * invDy;
|
|
#else
|
|
PxF32 ty0 = (minimum.y - ro.y) * invDy;
|
|
PxF32 ty1 = (maximum.y - ro.y) * invDy;
|
|
#endif
|
|
PxF32 tyMin = i::selectMin(ty0, ty1);
|
|
PxF32 tyMax = i::selectMax(ty0, ty1);
|
|
|
|
PxF32 invDz = i::recip(i::selectMax(i::abs(rd.z), dEpsilon) * i::sign(rd.z));
|
|
#ifdef RAYAABB_EPSILON
|
|
PxF32 tz0 = (minimum.z - RAYAABB_EPSILON - ro.z) * invDz;
|
|
PxF32 tz1 = (maximum.z + RAYAABB_EPSILON - ro.z) * invDz;
|
|
#else
|
|
PxF32 tz0 = (minimum.z - ro.z) * invDz;
|
|
PxF32 tz1 = (maximum.z - ro.z) * invDz;
|
|
#endif
|
|
PxF32 tzMin = i::selectMin(tz0, tz1);
|
|
PxF32 tzMax = i::selectMax(tz0, tz1);
|
|
|
|
PxF32 maxOfNears = i::selectMax(i::selectMax(txMin, tyMin), tzMin);
|
|
PxF32 minOfFars = i::selectMin(i::selectMin(txMax, tyMax), tzMax);
|
|
|
|
tnear = i::selectMax(maxOfNears, 0.0f);
|
|
tfar = i::selectMin(minOfFars, maxDist);
|
|
|
|
return (tnear<tfar);
|
|
}
|
|
|
|
bool Gu::intersectRayAABB2(const aos::Vec3VArg minimum, const aos::Vec3VArg maximum,
|
|
const aos::Vec3VArg ro, const aos::Vec3VArg rd, const aos::FloatVArg maxDist,
|
|
aos::FloatV& tnear, aos::FloatV& tfar)
|
|
{
|
|
using namespace aos;
|
|
const FloatV zero = FZero();
|
|
const Vec3V eps = V3Load(1e-9f);
|
|
const Vec3V absRD = V3Max(V3Abs(rd), eps);
|
|
const Vec3V signRD = V3Sign(rd);
|
|
const Vec3V rdV = V3Mul(absRD, signRD);
|
|
const Vec3V rdVRecip = V3Recip(rdV);
|
|
const Vec3V _min = V3Mul(V3Sub(minimum, ro), rdVRecip);
|
|
const Vec3V _max = V3Mul(V3Sub(maximum, ro), rdVRecip);
|
|
const Vec3V min = V3Min(_max, _min);
|
|
const Vec3V max = V3Max(_max, _min);
|
|
const FloatV maxOfNears = FMax(V3GetX(min), FMax(V3GetY(min), V3GetZ(min)));
|
|
const FloatV minOfFars = FMin(V3GetX(max), FMin(V3GetY(max), V3GetZ(max)));
|
|
|
|
tnear = FMax(maxOfNears, zero);
|
|
tfar = FMin(minOfFars, maxDist);
|
|
//tfar = FAdd(FMin(minOfFars, maxDist), V3GetX(eps)); // AP: + epsilon because a test vs empty box should return true
|
|
|
|
return FAllGrtr(tfar, tnear) != 0;
|
|
}
|