285 lines
8.8 KiB
C++
285 lines
8.8 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 "extensions/PxGjkQueryExt.h"
|
|
#include "geometry/PxSphereGeometry.h"
|
|
#include "geometry/PxBoxGeometry.h"
|
|
#include "geometry/PxPlaneGeometry.h"
|
|
#include "geometry/PxCapsuleGeometry.h"
|
|
#include "geometry/PxConvexCoreGeometry.h"
|
|
#include "geometry/PxConvexMeshGeometry.h"
|
|
#include "foundation/PxAllocator.h"
|
|
#include "geomutils/PxContactBuffer.h"
|
|
#include "GuConvexSupport.h"
|
|
#include "GuConvexGeometry.h"
|
|
|
|
using namespace physx;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::SphereSupport::SphereSupport() : radius(0.0f)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::SphereSupport::SphereSupport(PxReal _radius) : radius(_radius)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::SphereSupport::SphereSupport(const PxSphereGeometry& geom) : radius(geom.radius)
|
|
{
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::SphereSupport::getMargin() const
|
|
{
|
|
return radius;
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::SphereSupport::supportLocal(const PxVec3& /*dir*/) const
|
|
{
|
|
return PxVec3(0.0f);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::CapsuleSupport::CapsuleSupport() : radius(0.0f), halfHeight(0.0f)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::CapsuleSupport::CapsuleSupport(PxReal _radius, PxReal _halfHeight) : radius(_radius), halfHeight(_halfHeight)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::CapsuleSupport::CapsuleSupport(const PxCapsuleGeometry& geom) : radius(geom.radius)
|
|
{
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::CapsuleSupport::getMargin() const
|
|
{
|
|
return radius;
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::CapsuleSupport::supportLocal(const PxVec3& dir) const
|
|
{
|
|
return PxVec3(PxSign2(dir.x) * halfHeight, 0.0f, 0.0f);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::BoxSupport::BoxSupport() : halfExtents(0.0f), margin(0.0f)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::BoxSupport::BoxSupport(const PxVec3& _halfExtents, PxReal _margin) : halfExtents(_halfExtents), margin(_margin)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::BoxSupport::BoxSupport(const PxBoxGeometry& box, PxReal _margin) : halfExtents(box.halfExtents), margin(_margin)
|
|
{
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::BoxSupport::getMargin() const
|
|
{
|
|
return margin;
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::BoxSupport::supportLocal(const PxVec3& dir) const
|
|
{
|
|
const PxVec3 d = dir.getNormalized();
|
|
return PxVec3(PxSign2(d.x) * halfExtents.x, PxSign2(d.y) * halfExtents.y, PxSign2(d.z) * halfExtents.z);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::ConvexCoreSupport::ConvexCoreSupport()
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::ConvexCoreSupport::ConvexCoreSupport(const PxConvexCoreGeometry& geom, PxReal margin)
|
|
{
|
|
PX_COMPILE_TIME_ASSERT(sizeof(shapeData) >= sizeof(Gu::ConvexShape));
|
|
|
|
Gu::ConvexShape& convex = *reinterpret_cast<Gu::ConvexShape*>(shapeData);
|
|
Gu::makeConvexShape(geom, PxTransform(PxIdentity), convex);
|
|
convex.margin += margin;
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::ConvexCoreSupport::getMargin() const
|
|
{
|
|
const Gu::ConvexShape& convex = *reinterpret_cast<const Gu::ConvexShape*>(shapeData);
|
|
return convex.margin;
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::ConvexCoreSupport::supportLocal(const PxVec3& dir) const
|
|
{
|
|
const Gu::ConvexShape& convex = *reinterpret_cast<const Gu::ConvexShape*>(shapeData);
|
|
return convex.supportLocal(dir);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::ConvexMeshSupport::ConvexMeshSupport() :
|
|
convexMesh (NULL),
|
|
scale (0.0f),
|
|
scaleRotation (0.0f),
|
|
margin (0.0f)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::ConvexMeshSupport::ConvexMeshSupport(const PxConvexMesh& _convexMesh, const PxVec3& _scale, const PxQuat& _scaleRotation, PxReal _margin) :
|
|
convexMesh (&_convexMesh),
|
|
scale (_scale),
|
|
scaleRotation (_scaleRotation),
|
|
margin (_margin)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::ConvexMeshSupport::ConvexMeshSupport(const PxConvexMeshGeometry& _convexMesh, PxReal _margin) :
|
|
convexMesh (_convexMesh.convexMesh),
|
|
scale (_convexMesh.scale.scale),
|
|
scaleRotation (_convexMesh.scale.rotation),
|
|
margin (_margin)
|
|
{
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::ConvexMeshSupport::getMargin() const
|
|
{
|
|
return margin * scale.minElement();
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::ConvexMeshSupport::supportLocal(const PxVec3& dir) const
|
|
{
|
|
if (convexMesh == NULL)
|
|
return PxVec3(0.0f);
|
|
|
|
PxVec3 d = scaleRotation.rotateInv(scaleRotation.rotate(dir).multiply(scale));
|
|
const PxVec3* verts = convexMesh->getVertices();
|
|
int count = int(convexMesh->getNbVertices());
|
|
float maxDot = -FLT_MAX;
|
|
int index = -1;
|
|
for (int i = 0; i < count; ++i)
|
|
{
|
|
float dot = verts[i].dot(d);
|
|
if (dot > maxDot)
|
|
{
|
|
maxDot = dot;
|
|
index = i;
|
|
}
|
|
}
|
|
|
|
if (index == -1)
|
|
return PxVec3(0);
|
|
|
|
return scaleRotation.rotateInv(scaleRotation.rotate(verts[index]).multiply(scale));
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxGjkQueryExt::ConvexGeomSupport::ConvexGeomSupport() : mType(PxGeometryType::eINVALID)
|
|
{
|
|
}
|
|
|
|
PxGjkQueryExt::ConvexGeomSupport::ConvexGeomSupport(const PxGeometry& geom, PxReal margin)
|
|
{
|
|
mType = PxGeometryType::eINVALID;
|
|
switch (geom.getType())
|
|
{
|
|
case PxGeometryType::eSPHERE:
|
|
{
|
|
mType = PxGeometryType::eSPHERE;
|
|
const PxSphereGeometry& sphere = static_cast<const PxSphereGeometry&>(geom);
|
|
PX_PLACEMENT_NEW(&mSupport, SphereSupport(sphere.radius + margin));
|
|
break;
|
|
}
|
|
case PxGeometryType::eCAPSULE:
|
|
{
|
|
mType = PxGeometryType::eCAPSULE;
|
|
const PxCapsuleGeometry& capsule = static_cast<const PxCapsuleGeometry&>(geom);
|
|
PX_PLACEMENT_NEW(&mSupport, CapsuleSupport(capsule.radius + margin, capsule.halfHeight));
|
|
break;
|
|
}
|
|
case PxGeometryType::eBOX:
|
|
{
|
|
mType = PxGeometryType::eBOX;
|
|
PX_PLACEMENT_NEW(&mSupport, BoxSupport(static_cast<const PxBoxGeometry&>(geom), margin));
|
|
break;
|
|
}
|
|
case PxGeometryType::eCONVEXCORE:
|
|
{
|
|
mType = PxGeometryType::eCONVEXCORE;
|
|
PX_PLACEMENT_NEW(&mSupport, ConvexCoreSupport(static_cast<const PxConvexCoreGeometry&>(geom), margin));
|
|
break;
|
|
}
|
|
case PxGeometryType::eCONVEXMESH:
|
|
{
|
|
mType = PxGeometryType::eCONVEXMESH;
|
|
PX_PLACEMENT_NEW(&mSupport, ConvexMeshSupport(static_cast<const PxConvexMeshGeometry&>(geom), margin));
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
PxGjkQueryExt::ConvexGeomSupport::~ConvexGeomSupport()
|
|
{
|
|
if (isValid())
|
|
reinterpret_cast<Support&>(mSupport).~Support();
|
|
}
|
|
|
|
bool PxGjkQueryExt::ConvexGeomSupport::isValid() const
|
|
{
|
|
return mType != PxGeometryType::eINVALID;
|
|
}
|
|
|
|
PxReal PxGjkQueryExt::ConvexGeomSupport::getMargin() const
|
|
{
|
|
return isValid() ? reinterpret_cast<const Support&>(mSupport).getMargin() : 0.0f;
|
|
}
|
|
|
|
PxVec3 PxGjkQueryExt::ConvexGeomSupport::supportLocal(const PxVec3& dir) const
|
|
{
|
|
return isValid() ? reinterpret_cast<const Support&>(mSupport).supportLocal(dir) : PxVec3(0.0f);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool PxGjkQueryExt::generateContacts(const PxGjkQuery::Support& a, const PxGjkQuery::Support& b, const PxTransform& poseA, const PxTransform& poseB, PxReal contactDistance, PxReal toleranceLength, PxContactBuffer& contactBuffer)
|
|
{
|
|
PxVec3 pointA, pointB, separatingAxis; PxReal separation;
|
|
if (!PxGjkQuery::proximityInfo(a, b, poseA, poseB, contactDistance, toleranceLength, pointA, pointB, separatingAxis, separation))
|
|
return false;
|
|
|
|
PxContactPoint contact;
|
|
contact.point = (pointA + pointB) * 0.5f; // VR: should I make it just pointB?
|
|
contact.normal = separatingAxis;
|
|
contact.separation = separation;
|
|
contactBuffer.contact(contact);
|
|
|
|
return true;
|
|
}
|