231 lines
8.5 KiB
C++
231 lines
8.5 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.
|
|
|
|
#ifndef GU_SCHLOCKSHARED_H
|
|
#define GU_SCHLOCKSHARED_H
|
|
|
|
#include "foundation/PxVec3.h"
|
|
#include "foundation/PxMat33.h"
|
|
#include "foundation/PxTransform.h"
|
|
#include "geometry/PxMeshScale.h"
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
|
|
#define GJK_LOGGING 0
|
|
#define EPA_REPLACE_TRIANGLE_LOGGING 0
|
|
#define EPA_LOGGING 0
|
|
#define EPA_LOG_DEGENERATE_CASES 0
|
|
|
|
#if GJK_LOGGING
|
|
#define GJK_LOG(...) if(gjkDebug) printf(__VA_ARGS__)
|
|
#else
|
|
#define GJK_LOG(...)
|
|
#endif
|
|
|
|
|
|
namespace schlock
|
|
{
|
|
|
|
extern bool epaDebug;
|
|
|
|
using namespace physx;
|
|
|
|
template<typename T>
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE void swap(T& a, T& b)
|
|
{
|
|
T tmp = a;
|
|
a = b;
|
|
b = tmp;
|
|
}
|
|
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU32 encodeIndices(PxU8 a, PxU8 b) { return 0x3f800000u | b<<8 | a;}
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 aIndex(PxU32 indices) { return indices&0xff; }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 bIndex(PxU32 indices) { return (indices>>8)&0xff; }
|
|
|
|
struct CachedVertex
|
|
{
|
|
PxVec3 v;
|
|
PxU32 indices;
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE CachedVertex() {}
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE CachedVertex(PxU8 iA, PxU8 iB, const PxVec3& iV): v(iV), indices(encodeIndices(iA,iB)) {}
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE CachedVertex(const CachedVertex& other): v(other.v), indices(other.indices) {}
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE void operator=(const CachedVertex& other) { v = other.v, indices = other.indices; }
|
|
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 a() const { return aIndex(indices); }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 b() const { return bIndex(indices); }
|
|
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 a() const volatile { return aIndex(indices); }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 b() const volatile { return bIndex(indices); }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxVec3 getV() const volatile { return PxVec3(v.x, v.y, v.z); }
|
|
};
|
|
|
|
struct GjkCachedData
|
|
{
|
|
CachedVertex vertices[4];
|
|
struct
|
|
{
|
|
PxVec3 n;
|
|
PxReal d;
|
|
} normals[4];
|
|
|
|
PxU32 size;
|
|
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE GjkCachedData(){}
|
|
|
|
__device__ void Reset() { size = 0; __syncwarp(); }
|
|
};
|
|
|
|
struct GjkResult
|
|
{
|
|
enum Enum
|
|
{
|
|
eDISTANT,
|
|
eCLOSE,
|
|
eOVERLAP
|
|
};
|
|
};
|
|
|
|
struct GjkOutput
|
|
{
|
|
PxVec3 direction; // unit witness for lower bound
|
|
PxReal lowerBound; // provable lower bound
|
|
PxReal upperBound; // provable upper bound
|
|
|
|
PxVec3 closestPointA; // witnesses for upper bound
|
|
PxVec3 closestPointB;
|
|
|
|
bool degenerate;
|
|
PxVec3 closestPointDir;
|
|
};
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE void barycentrics(const PxVec3& a, const PxVec3& b, const PxVec3& c, float &u, float &v, float &w, const PxVec3& p)
|
|
{
|
|
PxVec3 m = (b - a).cross(c - a), ma = m.abs(), a1 = (b-p).cross(c-p), a2 = (c-p).cross(a-p);
|
|
PxReal d;
|
|
|
|
// area ratios in plane with largest normal component
|
|
if (ma.x >= ma.y && ma.x >= ma.z)
|
|
d = 1.0f/m.x, u = a1.x * d, v = a2.x * d;
|
|
else if (ma.y >= ma.z)
|
|
d = 1.0f/m.y, u = a1.y * d, v = a2.y * d;
|
|
else
|
|
d = 1.0f/m.z, u = a1.z * d, v = a2.z * d;
|
|
|
|
w = 1.0f - u - v;
|
|
}
|
|
|
|
|
|
// top-level functions
|
|
|
|
// unit-level functions for testing
|
|
static const PxU32 EPA_MAX_VERTS = 18, EPA_MAX_FACES = 2*EPA_MAX_VERTS-4; // for N verts, we'll have 2(N-2) faces.
|
|
|
|
typedef int EPABitMap;
|
|
|
|
#define MAKE_Vec3I8(A, B, C) (physx::PxU32(A) | (physx::PxU32(B) << 8) | (physx::PxU32(C) << 16))
|
|
|
|
struct Vec3I8
|
|
{
|
|
PxU32 a;
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE PxU8 get(PxU32 i) const { return (a >> (i << 3)) & 0xFFul; }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE void set(PxU32 i, PxU8 v) { a = (a & ~(0xFFul << (i << 3))) | (PxU32(v) << (i << 3)); }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE void setInt(int i) { a = i; }
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE int getInt() { return a; }
|
|
|
|
PX_FORCE_INLINE PX_CUDA_CALLABLE const Vec3I8& operator=(const Vec3I8& other) {a = other.a; return *this;}
|
|
|
|
bool operator==(const Vec3I8& other) { return (a & 0xFFffFFul) == (other.a & 0xFFffFFul); }
|
|
};
|
|
|
|
struct EPATriangle
|
|
{
|
|
PxVec3 normal; // unit normal
|
|
PxReal distance; // *plane* distance from origin
|
|
Vec3I8 v; // vertices
|
|
};
|
|
|
|
struct EPAHullData
|
|
{
|
|
PxVec3 v[EPA_MAX_VERTS];
|
|
PxU32 i[EPA_MAX_VERTS];
|
|
EPATriangle t[EPA_MAX_FACES];
|
|
Vec3I8 a[EPA_MAX_FACES];
|
|
|
|
PxU8 nbVertices;
|
|
EPABitMap liveMap;
|
|
};
|
|
|
|
|
|
// GJK routine.
|
|
//
|
|
// Input:
|
|
// convexA, convexB: two convexes in the same space
|
|
// initialDir: an unnormalized start direction for GJK iteration
|
|
// minSep: the minimum distance at which the two objects are judged to be separated. Must be positive.
|
|
// convergenceRatio: minimum ratio of lowerBound/upperBound for convergence (a number close to, but less than, 1)
|
|
//
|
|
// returns DISTANT if a lower bound on distance is found that is > minSep
|
|
// OVERLAP if the objects are provably overlapping
|
|
// else CLOSE.
|
|
//
|
|
// for OVERLAP, the cachedData (vertex indices & search directions) can be passed to EPA
|
|
// for DISTANT, a direction is generated which provides a witness for the lower bound
|
|
// for CLOSE, a pair of closest points is generated, which provide (in MS space) a witness for the upper bound, and
|
|
// if nondegenerate, also a direction which witnesses the lower bound
|
|
//
|
|
// the degenerate flag is raised in the output if the algorithm encountered geometric instability while the minSep value is in [lower, upper].
|
|
// In particular, we are not overlapping (lower>minSep) nor overlapping (upper == 0), so this can only accompany a result of CLOSE.
|
|
// geometric instability can only happen very close to convergence, so unless very high tolerances are necessary, convergence can be assumed.
|
|
// in this case if the lower bound is zero, then the the lower bound witness direction is not generated.
|
|
// So when CLOSE is returned, the upper bound should be used as the authoritative distance, and the direction should be taken from the closest points
|
|
|
|
// EPA routine.
|
|
//
|
|
// Input:
|
|
// convexA, convexB: two convexes in the same space
|
|
// cache: a cache of initial vertices produced by GJK. Must have size at least 2
|
|
// convergenceRatio: minimum ratio of lowerBound/upperBound for convergence (a number close to, but less than, 1)
|
|
//
|
|
// on output, the following are emitted:
|
|
// * a pair of closest points which are the best depenetration found in the allowed space
|
|
// * a lower bound and upper bound on penetration depth.
|
|
// * a direction corresponding to the lower bound (this direction *opposes* closestB - closestA), so if we translate object B by lowerBound * direction, we
|
|
// should approximately separate the shapes
|
|
//
|
|
// the degenerate flag is raised in the output if the algorithm encountered geometric instability before convergence, or ran out of space.
|
|
// in this case, the bounds, direction and closest points are still generated.
|
|
//
|
|
// The algorithm returns GjkResult::eOVERLAP in almost all cases. However, it's possible for EPA to find separation even if GJK found penetration,
|
|
// and in this anomalous case it returns GjkResult::eCLOSE
|
|
//
|
|
// // There is currently no direction for the upper bound, which would allow for reliable (if not minimal) separation in degenerate cases
|
|
|
|
}
|
|
#endif
|