aboutsummaryrefslogtreecommitdiff
path: root/NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp')
-rw-r--r--NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp1004
1 files changed, 1004 insertions, 0 deletions
diff --git a/NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp b/NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp
new file mode 100644
index 0000000..73c59b8
--- /dev/null
+++ b/NvBlast/sdk/extensions/authoring/source/NvBlastExtApexSharedParts.cpp
@@ -0,0 +1,1004 @@
+/*
+* Copyright (c) 2016-2017, NVIDIA CORPORATION. All rights reserved.
+*
+* NVIDIA CORPORATION and its licensors retain all intellectual property
+* and proprietary rights in and to this software, related documentation
+* and any modifications thereto. Any use, reproduction, disclosure or
+* distribution of this software and related documentation without an express
+* license agreement from NVIDIA CORPORATION is strictly prohibited.
+*/
+
+#include "NvBlastExtApexSharedParts.h"
+
+#include "PxMat44.h"
+#include "PxBounds3.h"
+#include "PxFoundation.h"
+#include "PxPhysics.h"
+#include "PsVecMath.h"
+#include <vector>
+
+using namespace physx;
+using namespace physx::shdfnd::aos;
+
+
+namespace Nv
+{
+namespace Blast
+{
+
+PX_NOALIAS PX_FORCE_INLINE BoolV PointOutsideOfPlane4(const Vec3VArg _a, const Vec3VArg _b, const Vec3VArg _c, const Vec3VArg _d)
+{
+ // this is not 0 because of the following scenario:
+ // All the points lie on the same plane and the plane goes through the origin (0,0,0).
+ // On the Wii U, the math below has the problem that when point A gets projected on the
+ // plane cumputed by A, B, C, the distance to the plane might not be 0 for the mentioned
+ // scenario but a small positive or negative value. This can lead to the wrong boolean
+ // results. Using a small negative value as threshold is more conservative but safer.
+ const Vec4V zero = V4Load(-1e-6);
+
+ const Vec3V ab = V3Sub(_b, _a);
+ const Vec3V ac = V3Sub(_c, _a);
+ const Vec3V ad = V3Sub(_d, _a);
+ const Vec3V bd = V3Sub(_d, _b);
+ const Vec3V bc = V3Sub(_c, _b);
+
+ const Vec3V v0 = V3Cross(ab, ac);
+ const Vec3V v1 = V3Cross(ac, ad);
+ const Vec3V v2 = V3Cross(ad, ab);
+ const Vec3V v3 = V3Cross(bd, bc);
+
+ const FloatV signa0 = V3Dot(v0, _a);
+ const FloatV signa1 = V3Dot(v1, _a);
+ const FloatV signa2 = V3Dot(v2, _a);
+ const FloatV signd3 = V3Dot(v3, _a);
+
+ const FloatV signd0 = V3Dot(v0, _d);
+ const FloatV signd1 = V3Dot(v1, _b);
+ const FloatV signd2 = V3Dot(v2, _c);
+ const FloatV signa3 = V3Dot(v3, _b);
+
+ const Vec4V signa = V4Merge(signa0, signa1, signa2, signa3);
+ const Vec4V signd = V4Merge(signd0, signd1, signd2, signd3);
+ return V4IsGrtrOrEq(V4Mul(signa, signd), zero);//same side, outside of the plane
+}
+
+PX_NOALIAS PX_FORCE_INLINE Vec3V closestPtPointSegment(const Vec3VArg a, const Vec3VArg b)
+{
+ const FloatV zero = FZero();
+ const FloatV one = FOne();
+
+ //Test degenerated case
+ const Vec3V ab = V3Sub(b, a);
+ const FloatV denom = V3Dot(ab, ab);
+ const Vec3V ap = V3Neg(a);//V3Sub(origin, a);
+ const FloatV nom = V3Dot(ap, ab);
+ const BoolV con = FIsEq(denom, zero);
+ const FloatV tValue = FClamp(FDiv(nom, denom), zero, one);
+ const FloatV t = FSel(con, zero, tValue);
+
+ return V3Sel(con, a, V3ScaleAdd(ab, t, a));
+}
+
+PX_NOALIAS PX_FORCE_INLINE Vec3V closestPtPointSegment(const Vec3VArg Q0, const Vec3VArg Q1, const Vec3VArg A0, const Vec3VArg A1,
+ const Vec3VArg B0, const Vec3VArg B1, PxU32& size, Vec3V& closestA, Vec3V& closestB)
+{
+ const Vec3V a = Q0;
+ const Vec3V b = Q1;
+
+ const BoolV bTrue = BTTTT();
+ const FloatV zero = FZero();
+ const FloatV one = FOne();
+
+ //Test degenerated case
+ const Vec3V ab = V3Sub(b, a);
+ const FloatV denom = V3Dot(ab, ab);
+ const Vec3V ap = V3Neg(a);//V3Sub(origin, a);
+ const FloatV nom = V3Dot(ap, ab);
+ const BoolV con = FIsEq(denom, zero);
+
+ if (BAllEq(con, bTrue))
+ {
+ size = 1;
+ closestA = A0;
+ closestB = B0;
+ return Q0;
+ }
+
+ const Vec3V v = V3Sub(A1, A0);
+ const Vec3V w = V3Sub(B1, B0);
+ const FloatV tValue = FClamp(FDiv(nom, denom), zero, one);
+ const FloatV t = FSel(con, zero, tValue);
+
+ const Vec3V tempClosestA = V3ScaleAdd(v, t, A0);
+ const Vec3V tempClosestB = V3ScaleAdd(w, t, B0);
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ return V3Sub(tempClosestA, tempClosestB);
+}
+
+PX_NOALIAS Vec3V closestPtPointSegmentTesselation(const Vec3VArg Q0, const Vec3VArg Q1, const Vec3VArg A0, const Vec3VArg A1,
+ const Vec3VArg B0, const Vec3VArg B1, PxU32& size, Vec3V& closestA, Vec3V& closestB)
+{
+ const FloatV half = FHalf();
+
+ const FloatV targetSegmentLengthSq = FLoad(10000.f);//100 unit
+
+ Vec3V q0 = Q0;
+ Vec3V q1 = Q1;
+ Vec3V a0 = A0;
+ Vec3V a1 = A1;
+ Vec3V b0 = B0;
+ Vec3V b1 = B1;
+
+ for (;;)
+ {
+ const Vec3V midPoint = V3Scale(V3Add(q0, q1), half);
+ const Vec3V midA = V3Scale(V3Add(a0, a1), half);
+ const Vec3V midB = V3Scale(V3Add(b0, b1), half);
+
+ const Vec3V v = V3Sub(midPoint, q0);
+ const FloatV sqV = V3Dot(v, v);
+ if (FAllGrtr(targetSegmentLengthSq, sqV))
+ break;
+ //split the segment into half
+ const Vec3V tClos0 = closestPtPointSegment(q0, midPoint);
+ const FloatV sqDist0 = V3Dot(tClos0, tClos0);
+
+ const Vec3V tClos1 = closestPtPointSegment(q1, midPoint);
+ const FloatV sqDist1 = V3Dot(tClos1, tClos1);
+ //const BoolV con = FIsGrtr(sqDist0, sqDist1);
+ if (FAllGrtr(sqDist0, sqDist1))
+ {
+ //segment [m, q1]
+ q0 = midPoint;
+ a0 = midA;
+ b0 = midB;
+ }
+ else
+ {
+ //segment [q0, m]
+ q1 = midPoint;
+ a1 = midA;
+ b1 = midB;
+ }
+
+ }
+
+ return closestPtPointSegment(q0, q1, a0, a1, b0, b1, size, closestA, closestB);
+}
+
+PX_NOALIAS Vec3V closestPtPointTriangleTesselation(const Vec3V* PX_RESTRICT Q, const Vec3V* PX_RESTRICT A, const Vec3V* PX_RESTRICT B, const PxU32* PX_RESTRICT indices, PxU32& size, Vec3V& closestA, Vec3V& closestB)
+{
+ size = 3;
+ const FloatV zero = FZero();
+ const FloatV eps = FEps();
+ const FloatV half = FHalf();
+ const BoolV bTrue = BTTTT();
+ const FloatV four = FLoad(4.f);
+ const FloatV sixty = FLoad(100.f);
+
+ const PxU32 ind0 = indices[0];
+ const PxU32 ind1 = indices[1];
+ const PxU32 ind2 = indices[2];
+
+ const Vec3V a = Q[ind0];
+ const Vec3V b = Q[ind1];
+ const Vec3V c = Q[ind2];
+
+ Vec3V ab_ = V3Sub(b, a);
+ Vec3V ac_ = V3Sub(c, a);
+ Vec3V bc_ = V3Sub(b, c);
+
+ const FloatV dac_ = V3Dot(ac_, ac_);
+ const FloatV dbc_ = V3Dot(bc_, bc_);
+ if (FAllGrtrOrEq(eps, FMin(dac_, dbc_)))
+ {
+ //degenerate
+ size = 2;
+ return closestPtPointSegment(Q[ind0], Q[ind1], A[ind0], A[ind1], B[ind0], B[ind1], size, closestA, closestB);
+ }
+
+ Vec3V ap = V3Neg(a);
+ Vec3V bp = V3Neg(b);
+ Vec3V cp = V3Neg(c);
+
+ FloatV d1 = V3Dot(ab_, ap); // snom
+ FloatV d2 = V3Dot(ac_, ap); // tnom
+ FloatV d3 = V3Dot(ab_, bp); // -sdenom
+ FloatV d4 = V3Dot(ac_, bp); // unom = d4 - d3
+ FloatV d5 = V3Dot(ab_, cp); // udenom = d5 - d6
+ FloatV d6 = V3Dot(ac_, cp); // -tdenom
+ /* FloatV unom = FSub(d4, d3);
+ FloatV udenom = FSub(d5, d6);*/
+
+ FloatV va = FNegScaleSub(d5, d4, FMul(d3, d6));//edge region of BC
+ FloatV vb = FNegScaleSub(d1, d6, FMul(d5, d2));//edge region of AC
+ FloatV vc = FNegScaleSub(d3, d2, FMul(d1, d4));//edge region of AB
+
+ //check if p in vertex region outside a
+ const BoolV con00 = FIsGrtrOrEq(zero, d1); // snom <= 0
+ const BoolV con01 = FIsGrtrOrEq(zero, d2); // tnom <= 0
+ const BoolV con0 = BAnd(con00, con01); // vertex region a
+ if (BAllEq(con0, bTrue))
+ {
+ //size = 1;
+ closestA = A[ind0];
+ closestB = B[ind0];
+ return Q[ind0];
+ }
+
+ //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 (BAllEq(con1, bTrue))
+ {
+ /*size = 1;
+ indices[0] = ind1;*/
+ closestA = A[ind1];
+ closestB = B[ind1];
+ return Q[ind1];
+ }
+
+
+ //check if p in vertex region outside of c
+ const BoolV con20 = FIsGrtrOrEq(d6, zero);
+ const BoolV con21 = FIsGrtrOrEq(d6, d5);
+ const BoolV con2 = BAnd(con20, con21); // vertex region c
+ if (BAllEq(con2, bTrue))
+ {
+ closestA = A[ind2];
+ closestB = B[ind2];
+ return Q[ind2];
+ }
+
+ //check if p in edge region of AB
+ const BoolV con30 = FIsGrtrOrEq(zero, vc);
+ const BoolV con31 = FIsGrtrOrEq(d1, zero);
+ const BoolV con32 = FIsGrtrOrEq(zero, d3);
+ const BoolV con3 = BAnd(con30, BAnd(con31, con32));
+
+ if (BAllEq(con3, bTrue))
+ {
+ //size = 2;
+ //p in edge region of AB, split AB
+ return closestPtPointSegmentTesselation(Q[ind0], Q[ind1], A[ind0], A[ind1], B[ind0], B[ind1], size, closestA, closestB);
+ }
+
+ //check if p in edge region of BC
+ const BoolV con40 = FIsGrtrOrEq(zero, va);
+ const BoolV con41 = FIsGrtrOrEq(d4, d3);
+ const BoolV con42 = FIsGrtrOrEq(d5, d6);
+ const BoolV con4 = BAnd(con40, BAnd(con41, con42));
+
+ if (BAllEq(con4, bTrue))
+ {
+ //p in edge region of BC, split BC
+ return closestPtPointSegmentTesselation(Q[ind1], Q[ind2], A[ind1], A[ind2], B[ind1], B[ind2], size, closestA, closestB);
+ }
+
+ //check if p in edge region of AC
+ const BoolV con50 = FIsGrtrOrEq(zero, vb);
+ const BoolV con51 = FIsGrtrOrEq(d2, zero);
+ const BoolV con52 = FIsGrtrOrEq(zero, d6);
+ const BoolV con5 = BAnd(con50, BAnd(con51, con52));
+
+ if (BAllEq(con5, bTrue))
+ {
+ //p in edge region of AC, split AC
+ return closestPtPointSegmentTesselation(Q[ind0], Q[ind2], A[ind0], A[ind2], B[ind0], B[ind2], size, closestA, closestB);
+ }
+
+ size = 3;
+
+ Vec3V q0 = Q[ind0];
+ Vec3V q1 = Q[ind1];
+ Vec3V q2 = Q[ind2];
+ Vec3V a0 = A[ind0];
+ Vec3V a1 = A[ind1];
+ Vec3V a2 = A[ind2];
+ Vec3V b0 = B[ind0];
+ Vec3V b1 = B[ind1];
+ Vec3V b2 = B[ind2];
+
+ for (;;)
+ {
+
+ const Vec3V ab = V3Sub(q1, q0);
+ const Vec3V ac = V3Sub(q2, q0);
+ const Vec3V bc = V3Sub(q2, q1);
+
+ const FloatV dab = V3Dot(ab, ab);
+ const FloatV dac = V3Dot(ac, ac);
+ const FloatV dbc = V3Dot(bc, bc);
+
+ const FloatV fMax = FMax(dab, FMax(dac, dbc));
+ const FloatV fMin = FMin(dab, FMin(dac, dbc));
+
+ const Vec3V w = V3Cross(ab, ac);
+
+ const FloatV area = V3Length(w);
+ const FloatV ratio = FDiv(FSqrt(fMax), FSqrt(fMin));
+ if (FAllGrtr(four, ratio) && FAllGrtr(sixty, area))
+ break;
+
+ //calculate the triangle normal
+ const Vec3V triNormal = V3Normalize(w);
+
+ PX_ASSERT(V3AllEq(triNormal, V3Zero()) == 0);
+
+
+ //split the longest edge
+ if (FAllGrtrOrEq(dab, dac) && FAllGrtrOrEq(dab, dbc))
+ {
+ //split edge q0q1
+ const Vec3V midPoint = V3Scale(V3Add(q0, q1), half);
+ const Vec3V midA = V3Scale(V3Add(a0, a1), half);
+ const Vec3V midB = V3Scale(V3Add(b0, b1), half);
+
+ const Vec3V v = V3Sub(midPoint, q2);
+ const Vec3V n = V3Normalize(V3Cross(v, triNormal));
+
+ const FloatV d = FNeg(V3Dot(n, midPoint));
+ const FloatV dp = FAdd(V3Dot(n, q0), d);
+ const FloatV sum = FMul(d, dp);
+
+ if (FAllGrtr(sum, zero))
+ {
+ //q0 and origin at the same side, split triangle[q0, m, q2]
+ q1 = midPoint;
+ a1 = midA;
+ b1 = midB;
+ }
+ else
+ {
+ //q1 and origin at the same side, split triangle[m, q1, q2]
+ q0 = midPoint;
+ a0 = midA;
+ b0 = midB;
+ }
+
+ }
+ else if (FAllGrtrOrEq(dac, dbc))
+ {
+ //split edge q0q2
+ const Vec3V midPoint = V3Scale(V3Add(q0, q2), half);
+ const Vec3V midA = V3Scale(V3Add(a0, a2), half);
+ const Vec3V midB = V3Scale(V3Add(b0, b2), half);
+
+ const Vec3V v = V3Sub(midPoint, q1);
+ const Vec3V n = V3Normalize(V3Cross(v, triNormal));
+
+ const FloatV d = FNeg(V3Dot(n, midPoint));
+ const FloatV dp = FAdd(V3Dot(n, q0), d);
+ const FloatV sum = FMul(d, dp);
+
+ if (FAllGrtr(sum, zero))
+ {
+ //q0 and origin at the same side, split triangle[q0, q1, m]
+ q2 = midPoint;
+ a2 = midA;
+ b2 = midB;
+ }
+ else
+ {
+ //q2 and origin at the same side, split triangle[m, q1, q2]
+ q0 = midPoint;
+ a0 = midA;
+ b0 = midB;
+ }
+ }
+ else
+ {
+ //split edge q1q2
+ const Vec3V midPoint = V3Scale(V3Add(q1, q2), half);
+ const Vec3V midA = V3Scale(V3Add(a1, a2), half);
+ const Vec3V midB = V3Scale(V3Add(b1, b2), half);
+
+ const Vec3V v = V3Sub(midPoint, q0);
+ const Vec3V n = V3Normalize(V3Cross(v, triNormal));
+
+ const FloatV d = FNeg(V3Dot(n, midPoint));
+ const FloatV dp = FAdd(V3Dot(n, q1), d);
+ const FloatV sum = FMul(d, dp);
+
+ if (FAllGrtr(sum, zero))
+ {
+ //q1 and origin at the same side, split triangle[q0, q1, m]
+ q2 = midPoint;
+ a2 = midA;
+ b2 = midB;
+ }
+ else
+ {
+ //q2 and origin at the same side, split triangle[q0, m, q2]
+ q1 = midPoint;
+ a1 = midA;
+ b1 = midB;
+ }
+
+
+ }
+ }
+
+ //P must project inside face region. Compute Q using Barycentric coordinates
+ ab_ = V3Sub(q1, q0);
+ ac_ = V3Sub(q2, q0);
+ ap = V3Neg(q0);
+ bp = V3Neg(q1);
+ cp = V3Neg(q2);
+
+ d1 = V3Dot(ab_, ap); // snom
+ d2 = V3Dot(ac_, ap); // tnom
+ d3 = V3Dot(ab_, bp); // -sdenom
+ d4 = V3Dot(ac_, bp); // unom = d4 - d3
+ d5 = V3Dot(ab_, cp); // udenom = d5 - d6
+ d6 = V3Dot(ac_, cp); // -tdenom
+
+ va = FNegScaleSub(d5, d4, FMul(d3, d6));//edge region of BC
+ vb = FNegScaleSub(d1, d6, FMul(d5, d2));//edge region of AC
+ vc = FNegScaleSub(d3, d2, FMul(d1, d4));//edge region of AB
+
+ const FloatV toRecipD = FAdd(va, FAdd(vb, vc));
+ const FloatV denom = FRecip(toRecipD);//V4GetW(recipTmp);
+ const Vec3V v0 = V3Sub(a1, a0);
+ const Vec3V v1 = V3Sub(a2, a0);
+ const Vec3V w0 = V3Sub(b1, b0);
+ const Vec3V w1 = V3Sub(b2, b0);
+
+ const FloatV t = FMul(vb, denom);
+ const FloatV w = FMul(vc, denom);
+ const Vec3V vA1 = V3Scale(v1, w);
+ const Vec3V vB1 = V3Scale(w1, w);
+ const Vec3V tempClosestA = V3Add(a0, V3ScaleAdd(v0, t, vA1));
+ const Vec3V tempClosestB = V3Add(b0, V3ScaleAdd(w0, t, vB1));
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ return V3Sub(tempClosestA, tempClosestB);
+}
+
+PX_NOALIAS Vec3V closestPtPointTetrahedronTesselation(Vec3V* PX_RESTRICT Q, Vec3V* PX_RESTRICT A, Vec3V* PX_RESTRICT B, PxU32& size, Vec3V& closestA, Vec3V& closestB)
+{
+ const FloatV eps = FEps();
+ const Vec3V zeroV = V3Zero();
+ PxU32 tempSize = size;
+
+ FloatV bestSqDist = FLoad(PX_MAX_REAL);
+ const Vec3V a = Q[0];
+ const Vec3V b = Q[1];
+ const Vec3V c = Q[2];
+ const Vec3V d = Q[3];
+ const BoolV bTrue = BTTTT();
+ const BoolV bFalse = BFFFF();
+
+ //degenerated
+ const Vec3V ad = V3Sub(d, a);
+ const Vec3V bd = V3Sub(d, b);
+ const Vec3V cd = V3Sub(d, c);
+ const FloatV dad = V3Dot(ad, ad);
+ const FloatV dbd = V3Dot(bd, bd);
+ const FloatV dcd = V3Dot(cd, cd);
+ const FloatV fMin = FMin(dad, FMin(dbd, dcd));
+ if (FAllGrtr(eps, fMin))
+ {
+ size = 3;
+ PxU32 tempIndices[] = { 0, 1, 2 };
+ return closestPtPointTriangleTesselation(Q, A, B, tempIndices, size, closestA, closestB);
+ }
+
+ Vec3V _Q[] = { Q[0], Q[1], Q[2], Q[3] };
+ Vec3V _A[] = { A[0], A[1], A[2], A[3] };
+ Vec3V _B[] = { B[0], B[1], B[2], B[3] };
+
+ PxU32 indices[3] = { 0, 1, 2 };
+
+ const BoolV bIsOutside4 = PointOutsideOfPlane4(a, b, c, d);
+
+ if (BAllEq(bIsOutside4, bFalse))
+ {
+ //origin is inside the tetrahedron, we are done
+ return zeroV;
+ }
+
+ Vec3V result = zeroV;
+ Vec3V tempClosestA, tempClosestB;
+
+ if (BAllEq(BGetX(bIsOutside4), bTrue))
+ {
+
+ PxU32 tempIndices[] = { 0, 1, 2 };
+ PxU32 _size = 3;
+
+ result = closestPtPointTriangleTesselation(_Q, _A, _B, tempIndices, _size, tempClosestA, tempClosestB);
+
+ const FloatV sqDist = V3Dot(result, result);
+ bestSqDist = sqDist;
+
+ indices[0] = tempIndices[0];
+ indices[1] = tempIndices[1];
+ indices[2] = tempIndices[2];
+
+ tempSize = _size;
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ }
+
+ if (BAllEq(BGetY(bIsOutside4), bTrue))
+ {
+
+ PxU32 tempIndices[] = { 0, 2, 3 };
+
+ PxU32 _size = 3;
+
+ const Vec3V q = closestPtPointTriangleTesselation(_Q, _A, _B, tempIndices, _size, tempClosestA, tempClosestB);
+
+ const FloatV sqDist = V3Dot(q, q);
+ const BoolV con = FIsGrtr(bestSqDist, sqDist);
+ if (BAllEq(con, bTrue))
+ {
+ result = q;
+ bestSqDist = sqDist;
+ indices[0] = tempIndices[0];
+ indices[1] = tempIndices[1];
+ indices[2] = tempIndices[2];
+
+ tempSize = _size;
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ }
+ }
+
+ if (BAllEq(BGetZ(bIsOutside4), bTrue))
+ {
+
+ PxU32 tempIndices[] = { 0, 3, 1 };
+ PxU32 _size = 3;
+
+ const Vec3V q = closestPtPointTriangleTesselation(_Q, _A, _B, tempIndices, _size, tempClosestA, tempClosestB);
+
+ const FloatV sqDist = V3Dot(q, q);
+ const BoolV con = FIsGrtr(bestSqDist, sqDist);
+ if (BAllEq(con, bTrue))
+ {
+ result = q;
+ bestSqDist = sqDist;
+ indices[0] = tempIndices[0];
+ indices[1] = tempIndices[1];
+ indices[2] = tempIndices[2];
+ tempSize = _size;
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ }
+
+ }
+
+ if (BAllEq(BGetW(bIsOutside4), bTrue))
+ {
+
+ PxU32 tempIndices[] = { 1, 3, 2 };
+ PxU32 _size = 3;
+
+ const Vec3V q = closestPtPointTriangleTesselation(_Q, _A, _B, tempIndices, _size, tempClosestA, tempClosestB);
+
+ const FloatV sqDist = V3Dot(q, q);
+ const BoolV con = FIsGrtr(bestSqDist, sqDist);
+
+ if (BAllEq(con, bTrue))
+ {
+ result = q;
+ bestSqDist = sqDist;
+
+ indices[0] = tempIndices[0];
+ indices[1] = tempIndices[1];
+ indices[2] = tempIndices[2];
+
+ tempSize = _size;
+ closestA = tempClosestA;
+ closestB = tempClosestB;
+ }
+ }
+
+ A[0] = _A[indices[0]]; A[1] = _A[indices[1]]; A[2] = _A[indices[2]];
+ B[0] = _B[indices[0]]; B[1] = _B[indices[1]]; B[2] = _B[indices[2]];
+ Q[0] = _Q[indices[0]]; Q[1] = _Q[indices[1]]; Q[2] = _Q[indices[2]];
+
+
+ size = tempSize;
+ return result;
+}
+
+PX_NOALIAS PX_FORCE_INLINE Vec3V doTesselation(Vec3V* PX_RESTRICT Q, Vec3V* PX_RESTRICT A, Vec3V* PX_RESTRICT B,
+ const Vec3VArg support, const Vec3VArg supportA, const Vec3VArg supportB, PxU32& size, Vec3V& closestA, Vec3V& closestB)
+{
+ switch (size)
+ {
+ case 1:
+ {
+ closestA = supportA;
+ closestB = supportB;
+ return support;
+ }
+ case 2:
+ {
+ return closestPtPointSegmentTesselation(Q[0], support, A[0], supportA, B[0], supportB, size, closestA, closestB);
+ }
+ case 3:
+ {
+
+ PxU32 tempIndices[3] = { 0, 1, 2 };
+ return closestPtPointTriangleTesselation(Q, A, B, tempIndices, size, closestA, closestB);
+ }
+ case 4:
+ {
+ return closestPtPointTetrahedronTesselation(Q, A, B, size, closestA, closestB);
+ }
+ default:
+ PX_ASSERT(0);
+ }
+ return support;
+}
+
+
+
+
+enum Status
+{
+ STATUS_NON_INTERSECT,
+ STATUS_CONTACT,
+ STATUS_DEGENERATE,
+};
+
+struct Output
+{
+ /// Get the normal to push apart in direction from A to B
+ PX_FORCE_INLINE Vec3V getNormal() const { return V3Normalize(V3Sub(mClosestB, mClosestA)); }
+ Vec3V mClosestA; ///< Closest point on A
+ Vec3V mClosestB; ///< Closest point on B
+ FloatV mDistSq;
+};
+
+struct ConvexV
+{
+ void calcExtent(const Vec3V& dir, PxF32& minOut, PxF32& maxOut) const
+ {
+ // Expand
+ const Vec4V x = Vec4V_From_FloatV(V3GetX(dir));
+ const Vec4V y = Vec4V_From_FloatV(V3GetY(dir));
+ const Vec4V z = Vec4V_From_FloatV(V3GetZ(dir));
+
+ const Vec4V* src = mAovVertices;
+ const Vec4V* end = src + mNumAovVertices * 3;
+
+ // Do first step
+ Vec4V max = V4MulAdd(x, src[0], V4MulAdd(y, src[1], V4Mul(z, src[2])));
+ Vec4V min = max;
+ src += 3;
+ // Do the rest
+ for (; src < end; src += 3)
+ {
+ const Vec4V dot = V4MulAdd(x, src[0], V4MulAdd(y, src[1], V4Mul(z, src[2])));
+ max = V4Max(dot, max);
+ min = V4Min(dot, min);
+ }
+ FStore(V4ExtractMax(max), &maxOut);
+ FStore(V4ExtractMin(min), &minOut);
+ }
+ Vec3V calcSupport(const Vec3V& dir) const
+ {
+ // Expand
+ const Vec4V x = Vec4V_From_FloatV(V3GetX(dir));
+ const Vec4V y = Vec4V_From_FloatV(V3GetY(dir));
+ const Vec4V z = Vec4V_From_FloatV(V3GetZ(dir));
+
+ PX_ALIGN(16, static const PxF32 index4const[]) = { 0.0f, 1.0f, 2.0f, 3.0f };
+ Vec4V index4 = *(const Vec4V*)index4const;
+ PX_ALIGN(16, static const PxF32 delta4const[]) = { 4.0f, 4.0f, 4.0f, 4.0f };
+ const Vec4V delta4 = *(const Vec4V*)delta4const;
+
+ const Vec4V* src = mAovVertices;
+ const Vec4V* end = src + mNumAovVertices * 3;
+
+ // Do first step
+ Vec4V max = V4MulAdd(x, src[0], V4MulAdd(y, src[1], V4Mul(z, src[2])));
+ Vec4V maxIndex = index4;
+ index4 = V4Add(index4, delta4);
+ src += 3;
+ // Do the rest
+ for (; src < end; src += 3)
+ {
+ const Vec4V dot = V4MulAdd(x, src[0], V4MulAdd(y, src[1], V4Mul(z, src[2])));
+ const BoolV cmp = V4IsGrtr(dot, max);
+ max = V4Max(dot, max);
+ maxIndex = V4Sel(cmp, index4, maxIndex);
+ index4 = V4Add(index4, delta4);
+ }
+ Vec4V horiMax = Vec4V_From_FloatV(V4ExtractMax(max));
+ PxU32 mask = BGetBitMask(V4IsEq(horiMax, max));
+ const PxU32 simdIndex = (0x12131210 >> (mask + mask)) & PxU32(3);
+
+ /// NOTE! Could be load hit store
+ /// Would be better to have all simd.
+ PX_ALIGN(16, PxF32 f[4]);
+ V4StoreA(maxIndex, f);
+ PxU32 index = PxU32(PxI32(f[simdIndex]));
+
+ const Vec4V* aovIndex = (mAovVertices + (index >> 2) * 3);
+ const PxF32* aovOffset = ((const PxF32*)aovIndex) + (index & 3);
+
+ return Vec3V_From_Vec4V(V4LoadXYZW(aovOffset[0], aovOffset[4], aovOffset[8], 1.0f));
+ }
+
+ const Vec4V* mAovVertices; ///< Vertices storex x,x,x,x, y,y,y,y, z,z,z,z
+ PxU32 mNumAovVertices; ///< Number of groups of 4 of vertices
+};
+
+Status Collide(const Vec3V& initialDir, const ConvexV& convexA, const Mat34V& bToA, const ConvexV& convexB, Output& out)
+{
+ Vec3V Q[4];
+ Vec3V A[4];
+ Vec3V B[4];
+
+ Mat33V aToB = M34Trnsps33(bToA);
+
+ PxU32 size = 0;
+
+ const Vec3V zeroV = V3Zero();
+ const BoolV bTrue = BTTTT();
+
+ //Vec3V v = V3UnitX();
+ Vec3V v = V3Sel(FIsGrtr(V3Dot(initialDir, initialDir), FZero()), initialDir, V3UnitX());
+
+ //const FloatV minMargin = zero;
+ //const FloatV eps2 = FMul(minMargin, FLoad(0.01f));
+ //FloatV eps2 = zero;
+ FloatV eps2 = FLoad(1e-6f);
+ const FloatV epsRel = FLoad(0.000225f);
+
+ Vec3V closA(zeroV), closB(zeroV);
+ FloatV sDist = FMax();
+ FloatV minDist = sDist;
+ Vec3V closAA = zeroV;
+ Vec3V closBB = zeroV;
+
+ BoolV bNotTerminated = bTrue;
+ BoolV bCon = bTrue;
+
+ do
+ {
+ minDist = sDist;
+ closAA = closA;
+ closBB = closB;
+
+ PxU32 index = size++;
+ PX_ASSERT(index < 4);
+
+ const Vec3V supportA = convexA.calcSupport(V3Neg(v));
+ const Vec3V supportB = M34MulV3(bToA, convexB.calcSupport(M33MulV3(aToB, v)));
+ const Vec3V support = Vec3V_From_Vec4V(Vec4V_From_Vec3V(V3Sub(supportA, supportB)));
+
+ A[index] = supportA;
+ B[index] = supportB;
+ Q[index] = support;
+
+ const FloatV signDist = V3Dot(v, support);
+ const FloatV tmp0 = FSub(sDist, signDist);
+ if (FAllGrtr(FMul(epsRel, sDist), tmp0))
+ {
+ out.mClosestA = closA;
+ out.mClosestB = closB;
+ out.mDistSq = sDist;
+ return STATUS_NON_INTERSECT;
+ }
+
+ //calculate the closest point between two convex hull
+ v = doTesselation(Q, A, B, support, supportA, supportB, size, closA, closB);
+ sDist = V3Dot(v, v);
+ bCon = FIsGrtr(minDist, sDist);
+
+ bNotTerminated = BAnd(FIsGrtr(sDist, eps2), bCon);
+ } while (BAllEq(bNotTerminated, bTrue));
+
+ out.mClosestA = V3Sel(bCon, closA, closAA);
+ out.mClosestB = V3Sel(bCon, closB, closBB);
+ out.mDistSq = FSel(bCon, sDist, minDist);
+ return Status(BAllEq(bCon, bTrue) == 1 ? STATUS_CONTACT : STATUS_DEGENERATE);
+}
+
+static void _calcSeparation(const ConvexV& convexA, const physx::PxTransform& aToWorldIn, const Mat34V& bToA, ConvexV& convexB, Output& out, Separation& sep)
+{
+
+ Mat33V aToB = M34Trnsps33(bToA);
+ Vec3V normalA = out.getNormal();
+
+ convexA.calcExtent(normalA, sep.min0, sep.max0);
+ Vec3V normalB = M33MulV3(aToB, normalA);
+ convexB.calcExtent(normalB, sep.min1, sep.max1);
+
+ {
+ // Offset the min max taking into account transform
+ // Distance of origin from B's space in As space in direction of the normal in As space should fix it...
+ PxF32 fix;
+ FStore(V3Dot(bToA.col3, normalA), &fix);
+ sep.min1 += fix;
+ sep.max1 += fix;
+ }
+
+ // Looks like it's the plane at the midpoint
+ Vec3V center = V3Scale(V3Add(out.mClosestA, out.mClosestB), FLoad(0.5f));
+ // Transform to world space
+ Mat34V aToWorld;
+ *(PxMat44*)&aToWorld = aToWorldIn;
+ // Put the normal in world space
+ Vec3V worldCenter = M34MulV3(aToWorld, center);
+ Vec3V worldNormal = M34Mul33V3(aToWorld, normalA);
+
+ FloatV dist = V3Dot(worldNormal, worldCenter);
+ V3StoreU(worldNormal, sep.plane.n);
+ FStore(dist, &sep.plane.d);
+ sep.plane.d = -sep.plane.d;
+}
+
+static void _arrayVec3ToVec4(const PxVec3* src, Vec4V* dst, PxU32 num)
+{
+ const PxU32 num4 = num >> 2;
+ for (PxU32 i = 0; i < num4; i++, dst += 3, src += 4)
+ {
+ Vec3V v0 = V3LoadU(&src[0].x);
+ Vec3V v1 = V3LoadU(&src[1].x);
+ Vec3V v2 = V3LoadU(&src[2].x);
+ Vec3V v3 = V3LoadU(&src[3].x);
+ // Transpose
+ V4Transpose(v0, v1, v2, v3);
+ // Save
+ dst[0] = v0;
+ dst[1] = v1;
+ dst[2] = v2;
+ }
+ const PxU32 remain = num & 3;
+ if (remain)
+ {
+ Vec3V work[4];
+ PxU32 i = 0;
+ for (; i < remain; i++) work[i] = V3LoadU(&src[i].x);
+ for (; i < 4; i++) work[i] = work[remain - 1];
+ V4Transpose(work[0], work[1], work[2], work[3]);
+ dst[0] = work[0];
+ dst[1] = work[1];
+ dst[2] = work[2];
+ }
+}
+
+
+static void _arrayVec3ToVec4(const PxVec3* src, const Vec3V& scale, Vec4V* dst, PxU32 num)
+{
+ // If no scale - use the faster version
+ if (V3AllEq(scale, V3One()))
+ {
+ return _arrayVec3ToVec4(src, dst, num);
+ }
+
+ const PxU32 num4 = num >> 2;
+ for (PxU32 i = 0; i < num4; i++, dst += 3, src += 4)
+ {
+ Vec3V v0 = V3Mul(scale, V3LoadU(&src[0].x));
+ Vec3V v1 = V3Mul(scale, V3LoadU(&src[1].x));
+ Vec3V v2 = V3Mul(scale, V3LoadU(&src[2].x));
+ Vec3V v3 = V3Mul(scale, V3LoadU(&src[3].x));
+ // Transpose
+ V4Transpose(v0, v1, v2, v3);
+ // Save
+ dst[0] = v0;
+ dst[1] = v1;
+ dst[2] = v2;
+ }
+ const PxU32 remain = num & 3;
+ if (remain)
+ {
+ Vec3V work[4];
+ PxU32 i = 0;
+ for (; i < remain; i++) work[i] = V3Mul(scale, V3LoadU(&src[i].x));
+ for (; i < 4; i++) work[i] = work[remain - 1];
+ V4Transpose(work[0], work[1], work[2], work[3]);
+ dst[0] = work[0];
+ dst[1] = work[1];
+ dst[2] = work[2];
+ }
+}
+
+
+bool importerHullsInProximityApexFree(const std::vector<PxVec3>& hull0, PxBounds3& hull0Bounds, const physx::PxTransform& localToWorldRT0In, const physx::PxVec3& scale0In,
+ const std::vector<PxVec3>& hull1, PxBounds3& hull1Bounds, const physx::PxTransform& localToWorldRT1In, const physx::PxVec3& scale1In,
+ physx::PxF32 maxDistance, Separation* separation)
+{
+
+
+ const PxU32 numVerts0 = static_cast<PxU32>(hull0.size());
+ const PxU32 numVerts1 = static_cast<PxU32>(hull1.size());
+ const PxU32 numAov0 = (numVerts0 + 3) >> 2;
+ const PxU32 numAov1 = (numVerts1 + 3) >> 2;
+ Vec4V* verts0 = (Vec4V*)alloca((numAov0 + numAov1) * sizeof(Vec4V) * 3);
+
+ // Make sure it's aligned
+ PX_ASSERT((size_t(verts0) & 0xf) == 0);
+
+ Vec4V* verts1 = verts0 + (numAov0 * 3);
+
+ const Vec3V scale0 = V3LoadU(&scale0In.x);
+ const Vec3V scale1 = V3LoadU(&scale1In.x);
+ std::vector<PxVec3> vert0(numVerts0);
+ for (uint32_t i = 0; i < numVerts0; ++i)
+ {
+ vert0[i] = hull0[i];
+ }
+ std::vector<PxVec3> vert1(numVerts1);
+ for (uint32_t i = 0; i < numVerts1; ++i)
+ {
+ vert1[i] = hull1[i];
+ }
+
+ _arrayVec3ToVec4(&vert0[0], scale0, verts0, numVerts0);
+ _arrayVec3ToVec4(&vert1[0], scale1, verts1, numVerts1);
+
+ const PxTransform trans1To0 = localToWorldRT0In.transformInv(localToWorldRT1In);
+
+ // Load into simd mat
+ Mat34V bToA;
+ *(PxMat44*)&bToA = trans1To0;
+ (*(PxMat44*)&bToA).column3.w = 0.0f; // AOS wants the 4th component of Vec3V to be 0 to work properly
+
+ ConvexV convexA;
+ ConvexV convexB;
+
+ convexA.mNumAovVertices = numAov0;
+ convexA.mAovVertices = verts0;
+
+ convexB.mNumAovVertices = numAov1;
+ convexB.mAovVertices = verts1;
+
+ // Take the origin of B in As space as the inital direction as it is 'the difference in transform origins B-A in A's space'
+ // Should be a good first guess
+ const Vec3V initialDir = bToA.col3;
+ Output output;
+ Status status = Collide(initialDir, convexA, bToA, convexB, output);
+
+ if (status == STATUS_DEGENERATE)
+ {
+ // Calculate the tolerance from the extents
+ const PxVec3 extents0 = hull0Bounds.getExtents();
+ const PxVec3 extents1 = hull1Bounds.getExtents();
+
+ const FloatV tolerance0 = V3ExtractMin(V3Mul(V3LoadU(&extents0.x), scale0));
+ const FloatV tolerance1 = V3ExtractMin(V3Mul(V3LoadU(&extents1.x), scale1));
+
+ const FloatV tolerance = FMul(FAdd(tolerance0, tolerance1), FLoad(0.01f));
+ const FloatV sqTolerance = FMul(tolerance, tolerance);
+
+ status = FAllGrtr(sqTolerance, output.mDistSq) ? STATUS_CONTACT : STATUS_NON_INTERSECT;
+ }
+
+ switch (status)
+ {
+ case STATUS_CONTACT:
+ {
+ if (separation)
+ {
+ _calcSeparation(convexA, localToWorldRT0In, bToA, convexB, output, *separation);
+ }
+ return true;
+ }
+ default:
+ case STATUS_NON_INTERSECT:
+ {
+ if (separation)
+ {
+ _calcSeparation(convexA, localToWorldRT0In, bToA, convexB, output, *separation);
+ }
+ PxF32 val;
+ FStore(output.mDistSq, &val);
+ return val < (maxDistance * maxDistance);
+ }
+ }
+}
+
+} // namespace Blast
+} // namespace Nv
generated by cgit v1.2.3 (git 2.25.1) at 2026-03-13 02:04:41 +0000