xuanchi/XCEngine
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(physics): wire physx sdk into build

Browse Source
This commit is contained in:
ssdfasd
2026-04-15 12:22:15 +08:00
parent 5bf258df6d
commit 31f40e2cbb
2044 changed files with 752623 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
engine/third_party/physx/source/geomutils/src/distance/GuDistancePointBox.cpp vendored Normal file
View File

@@ -0,0 +1,65 @@
// 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 "GuDistancePointBox.h"
using namespace physx;
PxReal Gu::distancePointBoxSquared( const PxVec3& point,
const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
PxVec3* boxParam)
{
// Compute coordinates of point in box coordinate system
const PxVec3 diff = point - boxOrigin;
PxVec3 closest( boxBase.column0.dot(diff),
boxBase.column1.dot(diff),
boxBase.column2.dot(diff));
// Project test point onto box
PxReal sqrDistance = 0.0f;
for(PxU32 ax=0; ax<3; ax++)
{
if(closest[ax] < -boxExtent[ax])
{
const PxReal delta = closest[ax] + boxExtent[ax];
sqrDistance += delta*delta;
closest[ax] = -boxExtent[ax];
}
else if(closest[ax] > boxExtent[ax])
{
const PxReal delta = closest[ax] - boxExtent[ax];
sqrDistance += delta*delta;
closest[ax] = boxExtent[ax];
}
}
if(boxParam) *boxParam = closest;
return sqrDistance;
}

69
engine/third_party/physx/source/geomutils/src/distance/GuDistancePointBox.h vendored Normal file
View File

@@ -0,0 +1,69 @@
// 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_DISTANCE_POINT_BOX_H
#define GU_DISTANCE_POINT_BOX_H
#include "GuBox.h"
namespace physx
{
namespace Gu
{
/**
Return the square of the minimum distance from the surface of the box to the given point.
\param point The point
\param boxOrigin The origin of the box
\param boxExtent The extent of the box
\param boxBase The orientation of the box
\param boxParam Set to coordinates of the closest point on the box in its local space
*/
PX_PHYSX_COMMON_API PxReal distancePointBoxSquared( const PxVec3& point,
const PxVec3& boxOrigin,
const PxVec3& boxExtent,
const PxMat33& boxBase,
PxVec3* boxParam=NULL);
/**
Return the square of the minimum distance from the surface of the box to the given point.
\param point The point
\param box The box
\param boxParam Set to coordinates of the closest point on the box in its local space
*/
PX_FORCE_INLINE PxReal distancePointBoxSquared( const PxVec3& point,
const Gu::Box& box,
PxVec3* boxParam=NULL)
{
return distancePointBoxSquared(point, box.center, box.extents, box.rot, boxParam);
}
} // namespace Gu
}
#endif

89
engine/third_party/physx/source/geomutils/src/distance/GuDistancePointSegment.h vendored Normal file
View File

@@ -0,0 +1,89 @@
// 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_DISTANCE_POINT_SEGMENT_H
#define GU_DISTANCE_POINT_SEGMENT_H
#include "common/PxPhysXCommonConfig.h"
#include "GuSegment.h"
namespace physx
{
namespace Gu
{
// dir = p1 - p0
PX_FORCE_INLINE PxReal distancePointSegmentSquaredInternal(const PxVec3& p0, const PxVec3& dir, const PxVec3& point, PxReal* param=NULL)
{
PxVec3 diff = point - p0;
PxReal fT = diff.dot(dir);
if(fT<=0.0f)
{
fT = 0.0f;
}
else
{
const PxReal sqrLen = dir.magnitudeSquared();
if(fT>=sqrLen)
{
fT = 1.0f;
diff -= dir;
}
else
{
fT /= sqrLen;
diff -= fT*dir;
}
}
if(param)
*param = fT;
return diff.magnitudeSquared();
}
/**
A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1
Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1.
Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1.
*/
PX_FORCE_INLINE PxReal distancePointSegmentSquared(const PxVec3& p0, const PxVec3& p1, const PxVec3& point, PxReal* param=NULL)
{
return distancePointSegmentSquaredInternal(p0, p1 - p0, point, param);
}
PX_INLINE PxReal distancePointSegmentSquared(const Gu::Segment& segment, const PxVec3& point, PxReal* param=NULL)
{
return distancePointSegmentSquared(segment.p0, segment.p1, point, param);
}
} // namespace Gu
}
#endif

88
engine/third_party/physx/source/geomutils/src/distance/GuDistancePointTetrahedron.cpp vendored Normal file
View File

@@ -0,0 +1,88 @@
// 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 "GuDistancePointTetrahedron.h"
#include "GuDistancePointTriangle.h"
using namespace physx;
PxVec3 Gu::closestPtPointTetrahedron(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, const PxVec3& d, const PxVec4& result)
{
const PxVec3 ab = b - a;
const PxVec3 ac = c - a;
const PxVec3 ad = d - a;
const PxVec3 bc = c - b;
const PxVec3 bd = d - b;
//point is outside of this face
PxVec3 bestClosestPt(0.f, 0.f, 0.f);
PxReal bestSqDist = PX_MAX_F32;
if (result.x < 0.f)
{
// 0, 1, 2
bestClosestPt = closestPtPointTriangle2(p, a, b, c, ab, ac);
bestSqDist = bestClosestPt.dot(bestClosestPt);
}
if (result.y < 0.f)
{
// 0, 2, 3
const PxVec3 closestPt = closestPtPointTriangle2(p, a, c, d, ac, ad);
const PxReal sqDist = closestPt.dot(closestPt);
if (sqDist < bestSqDist)
{
bestClosestPt = closestPt;
bestSqDist = sqDist;
}
}
if (result.z < 0.f)
{
// 0, 3, 1
const PxVec3 closestPt = closestPtPointTriangle2(p, a, d, b, ad, ab);
const PxReal sqDist = closestPt.dot(closestPt);
if (sqDist < bestSqDist)
{
bestClosestPt = closestPt;
bestSqDist = sqDist;
}
}
if (result.w < 0.f)
{
// 1, 3, 2
const PxVec3 closestPt = closestPtPointTriangle2(p, b, d, c, bd, bc);
const PxReal sqDist = closestPt.dot(closestPt);
if (sqDist < bestSqDist)
{
bestClosestPt = closestPt;
bestSqDist = sqDist;
}
}
return bestClosestPt;
}

382
engine/third_party/physx/source/geomutils/src/distance/GuDistancePointTriangle.cpp vendored Normal file
View File

@@ -0,0 +1,382 @@
// 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 "GuDistancePointTriangle.h"
using namespace physx;
// Based on Christer Ericson's book
PxVec3 Gu::closestPtPointTriangle(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, float& s, float& t)
{
// Check if P in vertex region outside A
const PxVec3 ab = b - a;
const PxVec3 ac = c - a;
const PxVec3 ap = p - a;
const float d1 = ab.dot(ap);
const float d2 = ac.dot(ap);
if(d1<=0.0f && d2<=0.0f)
{
s = 0.0f;
t = 0.0f;
return a; // Barycentric coords 1,0,0
}
// Check if P in vertex region outside B
const PxVec3 bp = p - b;
const float d3 = ab.dot(bp);
const float d4 = ac.dot(bp);
if(d3>=0.0f && d4<=d3)
{
s = 1.0f;
t = 0.0f;
return b; // Barycentric coords 0,1,0
}
// Check if P in edge region of AB, if so return projection of P onto AB
const float vc = d1*d4 - d3*d2;
if(vc<=0.0f && d1>=0.0f && d3<=0.0f)
{
const float v = d1 / (d1 - d3);
s = v;
t = 0.0f;
return a + v * ab; // barycentric coords (1-v, v, 0)
}
// Check if P in vertex region outside C
const PxVec3 cp = p - c;
const float d5 = ab.dot(cp);
const float d6 = ac.dot(cp);
if(d6>=0.0f && d5<=d6)
{
s = 0.0f;
t = 1.0f;
return c; // Barycentric coords 0,0,1
}
// Check if P in edge region of AC, if so return projection of P onto AC
const float vb = d5*d2 - d1*d6;
if(vb<=0.0f && d2>=0.0f && d6<=0.0f)
{
const float w = d2 / (d2 - d6);
s = 0.0f;
t = w;
return a + w * ac; // barycentric coords (1-w, 0, w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
const float va = d3*d6 - d5*d4;
if(va<=0.0f && (d4-d3)>=0.0f && (d5-d6)>=0.0f)
{
const float w = (d4-d3) / ((d4 - d3) + (d5-d6));
s = 1.0f-w;
t = w;
return b + w * (c-b); // barycentric coords (0, 1-w, w)
}
// P inside face region. Compute Q through its barycentric coords (u,v,w)
const float denom = 1.0f / (va + vb + vc);
const float v = vb * denom;
const float w = vc * denom;
s = v;
t = w;
return a + ab*v + ac*w;
}
//aos::FloatV Gu::distancePointTriangleSquared( const aos::Vec3VArg p,
// const aos::Vec3VArg a,
// const aos::Vec3VArg b,
// const aos::Vec3VArg c,
// aos::FloatV& u,
// aos::FloatV& v,
// aos::Vec3V& closestP)
//{
// using namespace aos;
//
// const FloatV zero = FZero();
// const FloatV one = FOne();
// //const Vec3V zero = V3Zero();
// const Vec3V ab = V3Sub(b, a);
// const Vec3V ac = V3Sub(c, a);
// const Vec3V bc = V3Sub(c, b);
// const Vec3V ap = V3Sub(p, a);
// const Vec3V bp = V3Sub(p, b);
// const Vec3V cp = V3Sub(p, c);
//
// const FloatV d1 = V3Dot(ab, ap); // snom
// const FloatV d2 = V3Dot(ac, ap); // tnom
// const FloatV d3 = V3Dot(ab, bp); // -sdenom
// const FloatV d4 = V3Dot(ac, bp); // unom = d4 - d3
// const FloatV d5 = V3Dot(ab, cp); // udenom = d5 - d6
// const FloatV d6 = V3Dot(ac, cp); // -tdenom
// const FloatV unom = FSub(d4, d3);
// const FloatV udenom = FSub(d5, d6);
//
// //check if p in vertex region outside a
// const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
// const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
// const BoolV con0 = BAnd(con00, con01); // vertex region a
// const FloatV u0 = zero;
// const FloatV v0 = zero;
//
// //check if p in vertex region outside b
// const BoolV con10 = FIsGrtrOrEq(d3, zero);
// const BoolV con11 = FIsGrtrOrEq(d3, d4);
// const BoolV con1 = BAnd(con10, con11); // vertex region b
// const FloatV u1 = one;
// const FloatV v1 = zero;
//
// //check if p in vertex region outside c
// const BoolV con20 = FIsGrtrOrEq(d6, zero);
// const BoolV con21 = FIsGrtrOrEq(d6, d5);
// const BoolV con2 = BAnd(con20, con21); // vertex region c
// const FloatV u2 = zero;
// const FloatV v2 = one;
//
// //check if p in edge region of AB
// const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
//
// const BoolV con30 = FIsGrtr(zero, vc);
// const BoolV con31 = FIsGrtrOrEq(d1, zero);
// const BoolV con32 = FIsGrtr(zero, d3);
// const BoolV con3 = BAnd(con30, BAnd(con31, con32));
// const FloatV sScale = FDiv(d1, FSub(d1, d3));
// const Vec3V closest3 = V3Add(a, V3Scale(ab, sScale));
// const FloatV u3 = sScale;
// const FloatV v3 = zero;
//
// //check if p in edge region of BC
// const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
// const BoolV con40 = FIsGrtr(zero, va);
// const BoolV con41 = FIsGrtrOrEq(d4, d3);
// const BoolV con42 = FIsGrtrOrEq(d5, d6);
// const BoolV con4 = BAnd(con40, BAnd(con41, con42));
// const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
// const Vec3V closest4 = V3Add(b, V3Scale(bc, uScale));
// const FloatV u4 = FSub(one, uScale);
// const FloatV v4 = uScale;
//
// //check if p in edge region of AC
// const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
// const BoolV con50 = FIsGrtr(zero, vb);
// const BoolV con51 = FIsGrtrOrEq(d2, zero);
// const BoolV con52 = FIsGrtr(zero, d6);
// const BoolV con5 = BAnd(con50, BAnd(con51, con52));
// const FloatV tScale = FDiv(d2, FSub(d2, d6));
// const Vec3V closest5 = V3Add(a, V3Scale(ac, tScale));
// const FloatV u5 = zero;
// const FloatV v5 = tScale;
//
// //P must project inside face region. Compute Q using Barycentric coordinates
// const FloatV denom = FRecip(FAdd(va, FAdd(vb, vc)));
// const FloatV t = FMul(vb, denom);
// const FloatV w = FMul(vc, denom);
// const Vec3V bCom = V3Scale(ab, t);
// const Vec3V cCom = V3Scale(ac, w);
// const Vec3V closest6 = V3Add(a, V3Add(bCom, cCom));
// const FloatV u6 = t;
// const FloatV v6 = w;
//
// const Vec3V closest= V3Sel(con0, a, V3Sel(con1, b, V3Sel(con2, c, V3Sel(con3, closest3, V3Sel(con4, closest4, V3Sel(con5, closest5, closest6))))));
// u = FSel(con0, u0, FSel(con1, u1, FSel(con2, u2, FSel(con3, u3, FSel(con4, u4, FSel(con5, u5, u6))))));
// v = FSel(con0, v0, FSel(con1, v1, FSel(con2, v2, FSel(con3, v3, FSel(con4, v4, FSel(con5, v5, v6))))));
// closestP = closest;
//
// const Vec3V vv = V3Sub(p, closest);
//
// return V3Dot(vv, vv);
//}
PX_PHYSX_COMMON_API aos::FloatV Gu::distancePointTriangleSquared2UnitBox(
const aos::Vec3VArg queryPoint,
const aos::Vec3VArg triA,
const aos::Vec3VArg triB,
const aos::Vec3VArg triC,
aos::FloatV& u,
aos::FloatV& v,
aos::Vec3V& closestP)
{
using namespace aos;
const Vec3V min = V3Min(V3Min(triA, triB), V3Min(triC, queryPoint));
const Vec3V max = V3Max(V3Max(triA, triB), V3Max(triC, queryPoint));
const Vec3V size = V3Sub(max, min);
FloatV invScaling = FMax(FLoad(1e-12f), V3ExtractMax(size));
const FloatV one = FOne();
FloatV scaling = FDiv(one, invScaling);
const Vec3V p = V3Scale(V3Sub(queryPoint, min), scaling);
const Vec3V a = V3Scale(V3Sub(triA, min), scaling);
const Vec3V b = V3Scale(V3Sub(triB, min), scaling);
const Vec3V c = V3Scale(V3Sub(triC, min), scaling);
Vec3V cp;
FloatV result = Gu::distancePointTriangleSquared(p, a, b, c, u, v, cp);
closestP = V3Add(V3Scale(cp, invScaling), min);
return FMul(result, FMul(invScaling, invScaling));
}
aos::FloatV Gu::distancePointTriangleSquared( const aos::Vec3VArg p,
const aos::Vec3VArg a,
const aos::Vec3VArg b,
const aos::Vec3VArg c,
aos::FloatV& u,
aos::FloatV& v,
aos::Vec3V& closestP)
{
using namespace aos;
const FloatV zero = FZero();
const FloatV one = FOne();
//const Vec3V zero = V3Zero();
const Vec3V ab = V3Sub(b, a);
const Vec3V ac = V3Sub(c, a);
const Vec3V bc = V3Sub(c, b);
const Vec3V ap = V3Sub(p, a);
const Vec3V bp = V3Sub(p, b);
const Vec3V cp = V3Sub(p, c);
const FloatV d1 = V3Dot(ab, ap); // snom
const FloatV d2 = V3Dot(ac, ap); // tnom
const FloatV d3 = V3Dot(ab, bp); // -sdenom
const FloatV d4 = V3Dot(ac, bp); // unom = d4 - d3
const FloatV d5 = V3Dot(ab, cp); // udenom = d5 - d6
const FloatV d6 = V3Dot(ac, cp); // -tdenom
const FloatV unom = FSub(d4, d3);
const FloatV udenom = FSub(d5, d6);
//check if p in vertex region outside a
const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
const BoolV con0 = BAnd(con00, con01); // vertex region a
if(BAllEqTTTT(con0))
{
u = zero;
v = zero;
const Vec3V vv = V3Sub(p, a);
closestP = a;
return V3Dot(vv, vv);
}
//check if p in vertex region outside b
const BoolV con10 = FIsGrtrOrEq(d3, zero);
const BoolV con11 = FIsGrtrOrEq(d3, d4);
const BoolV con1 = BAnd(con10, con11); // vertex region b
if(BAllEqTTTT(con1))
{
u = one;
v = zero;
const Vec3V vv = V3Sub(p, b);
closestP = b;
return V3Dot(vv, vv);
}
//check if p in vertex region outside c
const BoolV con20 = FIsGrtrOrEq(d6, zero);
const BoolV con21 = FIsGrtrOrEq(d6, d5);
const BoolV con2 = BAnd(con20, con21); // vertex region c
if(BAllEqTTTT(con2))
{
u = zero;
v = one;
const Vec3V vv = V3Sub(p, c);
closestP = c;
return V3Dot(vv, vv);
}
//check if p in edge region of AB
const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
const BoolV con30 = FIsGrtr(zero, vc);
const BoolV con31 = FIsGrtrOrEq(d1, zero);
const BoolV con32 = FIsGrtr(zero, d3);
const BoolV con3 = BAnd(con30, BAnd(con31, con32));
if(BAllEqTTTT(con3))
{
const FloatV sScale = FDiv(d1, FSub(d1, d3));
const Vec3V closest3 = V3Add(a, V3Scale(ab, sScale));
u = sScale;
v = zero;
const Vec3V vv = V3Sub(p, closest3);
closestP = closest3;
return V3Dot(vv, vv);
}
//check if p in edge region of BC
const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
const BoolV con40 = FIsGrtr(zero, va);
const BoolV con41 = FIsGrtrOrEq(d4, d3);
const BoolV con42 = FIsGrtrOrEq(d5, d6);
const BoolV con4 = BAnd(con40, BAnd(con41, con42));
if(BAllEqTTTT(con4))
{
const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
const Vec3V closest4 = V3Add(b, V3Scale(bc, uScale));
u = FSub(one, uScale);
v = uScale;
const Vec3V vv = V3Sub(p, closest4);
closestP = closest4;
return V3Dot(vv, vv);
}
//check if p in edge region of AC
const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
const BoolV con50 = FIsGrtr(zero, vb);
const BoolV con51 = FIsGrtrOrEq(d2, zero);
const BoolV con52 = FIsGrtr(zero, d6);
const BoolV con5 = BAnd(con50, BAnd(con51, con52));
if(BAllEqTTTT(con5))
{
const FloatV tScale = FDiv(d2, FSub(d2, d6));
const Vec3V closest5 = V3Add(a, V3Scale(ac, tScale));
u = zero;
v = tScale;
const Vec3V vv = V3Sub(p, closest5);
closestP = closest5;
return V3Dot(vv, vv);
}
//P must project inside face region. Compute Q using Barycentric coordinates
const FloatV denom = FRecip(FAdd(va, FAdd(vb, vc)));
const FloatV t = FMul(vb, denom);
const FloatV w = FMul(vc, denom);
const Vec3V bCom = V3Scale(ab, t);
const Vec3V cCom = V3Scale(ac, w);
const Vec3V closest6 = V3Add(a, V3Add(bCom, cCom));
u = t;
v = w;
closestP = closest6;
const Vec3V vv = V3Sub(p, closest6);
return V3Dot(vv, vv);
}

548
engine/third_party/physx/source/geomutils/src/distance/GuDistanceSegmentBox.cpp vendored Normal file
View File

@@ -0,0 +1,548 @@
// 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 "GuDistanceSegmentBox.h"
#include "GuDistancePointBox.h"
#include "GuDistanceSegmentSegment.h"
#include "GuDistancePointSegment.h"
#include "GuIntersectionRayBox.h"
using namespace physx;
static void face(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, const PxVec3& rkPmE, PxReal* pfLParam, PxReal& rfSqrDistance)
{
PxVec3 kPpE;
PxReal fLSqr, fInv, fTmp, fParam, fT, fDelta;
kPpE[i1] = rkPnt[i1] + extents[i1];
kPpE[i2] = rkPnt[i2] + extents[i2];
if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
{
if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
{
// v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
if(pfLParam)
{
rkPnt[i0] = extents[i0];
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
*pfLParam = -rkPmE[i0]*fInv;
}
}
else
{
// v[i1] >= -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
if(fTmp <= 2.0f*fLSqr*extents[i1])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
}
else
{
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
}
else
{
if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
{
// v[i1] < -e[i1], v[i2] >= -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
if(fTmp <= 2.0f*fLSqr*extents[i2])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
}
else
{
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
}
}
else
{
// v[i1] < -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
if(fTmp >= 0.0f)
{
// v[i1]-edge is closest
if ( fTmp <= 2.0f*fLSqr*extents[i1] )
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
}
else
{
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
}
return;
}
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
if(fTmp >= 0.0f)
{
// v[i2]-edge is closest
if(fTmp <= 2.0f*fLSqr*extents[i2])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
}
else
{
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
}
return;
}
// (v[i1],v[i2])-corner is closest
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
}
static void caseNoZeros(PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
{
PxVec3 kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
PxReal fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
fProdDxPy = rkDir.x*kPmE.y;
fProdDyPx = rkDir.y*kPmE.x;
if(fProdDyPx >= fProdDxPy)
{
fProdDzPx = rkDir.z*kPmE.x;
fProdDxPz = rkDir.x*kPmE.z;
if(fProdDzPx >= fProdDxPz)
{
// line intersects x = e0
face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
else
{
// line intersects z = e2
face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
}
else
{
fProdDzPy = rkDir.z*kPmE.y;
fProdDyPz = rkDir.y*kPmE.z;
if(fProdDzPy >= fProdDyPz)
{
// line intersects y = e1
face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
else
{
// line intersects z = e2
face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
}
}
static void case0(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
{
PxReal fPmE0 = rkPnt[i0] - extents[i0];
PxReal fPmE1 = rkPnt[i1] - extents[i1];
PxReal fProd0 = rkDir[i1]*fPmE0;
PxReal fProd1 = rkDir[i0]*fPmE1;
PxReal fDelta, fInvLSqr, fInv;
if(fProd0 >= fProd1)
{
// line intersects P[i0] = e[i0]
rkPnt[i0] = extents[i0];
PxReal fPpE1 = rkPnt[i1] + extents[i1];
fDelta = fProd0 - rkDir[i0]*fPpE1;
if(fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if(pfLParam)
{
rkPnt[i1] = -extents[i1];
*pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
}
}
else
{
if(pfLParam)
{
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= fProd0*fInv;
*pfLParam = -fPmE0*fInv;
}
}
}
else
{
// line intersects P[i1] = e[i1]
rkPnt[i1] = extents[i1];
PxReal fPpE0 = rkPnt[i0] + extents[i0];
fDelta = fProd1 - rkDir[i1]*fPpE0;
if(fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if(pfLParam)
{
rkPnt[i0] = -extents[i0];
*pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
}
}
else
{
if(pfLParam)
{
fInv = 1.0f/rkDir[i1];
rkPnt[i0] -= fProd1*fInv;
*pfLParam = -fPmE1*fInv;
}
}
}
if(rkPnt[i2] < -extents[i2])
{
fDelta = rkPnt[i2] + extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -extents[i2];
}
else if ( rkPnt[i2] > extents[i2] )
{
fDelta = rkPnt[i2] - extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = extents[i2];
}
}
static void case00(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
{
PxReal fDelta;
if(pfLParam)
*pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
rkPnt[i0] = extents[i0];
if(rkPnt[i1] < -extents[i1])
{
fDelta = rkPnt[i1] + extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = -extents[i1];
}
else if(rkPnt[i1] > extents[i1])
{
fDelta = rkPnt[i1] - extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = extents[i1];
}
if(rkPnt[i2] < -extents[i2])
{
fDelta = rkPnt[i2] + extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -extents[i2];
}
else if(rkPnt[i2] > extents[i2])
{
fDelta = rkPnt[i2] - extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = extents[i2];
}
}
static void case000(PxVec3& rkPnt, const PxVec3& extents, PxReal& rfSqrDistance)
{
PxReal fDelta;
if(rkPnt.x < -extents.x)
{
fDelta = rkPnt.x + extents.x;
rfSqrDistance += fDelta*fDelta;
rkPnt.x = -extents.x;
}
else if(rkPnt.x > extents.x)
{
fDelta = rkPnt.x - extents.x;
rfSqrDistance += fDelta*fDelta;
rkPnt.x = extents.x;
}
if(rkPnt.y < -extents.y)
{
fDelta = rkPnt.y + extents.y;
rfSqrDistance += fDelta*fDelta;
rkPnt.y = -extents.y;
}
else if(rkPnt.y > extents.y)
{
fDelta = rkPnt.y - extents.y;
rfSqrDistance += fDelta*fDelta;
rkPnt.y = extents.y;
}
if(rkPnt.z < -extents.z)
{
fDelta = rkPnt.z + extents.z;
rfSqrDistance += fDelta*fDelta;
rkPnt.z = -extents.z;
}
else if(rkPnt.z > extents.z)
{
fDelta = rkPnt.z - extents.z;
rfSqrDistance += fDelta*fDelta;
rkPnt.z = extents.z;
}
}
//! Compute the smallest distance from the (infinite) line to the box.
static PxReal distanceLineBoxSquared(const PxVec3& lineOrigin, const PxVec3& lineDirection,
const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
PxReal* lineParam,
PxVec3* boxParam)
{
const PxVec3& axis0 = boxBase.column0;
const PxVec3& axis1 = boxBase.column1;
const PxVec3& axis2 = boxBase.column2;
// compute coordinates of line in box coordinate system
const PxVec3 diff = lineOrigin - boxOrigin;
PxVec3 pnt(diff.dot(axis0), diff.dot(axis1), diff.dot(axis2));
PxVec3 dir(lineDirection.dot(axis0), lineDirection.dot(axis1), lineDirection.dot(axis2));
// Apply reflections so that direction vector has nonnegative components.
bool reflect[3];
for(unsigned int i=0;i<3;i++)
{
if(dir[i]<0.0f)
{
pnt[i] = -pnt[i];
dir[i] = -dir[i];
reflect[i] = true;
}
else
{
reflect[i] = false;
}
}
PxReal sqrDistance = 0.0f;
if(dir.x>0.0f)
{
if(dir.y>0.0f)
{
if(dir.z>0.0f) caseNoZeros(pnt, dir, boxExtent, lineParam, sqrDistance); // (+,+,+)
else case0(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,+,0)
}
else
{
if(dir.z>0.0f) case0(0, 2, 1, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,0,+)
else case00(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,0,0)
}
}
else
{
if(dir.y>0.0f)
{
if(dir.z>0.0f) case0(1, 2, 0, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,+,+)
else case00(1, 0, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,+,0)
}
else
{
if(dir.z>0.0f) case00(2, 0, 1, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,0,+)
else
{
case000(pnt, boxExtent, sqrDistance); // (0,0,0)
if(lineParam)
*lineParam = 0.0f;
}
}
}
if(boxParam)
{
// undo reflections
for(unsigned int i=0;i<3;i++)
{
if(reflect[i])
pnt[i] = -pnt[i];
}
*boxParam = pnt;
}
return sqrDistance;
}
//! Compute the smallest distance from the (finite) line segment to the box.
PxReal Gu::distanceSegmentBoxSquared( const PxVec3& segmentPoint0, const PxVec3& segmentPoint1,
const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
PxReal* segmentParam,
PxVec3* boxParam)
{
// compute coordinates of line in box coordinate system
PxReal lp;
PxVec3 bp;
PxReal sqrDistance = distanceLineBoxSquared(segmentPoint0, segmentPoint1 - segmentPoint0, boxOrigin, boxExtent, boxBase, &lp, &bp);
if(lp>=0.0f)
{
if(lp<=1.0f)
{
if(segmentParam)
*segmentParam = lp;
if(boxParam)
*boxParam = bp;
return sqrDistance;
}
else
{
if(segmentParam)
*segmentParam = 1.0f;
return Gu::distancePointBoxSquared(segmentPoint1, boxOrigin, boxExtent, boxBase, boxParam);
}
}
else
{
if(segmentParam)
*segmentParam = 0.0f;
return Gu::distancePointBoxSquared(segmentPoint0, boxOrigin, boxExtent, boxBase, boxParam);
}
}

573
engine/third_party/physx/source/geomutils/src/distance/GuDistanceSegmentSegment.cpp vendored Normal file
View File

@@ -0,0 +1,573 @@
// 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 "GuDistanceSegmentSegment.h"
using namespace physx;
using namespace aos;
static const float ZERO_TOLERANCE = 1e-06f;
// S0 = origin + extent * dir;
// S1 = origin - extent * dir;
PxReal Gu::distanceSegmentSegmentSquared( const PxVec3& origin0, const PxVec3& dir0, PxReal extent0,
const PxVec3& origin1, const PxVec3& dir1, PxReal extent1,
PxReal* param0, PxReal* param1)
{
const PxVec3 kDiff = origin0 - origin1;
const PxReal fA01 = -dir0.dot(dir1);
const PxReal fB0 = kDiff.dot(dir0);
const PxReal fB1 = -kDiff.dot(dir1);
const PxReal fC = kDiff.magnitudeSquared();
const PxReal fDet = PxAbs(1.0f - fA01*fA01);
PxReal fS0, fS1, fSqrDist, fExtDet0, fExtDet1, fTmpS0, fTmpS1;
if (fDet >= ZERO_TOLERANCE)
{
// segments are not parallel
fS0 = fA01*fB1-fB0;
fS1 = fA01*fB0-fB1;
fExtDet0 = extent0*fDet;
fExtDet1 = extent1*fDet;
if (fS0 >= -fExtDet0)
{
if (fS0 <= fExtDet0)
{
if (fS1 >= -fExtDet1)
{
if (fS1 <= fExtDet1) // region 0 (interior)
{
// minimum at two interior points of 3D lines
PxReal fInvDet = 1.0f/fDet;
fS0 *= fInvDet;
fS1 *= fInvDet;
fSqrDist = fS0*(fS0+fA01*fS1+2.0f*fB0) + fS1*(fA01*fS0+fS1+2.0f*fB1)+fC;
}
else // region 3 (side)
{
fS1 = extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 < -extent0)
{
fS0 = -extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 <= extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
}
}
else // region 7 (side)
{
fS1 = -extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 < -extent0)
{
fS0 = -extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 <= extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
}
}
else
{
if (fS1 >= -fExtDet1)
{
if (fS1 <= fExtDet1) // region 1 (side)
{
fS0 = extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 < -extent1)
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 <= extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0)+fC;
}
else
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
else // region 2 (corner)
{
fS1 = extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 < -extent0)
{
fS0 = -extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 <= extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 < -extent1)
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 <= extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
}
else
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
}
}
else // region 8 (corner)
{
fS1 = -extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 < -extent0)
{
fS0 = -extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 <= extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 > extent1)
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 >= -extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
}
else
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
}
}
}
else
{
if (fS1 >= -fExtDet1)
{
if (fS1 <= fExtDet1) // region 5 (side)
{
fS0 = -extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 < -extent1)
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 <= extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0)+fC;
}
else
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
else // region 4 (corner)
{
fS1 = extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 > extent0)
{
fS0 = extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 >= -extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = -extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 < -extent1)
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 <= extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
}
else
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
}
}
else // region 6 (corner)
{
fS1 = -extent1;
fTmpS0 = -(fA01*fS1+fB0);
if (fTmpS0 > extent0)
{
fS0 = extent0;
fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
}
else if (fTmpS0 >= -extent0)
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
}
else
{
fS0 = -extent0;
fTmpS1 = -(fA01*fS0+fB1);
if (fTmpS1 < -extent1)
{
fS1 = -extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
else if (fTmpS1 <= extent1)
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
}
else
{
fS1 = extent1;
fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
}
}
}
}
}
else
{
// The segments are parallel.
PxReal fE0pE1 = extent0 + extent1;
PxReal fSign = (fA01 > 0.0f ? -1.0f : 1.0f);
PxReal b0Avr = 0.5f*(fB0 - fSign*fB1);
PxReal fLambda = -b0Avr;
if(fLambda < -fE0pE1)
{
fLambda = -fE0pE1;
}
else if(fLambda > fE0pE1)
{
fLambda = fE0pE1;
}
fS1 = -fSign*fLambda*extent1/fE0pE1;
fS0 = fLambda + fSign*fS1;
fSqrDist = fLambda*(fLambda + 2.0f*b0Avr) + fC;
}
if(param0)
*param0 = fS0;
if(param1)
*param1 = fS1;
// account for numerical round-off error
return physx::intrinsics::selectMax(0.0f, fSqrDist);
}
PxReal Gu::distanceSegmentSegmentSquared( const PxVec3& origin0, const PxVec3& extent0,
const PxVec3& origin1, const PxVec3& extent1,
PxReal* param0,
PxReal* param1)
{
// Some conversion is needed between the old & new code
// Old:
// segment (s0, s1)
// origin = s0
// extent = s1 - s0
//
// New:
// s0 = origin + extent * dir;
// s1 = origin - extent * dir;
// dsequeira: is this really sensible? We use a highly optimized Wild Magic routine,
// then use a segment representation that requires an expensive conversion to/from...
PxVec3 dir0 = extent0;
const PxVec3 center0 = origin0 + extent0*0.5f;
PxReal length0 = extent0.magnitude(); //AM: change to make it work for degenerate (zero length) segments.
const bool b0 = length0 != 0.0f;
PxReal oneOverLength0 = 0.0f;
if(b0)
{
oneOverLength0 = 1.0f / length0;
dir0 *= oneOverLength0;
length0 *= 0.5f;
}
PxVec3 dir1 = extent1;
const PxVec3 center1 = origin1 + extent1*0.5f;
PxReal length1 = extent1.magnitude();
const bool b1 = length1 != 0.0f;
PxReal oneOverLength1 = 0.0f;
if(b1)
{
oneOverLength1 = 1.0f / length1;
dir1 *= oneOverLength1;
length1 *= 0.5f;
}
// the return param vals have -extent = s0, extent = s1
const PxReal d2 = distanceSegmentSegmentSquared(center0, dir0, length0,
center1, dir1, length1,
param0, param1);
//ML : This is wrong for some reason, I guess it has precision issue
//// renormalize into the 0 = s0, 1 = s1 range
//if (param0)
// *param0 = b0 ? ((*param0) * oneOverLength0 * 0.5f + 0.5f) : 0.0f;
//if (param1)
// *param1 = b1 ? ((*param1) * oneOverLength1 * 0.5f + 0.5f) : 0.0f;
if(param0)
*param0 = b0 ? ((length0 + (*param0))*oneOverLength0) : 0.0f;
if(param1)
*param1 = b1 ? ((length1 + (*param1))*oneOverLength1) : 0.0f;
return d2;
}
/*
S0 = origin + extent * dir;
S1 = origin + extent * dir;
dir is the vector from start to end point
p1 is the start point of segment1
d1 is the direction vector(q1 - p1)
p2 is the start point of segment2
d2 is the direction vector(q2 - p2)
*/
FloatV Gu::distanceSegmentSegmentSquared( const Vec3VArg p1,
const Vec3VArg d1,
const Vec3VArg p2,
const Vec3VArg d2,
FloatV& s,
FloatV& t)
{
const FloatV zero = FZero();
const FloatV one = FOne();
const FloatV eps = FEps();
const Vec3V r = V3Sub(p1, p2);
const Vec4V combinedDot = V3Dot4(d1, d1, d2, d2, d1, d2, d1, r);
const Vec4V combinedRecip = V4Sel(V4IsGrtr(combinedDot, V4Splat(eps)), V4Recip(combinedDot), V4Splat(zero));
const FloatV a = V4GetX(combinedDot);
const FloatV e = V4GetY(combinedDot);
const FloatV b = V4GetZ(combinedDot);
const FloatV c = V4GetW(combinedDot);
const FloatV aRecip = V4GetX(combinedRecip);//FSel(FIsGrtr(a, eps), FRecip(a), zero);
const FloatV eRecip = V4GetY(combinedRecip);//FSel(FIsGrtr(e, eps), FRecip(e), zero);
const FloatV f = V3Dot(d2, r);
/*
s = (b*f - c*e)/(a*e - b*b);
t = (a*f - b*c)/(a*e - b*b);
s = (b*t - c)/a;
t = (b*s + f)/e;
*/
//if segments not parallel, the general non-degenerated case, compute closest point on two segments and clamp to segment1
const FloatV denom = FSub(FMul(a, e), FMul(b, b));
const FloatV temp = FSub(FMul(b, f), FMul(c, e));
const FloatV s0 = FClamp(FDiv(temp, denom), zero, one);
//if segment is parallel, demon < eps
const BoolV con2 = FIsGrtr(eps, denom);//FIsEq(denom, zero);
const FloatV sTmp = FSel(con2, FHalf(), s0);
//compute point on segment2 closest to segment1
//const FloatV tTmp = FMul(FAdd(FMul(b, sTmp), f), eRecip);
const FloatV tTmp = FMul(FScaleAdd(b, sTmp, f), eRecip);
//if t is in [zero, one], done. otherwise clamp t
const FloatV t2 = FClamp(tTmp, zero, one);
//recompute s for the new value
const FloatV comp = FMul(FSub(FMul(b,t2), c), aRecip);
const FloatV s2 = FClamp(comp, zero, one);
s = s2;
t = t2;
const Vec3V closest1 = V3ScaleAdd(d1, s2, p1);//V3Add(p1, V3Scale(d1, tempS));
const Vec3V closest2 = V3ScaleAdd(d2, t2, p2);//V3Add(p2, V3Scale(d2, tempT));
const Vec3V vv = V3Sub(closest1, closest2);
return V3Dot(vv, vv);
}
/*
segment (p, d) and segment (p02, d02)
segment (p, d) and segment (p12, d12)
segment (p, d) and segment (p22, d22)
segment (p, d) and segment (p32, d32)
*/
Vec4V Gu::distanceSegmentSegmentSquared4( const Vec3VArg p, const Vec3VArg d0,
const Vec3VArg p02, const Vec3VArg d02,
const Vec3VArg p12, const Vec3VArg d12,
const Vec3VArg p22, const Vec3VArg d22,
const Vec3VArg p32, const Vec3VArg d32,
Vec4V& s, Vec4V& t)
{
const Vec4V zero = V4Zero();
const Vec4V one = V4One();
const Vec4V eps = V4Eps();
const Vec4V half = V4Splat(FHalf());
const Vec4V d0X = V4Splat(V3GetX(d0));
const Vec4V d0Y = V4Splat(V3GetY(d0));
const Vec4V d0Z = V4Splat(V3GetZ(d0));
const Vec4V pX = V4Splat(V3GetX(p));
const Vec4V pY = V4Splat(V3GetY(p));
const Vec4V pZ = V4Splat(V3GetZ(p));
Vec4V d024 = Vec4V_From_Vec3V(d02);
Vec4V d124 = Vec4V_From_Vec3V(d12);
Vec4V d224 = Vec4V_From_Vec3V(d22);
Vec4V d324 = Vec4V_From_Vec3V(d32);
Vec4V p024 = Vec4V_From_Vec3V(p02);
Vec4V p124 = Vec4V_From_Vec3V(p12);
Vec4V p224 = Vec4V_From_Vec3V(p22);
Vec4V p324 = Vec4V_From_Vec3V(p32);
Vec4V d0123X, d0123Y, d0123Z;
Vec4V p0123X, p0123Y, p0123Z;
PX_TRANSPOSE_44_34(d024, d124, d224, d324, d0123X, d0123Y, d0123Z);
PX_TRANSPOSE_44_34(p024, p124, p224, p324, p0123X, p0123Y, p0123Z);
const Vec4V rX = V4Sub(pX, p0123X);
const Vec4V rY = V4Sub(pY, p0123Y);
const Vec4V rZ = V4Sub(pZ, p0123Z);
//TODO - store this in a transposed state and avoid so many dot products?
const FloatV dd = V3Dot(d0, d0);
const Vec4V e = V4MulAdd(d0123Z, d0123Z, V4MulAdd(d0123X, d0123X, V4Mul(d0123Y, d0123Y)));
const Vec4V b = V4MulAdd(d0Z, d0123Z, V4MulAdd(d0X, d0123X, V4Mul(d0Y, d0123Y)));
const Vec4V c = V4MulAdd(d0Z, rZ, V4MulAdd(d0X, rX, V4Mul(d0Y, rY)));
const Vec4V f = V4MulAdd(d0123Z, rZ, V4MulAdd(d0123X, rX, V4Mul(d0123Y, rY)));
const Vec4V a(V4Splat(dd));
const Vec4V aRecip(V4Recip(a));
const Vec4V eRecip(V4Recip(e));
//if segments not parallell, compute closest point on two segments and clamp to segment1
const Vec4V denom = V4Sub(V4Mul(a, e), V4Mul(b, b));
const Vec4V temp = V4Sub(V4Mul(b, f), V4Mul(c, e));
const Vec4V s0 = V4Clamp(V4Div(temp, denom), zero, one);
//test whether segments are parallel
const BoolV con2 = V4IsGrtrOrEq(eps, denom);
const Vec4V sTmp = V4Sel(con2, half, s0);
//compute point on segment2 closest to segment1
const Vec4V tTmp = V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip);
//if t is in [zero, one], done. otherwise clamp t
const Vec4V t2 = V4Clamp(tTmp, zero, one);
//recompute s for the new value
const Vec4V comp = V4Mul(V4Sub(V4Mul(b,t2), c), aRecip);
const BoolV aaNearZero = V4IsGrtrOrEq(eps, a); // check if aRecip is valid (aa>eps)
const Vec4V s2 = V4Sel(aaNearZero, V4Zero(), V4Clamp(comp, zero, one));
/* s = V4Sel(con0, zero, V4Sel(con1, cd, s2));
t = V4Sel(con1, zero, V4Sel(con0, cg, t2)); */
s = s2;
t = t2;
const Vec4V closest1X = V4MulAdd(d0X, s2, pX);
const Vec4V closest1Y = V4MulAdd(d0Y, s2, pY);
const Vec4V closest1Z = V4MulAdd(d0Z, s2, pZ);
const Vec4V closest2X = V4MulAdd(d0123X, t2, p0123X);
const Vec4V closest2Y = V4MulAdd(d0123Y, t2, p0123Y);
const Vec4V closest2Z = V4MulAdd(d0123Z, t2, p0123Z);
const Vec4V vvX = V4Sub(closest1X, closest2X);
const Vec4V vvY = V4Sub(closest1Y, closest2Y);
const Vec4V vvZ = V4Sub(closest1Z, closest2Z);
const Vec4V vd = V4MulAdd(vvX, vvX, V4MulAdd(vvY, vvY, V4Mul(vvZ, vvZ)));
return vd;
}

521
engine/third_party/physx/source/geomutils/src/distance/GuDistanceSegmentTriangle.cpp vendored Normal file
View File

@@ -0,0 +1,521 @@
// 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/PxIntrinsics.h"
#include "GuDistanceSegmentTriangle.h"
#include "GuDistancePointTriangle.h"
#include "GuDistanceSegmentSegment.h"
#include "GuBarycentricCoordinates.h"
using namespace physx;
using namespace Gu;
// ptchernev:
// The Magic Software code uses a relative error test for parallel case.
// The Novodex code does not presumably as an optimization.
// Since the Novodex code is working in the trunk I see no reason
// to reintroduce the relative error test here.
// PT: this might just be because the relative error test has been added
// after we grabbed the code. I don't remember making this change. A good
// idea would be NOT to refactor Magic's code, to easily grab updated
// versions from the website.............................................
// ptchernev:
// The code has been modified to use a relative error test since the absolute
// test would break down for small geometries. (TTP 4021)
static PX_FORCE_INLINE void updateClosestHit( PxReal fSqrDist0, PxReal fR0, PxReal fS0, PxReal fT0,
PxReal& fSqrDist, PxReal& fR, PxReal& fS, PxReal& fT)
{
if(fSqrDist0 < fSqrDist)
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
PxReal Gu::distanceSegmentTriangleSquared( const PxVec3& origin, const PxVec3& dir,
const PxVec3& p0, const PxVec3& triEdge0, const PxVec3& triEdge1,
PxReal* t, PxReal* u, PxReal* v)
{
const PxReal fA00 = dir.magnitudeSquared();
if(fA00 < 1e-6f*1e-6f)
{
if(t)
*t = 0.0f;
return distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, u, v);
}
const PxVec3 kDiff = p0 - origin;
const PxReal fA01 = -(dir.dot(triEdge0));
const PxReal fA02 = -(dir.dot(triEdge1));
const PxReal fA11 = triEdge0.magnitudeSquared();
const PxReal fA12 = triEdge0.dot(triEdge1);
const PxReal fA22 = triEdge1.dot(triEdge1);
const PxReal fB0 = -(kDiff.dot(dir));
const PxReal fB1 = kDiff.dot(triEdge0);
const PxReal fB2 = kDiff.dot(triEdge1);
const PxReal fCof00 = fA11*fA22-fA12*fA12;
const PxReal fCof01 = fA02*fA12-fA01*fA22;
const PxReal fCof02 = fA01*fA12-fA02*fA11;
const PxReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
PxReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
// Set up for a relative error test on the angle between ray direction
// and triangle normal to determine parallel/nonparallel status.
const PxVec3 kNormal = triEdge0.cross(triEdge1);
const PxReal fDot = kNormal.dot(dir);
if(fDot*fDot >= 1e-6f*dir.magnitudeSquared()*kNormal.magnitudeSquared())
{
const PxReal fCof11 = fA00*fA22-fA02*fA02;
const PxReal fCof12 = fA02*fA01-fA00*fA12;
const PxReal fCof22 = fA00*fA11-fA01*fA01;
const PxReal fInvDet = fDet == 0.0f ? 0.0f : 1.0f/fDet;
const PxReal fRhs0 = -fB0*fInvDet;
const PxReal fRhs1 = -fB1*fInvDet;
const PxReal fRhs2 = -fB2*fInvDet;
fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
if(fR < 0.0f)
{
if(fS+fT <= 1.0f)
{
if(fS < 0.0f)
{
if(fT < 0.0f) // region 4m
{
// minimum on face s=0 or t=0 or r=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR0, &fS0);
fT0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 3m
{
// minimum on face s=0 or r=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
}
fSqrDist0 = distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else if(fT < 0.0f) // region 5m
{
// minimum on face t=0 or r=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
fSqrDist0 = distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 0m
{
// minimum on face r=0
fSqrDist = distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, &fS, &fT);
fR = 0.0f;
}
}
else
{
if(fS < 0.0f) // region 2m
{
// minimum on face s=0 or s+t=1 or r=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else if(fT < 0.0f) // region 6m
{
// minimum on face t=0 or s+t=1 or r=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 1m
{
// minimum on face s+t=1 or r=0
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR, &fT);
fS = 1.0f-fT;
}
fSqrDist0 = distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
}
else if(fR <= 1.0f)
{
if(fS+fT <= 1.0f)
{
if(fS < 0.0f)
{
if(fT < 0.0f) // region 4
{
// minimum on face s=0 or t=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR0, &fS0);
fT0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 3
{
// minimum on face s=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
}
}
else if(fT < 0.0f) // region 5
{
// minimum on face t=0
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
}
else // region 0
{
// global minimum is interior, done
fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0)
+fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1)
+fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2)
+kDiff.magnitudeSquared();
}
}
else
{
if(fS < 0.0f) // region 2
{
// minimum on face s=0 or s+t=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else if(fT < 0.0f) // region 6
{
// minimum on face t=0 or s+t=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 1
{
// minimum on face s+t=1
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR, &fT);
fS = 1.0f-fT;
}
}
}
else // fR > 1
{
if(fS+fT <= 1.0f)
{
if(fS < 0.0f)
{
if(fT < 0.0f) // region 4p
{
// minimum on face s=0 or t=0 or r=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR0, &fS0);
fT0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 3p
{
// minimum on face s=0 or r=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
}
const PxVec3 kPt = origin+dir;
fSqrDist0 = distancePointTriangleSquared(kPt, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 1.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else if(fT < 0.0f) // region 5p
{
// minimum on face t=0 or r=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
const PxVec3 kPt = origin+dir;
fSqrDist0 = distancePointTriangleSquared(kPt, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 1.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 0p
{
// minimum face on r=1
const PxVec3 kPt = origin+dir;
fSqrDist = distancePointTriangleSquared(kPt, p0, triEdge0, triEdge1, &fS, &fT);
fR = 1.0f;
}
}
else
{
if(fS < 0.0f) // region 2p
{
// minimum on face s=0 or s+t=1 or r=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR, &fT);
fS = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else if(fT < 0.0f) // region 6p
{
// minimum on face t=0 or s+t=1 or r=1
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
else // region 1p
{
// minimum on face s+t=1 or r=1
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1-triEdge0;
fSqrDist = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR, &fT);
fS = 1.0f-fT;
}
const PxVec3 kPt = origin+dir;
fSqrDist0 = distancePointTriangleSquared(kPt, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 1.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
}
}
else
{
// segment and triangle are parallel
fSqrDist = distanceSegmentSegmentSquared(origin, dir, p0, triEdge0, &fR, &fS);
fT = 0.0f;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, p0, triEdge1, &fR0, &fT0);
fS0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
const PxVec3 kTriSegOrig = p0+triEdge0;
const PxVec3 kTriSegDir = triEdge1 - triEdge0;
fSqrDist0 = distanceSegmentSegmentSquared(origin, dir, kTriSegOrig, kTriSegDir, &fR0, &fT0);
fS0 = 1.0f-fT0;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
fSqrDist0 = distancePointTriangleSquared(origin, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 0.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
const PxVec3 kPt = origin+dir;
fSqrDist0 = distancePointTriangleSquared(kPt, p0, triEdge0, triEdge1, &fS0, &fT0);
fR0 = 1.0f;
updateClosestHit(fSqrDist0, fR0, fS0, fT0, fSqrDist, fR, fS, fT);
}
if(t) *t = fR;
if(u) *u = fS;
if(v) *v = fT;
// account for numerical round-off error
return physx::intrinsics::selectMax(0.0f, fSqrDist);
}
// closest0 is the closest point on segment pq
// closest1 is the closest point on triangle abc
aos::FloatV Gu::distanceSegmentTriangleSquared( const aos::Vec3VArg p, const aos::Vec3VArg q,
const aos::Vec3VArg a, const aos::Vec3VArg b, const aos::Vec3VArg c,
aos::Vec3V& closest0, aos::Vec3V& closest1)
{
using namespace aos;
const FloatV zero = FZero();
//const FloatV one = FOne();
//const FloatV parallelTolerance = FloatV_From_F32(PX_PARALLEL_TOLERANCE);
const Vec3V pq = V3Sub(q, p);
const Vec3V ab = V3Sub(b, a);
const Vec3V ac = V3Sub(c, a);
const Vec3V bc = V3Sub(c, b);
const Vec3V ap = V3Sub(p, a);
const Vec3V aq = V3Sub(q, a);
//This is used to calculate the barycentric coordinate
const FloatV d00 = V3Dot(ab,ab);
const FloatV d01 = V3Dot(ab, ac);
const FloatV d11 = V3Dot(ac, ac);
const FloatV tDenom = FSub(FMul(d00, d11), FMul(d01, d01));
const FloatV bdenom = FSel(FIsGrtr(tDenom, zero), FRecip(tDenom), zero);
const Vec3V n = V3Normalize(V3Cross(ab, ac)); // normalize vector
//compute the closest point of p and triangle plane abc
const FloatV dist3 = V3Dot(ap, n);
const FloatV sqDist3 = FMul(dist3, dist3);
//compute the closest point of q and triangle plane abc
const FloatV dist4 = V3Dot(aq, n);
const FloatV sqDist4 = FMul(dist4, dist4);
const FloatV dMul = FMul(dist3, dist4);
const BoolV con = FIsGrtr(zero, dMul);
// intersect with the plane
if(BAllEqTTTT(con))
{
//compute the intersect point
const FloatV nom = FNeg(V3Dot(n, ap));
const FloatV denom = FRecip(V3Dot(n, pq));
const FloatV t = FMul(nom, denom);
const Vec3V ip = V3ScaleAdd(pq, t, p);//V3Add(p, V3Scale(pq, t));
const Vec3V v2 = V3Sub(ip, a);
const FloatV d20 = V3Dot(v2, ab);
const FloatV d21 = V3Dot(v2, ac);
const FloatV v0 = FMul(FSub(FMul(d11, d20), FMul(d01, d21)), bdenom);
const FloatV w0 = FMul(FSub(FMul(d00, d21), FMul(d01, d20)), bdenom);
const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
if(BAllEqTTTT(con0))
{
closest0 = closest1 = ip;
return zero;
}
}
Vec4V t40, t41;
const Vec4V sqDist44 = distanceSegmentSegmentSquared4(p,pq,a,ab, b,bc, a,ac, a,ab, t40, t41);
const FloatV t00 = V4GetX(t40);
const FloatV t10 = V4GetY(t40);
const FloatV t20 = V4GetZ(t40);
const FloatV t01 = V4GetX(t41);
const FloatV t11 = V4GetY(t41);
const FloatV t21 = V4GetZ(t41);
const FloatV sqDist0(V4GetX(sqDist44));
const FloatV sqDist1(V4GetY(sqDist44));
const FloatV sqDist2(V4GetZ(sqDist44));
const Vec3V closestP00 = V3ScaleAdd(pq, t00, p);
const Vec3V closestP01 = V3ScaleAdd(ab, t01, a);
const Vec3V closestP10 = V3ScaleAdd(pq, t10, p);
const Vec3V closestP11 = V3ScaleAdd(bc, t11, b);
const Vec3V closestP20 = V3ScaleAdd(pq, t20, p);
const Vec3V closestP21 = V3ScaleAdd(ac, t21, a);
//Get the closest point of all edges
const BoolV con20 = FIsGrtr(sqDist1, sqDist0);
const BoolV con21 = FIsGrtr(sqDist2, sqDist0);
const BoolV con2 = BAnd(con20,con21);
const BoolV con30 = FIsGrtrOrEq(sqDist0, sqDist1);
const BoolV con31 = FIsGrtr(sqDist2, sqDist1);
const BoolV con3 = BAnd(con30, con31);
const FloatV sqDistPE = FSel(con2, sqDist0, FSel(con3, sqDist1, sqDist2));
//const FloatV tValue = FSel(con2, t00, FSel(con3, t10, t20));
const Vec3V closestPE0 = V3Sel(con2, closestP00, V3Sel(con3, closestP10, closestP20)); // closestP on segment
const Vec3V closestPE1 = V3Sel(con2, closestP01, V3Sel(con3, closestP11, closestP21)); // closestP on triangle
const Vec3V closestP31 = V3NegScaleSub(n, dist3, p);//V3Sub(p, V3Scale(n, dist3));
const Vec3V closestP30 = p;
//Compute the barycentric coordinate for project point of q
const Vec3V pV20 = V3Sub(closestP31, a);
const FloatV pD20 = V3Dot(pV20, ab);
const FloatV pD21 = V3Dot(pV20, ac);
const FloatV v0 = FMul(FSub(FMul(d11, pD20), FMul(d01, pD21)), bdenom);
const FloatV w0 = FMul(FSub(FMul(d00, pD21), FMul(d01, pD20)), bdenom);
//check closestP3 is inside the triangle
const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
const Vec3V closestP41 = V3NegScaleSub(n, dist4, q);// V3Sub(q, V3Scale(n, dist4));
const Vec3V closestP40 = q;
//Compute the barycentric coordinate for project point of q
const Vec3V qV20 = V3Sub(closestP41, a);
const FloatV qD20 = V3Dot(qV20, ab);
const FloatV qD21 = V3Dot(qV20, ac);
const FloatV v1 = FMul(FSub(FMul(d11, qD20), FMul(d01, qD21)), bdenom);
const FloatV w1 = FMul(FSub(FMul(d00, qD21), FMul(d01, qD20)), bdenom);
const BoolV con1 = isValidTriangleBarycentricCoord(v1, w1);
// p is interior point but not q
const BoolV d0 = FIsGrtr(sqDistPE, sqDist3);
const Vec3V c00 = V3Sel(d0, closestP30, closestPE0);
const Vec3V c01 = V3Sel(d0, closestP31, closestPE1);
// q is interior point but not p
const BoolV d1 = FIsGrtr(sqDistPE, sqDist4);
const Vec3V c10 = V3Sel(d1, closestP40, closestPE0);
const Vec3V c11 = V3Sel(d1, closestP41, closestPE1);
// p and q are interior point
const BoolV d2 = FIsGrtr(sqDist4, sqDist3);
const Vec3V c20 = V3Sel(d2, closestP30, closestP40);
const Vec3V c21 = V3Sel(d2, closestP31, closestP41);
const BoolV cond2 = BAnd(con0, con1);
const Vec3V closestP0 = V3Sel(cond2, c20, V3Sel(con0, c00, V3Sel(con1, c10, closestPE0)));
const Vec3V closestP1 = V3Sel(cond2, c21, V3Sel(con0, c01, V3Sel(con1, c11, closestPE1)));
const Vec3V vv = V3Sub(closestP1, closestP0);
closest0 = closestP0;
closest1 = closestP1;
return V3Dot(vv, vv);
}

64
engine/third_party/physx/source/geomutils/src/distance/GuDistanceSegmentTriangle.h vendored Normal file
View File

@@ -0,0 +1,64 @@
// 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_DISTANCE_SEGMENT_TRIANGLE_H
#define GU_DISTANCE_SEGMENT_TRIANGLE_H
#include "common/PxPhysXCommonConfig.h"
#include "GuSegment.h"
#include "foundation/PxVecMath.h"
namespace physx
{
namespace Gu
{
PX_PHYSX_COMMON_API PxReal distanceSegmentTriangleSquared(
const PxVec3& segmentOrigin, const PxVec3& segmentExtent,
const PxVec3& triangleOrigin, const PxVec3& triangleEdge0, const PxVec3& triangleEdge1,
PxReal* t=NULL, PxReal* u=NULL, PxReal* v=NULL);
PX_INLINE PxReal distanceSegmentTriangleSquared(
const Gu::Segment& segment,
const PxVec3& triangleOrigin, const PxVec3& triangleEdge0, const PxVec3& triangleEdge1,
PxReal* t=NULL, PxReal* u=NULL, PxReal* v=NULL)
{
return distanceSegmentTriangleSquared(
segment.p0, segment.computeDirection(), triangleOrigin, triangleEdge0, triangleEdge1, t, u, v);
}
// closest0 is the closest point on segment pq
// closest1 is the closest point on triangle abc
PX_PHYSX_COMMON_API aos::FloatV distanceSegmentTriangleSquared(
const aos::Vec3VArg p, const aos::Vec3VArg q,
const aos::Vec3VArg a, const aos::Vec3VArg b, const aos::Vec3VArg c,
aos::Vec3V& closest0, aos::Vec3V& closest1);
} // namespace Gu
}
#endif

195
engine/third_party/physx/source/geomutils/src/distance/GuDistanceTriangleTriangle.cpp vendored Normal file
View File

@@ -0,0 +1,195 @@
// 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 "GuDistanceTriangleTriangle.h"
#include "foundation/PxVecMath.h"
using namespace physx;
using namespace Gu;
using namespace aos;
void edgeEdgeDist(PxVec3& x, PxVec3& y, const PxVec3& p, const PxVec3& a, const PxVec3& q, const PxVec3& b);
float Gu::distanceTriangleTriangleSquared(PxVec3& cp, PxVec3& cq, const PxVec3p p[3], const PxVec3p q[3])
{
PxVec3p Sv[3];
V4StoreU(V4Sub(V4LoadU(&p[1].x), V4LoadU(&p[0].x)), &Sv[0].x);
V4StoreU(V4Sub(V4LoadU(&p[2].x), V4LoadU(&p[1].x)), &Sv[1].x);
V4StoreU(V4Sub(V4LoadU(&p[0].x), V4LoadU(&p[2].x)), &Sv[2].x);
PxVec3p Tv[3];
V4StoreU(V4Sub(V4LoadU(&q[1].x), V4LoadU(&q[0].x)), &Tv[0].x);
V4StoreU(V4Sub(V4LoadU(&q[2].x), V4LoadU(&q[1].x)), &Tv[1].x);
V4StoreU(V4Sub(V4LoadU(&q[0].x), V4LoadU(&q[2].x)), &Tv[2].x);
PxVec3 minP, minQ;
bool shown_disjoint = false;
float mindd = PX_MAX_F32;
for(PxU32 i=0;i<3;i++)
{
for(PxU32 j=0;j<3;j++)
{
edgeEdgeDist(cp, cq, p[i], Sv[i], q[j], Tv[j]);
const PxVec3 V = cq - cp;
const float dd = V.dot(V);
if(dd<=mindd)
{
minP = cp;
minQ = cq;
mindd = dd;
PxU32 id = i+2;
if(id>=3)
id-=3;
PxVec3 Z = p[id] - cp;
float a = Z.dot(V);
id = j+2;
if(id>=3)
id-=3;
Z = q[id] - cq;
float b = Z.dot(V);
if((a<=0.0f) && (b>=0.0f))
return V.dot(V);
if(a<=0.0f) a = 0.0f;
else if(b>0.0f) b = 0.0f;
if((mindd - a + b) > 0.0f)
shown_disjoint = true;
}
}
}
PxVec3 Sn = Sv[0].cross(Sv[1]);
float Snl = Sn.dot(Sn);
if(Snl>1e-15f)
{
const PxVec3 Tp((p[0] - q[0]).dot(Sn),
(p[0] - q[1]).dot(Sn),
(p[0] - q[2]).dot(Sn));
int index = -1;
if((Tp[0]>0.0f) && (Tp[1]>0.0f) && (Tp[2]>0.0f))
{
if(Tp[0]<Tp[1]) index = 0; else index = 1;
if(Tp[2]<Tp[index]) index = 2;
}
else if((Tp[0]<0.0f) && (Tp[1]<0.0f) && (Tp[2]<0.0f))
{
if(Tp[0]>Tp[1]) index = 0; else index = 1;
if(Tp[2]>Tp[index]) index = 2;
}
if(index >= 0)
{
shown_disjoint = true;
const PxVec3& qIndex = q[index];
PxVec3 V = qIndex - p[0];
PxVec3 Z = Sn.cross(Sv[0]);
if(V.dot(Z)>0.0f)
{
V = qIndex - p[1];
Z = Sn.cross(Sv[1]);
if(V.dot(Z)>0.0f)
{
V = qIndex - p[2];
Z = Sn.cross(Sv[2]);
if(V.dot(Z)>0.0f)
{
cp = qIndex + Sn * Tp[index]/Snl;
cq = qIndex;
return (cp - cq).magnitudeSquared();
}
}
}
}
}
PxVec3 Tn = Tv[0].cross(Tv[1]);
float Tnl = Tn.dot(Tn);
if(Tnl>1e-15f)
{
const PxVec3 Sp((q[0] - p[0]).dot(Tn),
(q[0] - p[1]).dot(Tn),
(q[0] - p[2]).dot(Tn));
int index = -1;
if((Sp[0]>0.0f) && (Sp[1]>0.0f) && (Sp[2]>0.0f))
{
if(Sp[0]<Sp[1]) index = 0; else index = 1;
if(Sp[2]<Sp[index]) index = 2;
}
else if((Sp[0]<0.0f) && (Sp[1]<0.0f) && (Sp[2]<0.0f))
{
if(Sp[0]>Sp[1]) index = 0; else index = 1;
if(Sp[2]>Sp[index]) index = 2;
}
if(index >= 0)
{
shown_disjoint = true;
const PxVec3& pIndex = p[index];
PxVec3 V = pIndex - q[0];
PxVec3 Z = Tn.cross(Tv[0]);
if(V.dot(Z)>0.0f)
{
V = pIndex - q[1];
Z = Tn.cross(Tv[1]);
if(V.dot(Z)>0.0f)
{
V = pIndex - q[2];
Z = Tn.cross(Tv[2]);
if(V.dot(Z)>0.0f)
{
cp = pIndex;
cq = pIndex + Tn * Sp[index]/Tnl;
return (cp - cq).magnitudeSquared();
}
}
}
}
}
if(shown_disjoint)
{
cp = minP;
cq = minQ;
return mindd;
}
else return 0.0f;
}

44
engine/third_party/physx/source/geomutils/src/distance/GuDistanceTriangleTriangle.h vendored Normal file
View File

@@ -0,0 +1,44 @@
// 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_DISTANCE_TRIANGLE_TRIANGLE_H
#define GU_DISTANCE_TRIANGLE_TRIANGLE_H
#include "common/PxPhysXCommonConfig.h"
#include "foundation/PxVec3.h"
namespace physx
{
namespace Gu
{
float distanceTriangleTriangleSquared(PxVec3& cp, PxVec3& cq, const PxVec3p p[3], const PxVec3p q[3]);
} // namespace Gu
}
#endif