Use mini and geometric queries in bvh query kernels

Q-Minh · Q-Minh · commit e060c79ab182 · 2024-11-02T10:14:58.000-04:00
diff --git a/source/pbat/geometry/DistanceQueries.h b/source/pbat/geometry/DistanceQueries.h
@@ -34,7 +34,7 @@ PBAT_HOST_DEVICE typename TMatrixL1::ScalarType AxisAlignedBoundingBoxes(
     TMatrixU2 const& U2);
 
 /**
- * @brief
+ * @brief Obtain squared distance between point P and axis-aligned box (L,U)
  * @tparam TMatrixP
  * @tparam TMatrixL
  * @tparam TMatrixU
@@ -64,7 +64,7 @@ PBAT_HOST_DEVICE typename TMatrixP::ScalarType
 PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C);
 
 /**
- * @brief
+ * @brief Obtain squared distance between point P and tetrahedron ABCD
  * @tparam TMatrixP
  * @tparam TMatrixA
  * @tparam TMatrixB
@@ -102,7 +102,7 @@ PBAT_HOST_DEVICE typename TMatrixX::ScalarType
 PointPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n);
 
 /**
- * @brief Obtains the distance between sphere (X,R) and triangle ABC.
+ * @brief Obtains the squared distance between sphere (X,R) and triangle ABC.
  * @param X
  * @param R
  * @param A
@@ -153,7 +153,7 @@ PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const
         return 0.;
     // Otherwise compute distance to boundary
     auto const CP = ClosestPointQueries::PointOnAxisAlignedBoundingBox(P, L, U);
-    return Norm(P - CP);
+    return SquaredNorm(P - CP);
 }
 
 template <
@@ -165,7 +165,7 @@ PBAT_HOST_DEVICE typename TMatrixP::ScalarType
 PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
 {
     auto const PP = ClosestPointQueries::PointInTriangle(P, A, B, C);
-    return Norm(P - PP);
+    return SquaredNorm(P - PP);
 }
 
 template <
@@ -214,8 +214,8 @@ PBAT_HOST_DEVICE typename TMatrixX::ScalarType SphereTriangle(
     TMatrixB const& B,
     TMatrixC const& C)
 {
-    auto const d2c = PointTriangle(X, A, B, C);
-    return d2c - R;
+    auto const sd2c = PointTriangle(X, A, B, C);
+    return sd2c - R * R;
 }
 
 } // namespace DistanceQueries
diff --git a/source/pbat/geometry/OverlapQueries.h b/source/pbat/geometry/OverlapQueries.h
@@ -553,8 +553,8 @@ PBAT_HOST_DEVICE bool LineSegmentAxisAlignedBoundingBox(
         return false;
     // Add in an epsilon term to counteract arithmetic errors when segment is
     // (near) parallel to a coordinate axis (see text for detail)
-    common::ForRange<0, kDims>([]<auto dim>() {
-        ScalarType constexpr eps = 1e-15;
+    common::ForRange<0, kDims>([&]<auto dim>() {
+        ScalarType constexpr eps{1e-15};
         ad(dim) += eps;
         auto i = (dim + 1) % kDims;
         auto j = (dim + 2) % kDims;
diff --git a/source/pbat/gpu/geometry/BvhQueryImplKernels.cuh b/source/pbat/gpu/geometry/BvhQueryImplKernels.cuh
@@ -3,6 +3,8 @@
 
 #include "BvhQueryImpl.cuh"
 #include "pbat/HostDevice.h"
+#include "pbat/geometry/DistanceQueries.h"
+#include "pbat/geometry/OverlapQueries.h"
 #include "pbat/gpu/Aliases.h"
 #include "pbat/gpu/common/Morton.cuh"
 #include "pbat/gpu/common/Queue.cuh"
@@ -18,6 +20,8 @@ namespace gpu {
 namespace geometry {
 namespace BvhQueryImplKernels {
 
+namespace mini = pbat::math::linalg::mini;
+
 struct FComputeAabb
 {
     PBAT_DEVICE void operator()(int s)
@@ -70,7 +74,6 @@ struct FComputeMortonCode
 struct FDetectOverlaps
 {
     using OverlapType = typename BvhQueryImpl::OverlapType;
-    using Vector3     = pbat::math::linalg::mini::SMatrix<GpuScalar, 3>;
 
     PBAT_DEVICE bool AreSimplicesTopologicallyAdjacent(GpuIndex si, GpuIndex sj) const
     {
@@ -90,32 +93,16 @@ struct FDetectOverlaps
         // clang-format on
     }
 
-    PBAT_DEVICE Vector3 Position(auto v) const
-    {
-        Vector3 P;
-        P(0) = x[0][v];
-        P(1) = x[1][v];
-        P(2) = x[2][v];
-        return P;
-    }
-
     PBAT_DEVICE bool VertexTetrahedronOverlap(GpuIndex v, GpuIndex t) const
     {
-        Vector3 P = Position(queryInds[0][v]);
-        Vector3 A = Position(inds[0][t]);
-        Vector3 B = Position(inds[1][t]);
-        Vector3 C = Position(inds[2][t]);
-        Vector3 D = Position(inds[3][t]);
-        auto const PointOutsidePlane =
-            [](auto const& p, auto const& a, auto const& b, auto const& c) {
-                using namespace pbat::math::linalg::mini;
-                GpuScalar const d = Dot(p - a, Cross(b - a, c - a));
-                return d > GpuScalar{0};
-            };
-        bool const bOverlaps =
-            not(PointOutsidePlane(P, A, B, D) or PointOutsidePlane(P, B, C, D) or
-                PointOutsidePlane(P, C, A, D) or PointOutsidePlane(P, A, C, B));
-        return bOverlaps;
+        using namespace mini;
+        SVector<GpuScalar, 3> P = FromBuffers<3, 1>(x, queryInds[0][v]);
+        SVector<GpuScalar, 3> A = FromBuffers<3, 1>(x, inds[0][t]);
+        SVector<GpuScalar, 3> B = FromBuffers<3, 1>(x, inds[1][t]);
+        SVector<GpuScalar, 3> C = FromBuffers<3, 1>(x, inds[2][t]);
+        SVector<GpuScalar, 3> D = FromBuffers<3, 1>(x, inds[3][t]);
+        using pbat::geometry::OverlapQueries::PointTetrahedron3D;
+        return PointTetrahedron3D(P, A, B, C, D);
     }
 
     PBAT_DEVICE bool AreSimplicesOverlapping(GpuIndex si, GpuIndex sj) const
@@ -197,140 +184,41 @@ struct FContactPairs
 {
     using OverlapType              = typename BvhQueryImpl::OverlapType;
     using NearestNeighbourPairType = typename BvhQueryImpl::NearestNeighbourPairType;
-    using Vector3                  = pbat::math::linalg::mini::SMatrix<GpuScalar, 3>;
 
-    PBAT_DEVICE Vector3 Position(auto v) const
+    PBAT_DEVICE GpuScalar MinDistance(
+        mini::SVector<GpuScalar, 3> const& X,
+        mini::SVector<GpuScalar, 3> const& L,
+        mini::SVector<GpuScalar, 3> const& U) const
     {
-        Vector3 P{};
-        P(0) = x[0][v];
-        P(1) = x[1][v];
-        P(2) = x[2][v];
-        return P;
-    }
-    PBAT_DEVICE Vector3 Lower(auto s) const
-    {
-        Vector3 P{};
-        P(0) = b[0][s];
-        P(1) = b[1][s];
-        P(2) = b[2][s];
-        return P;
-    }
-    PBAT_DEVICE Vector3 Upper(auto s) const
-    {
-        Vector3 P{};
-        P(0) = e[0][s];
-        P(1) = e[1][s];
-        P(2) = e[2][s];
-        return P;
-    }
-    PBAT_DEVICE GpuScalar MinDistance(Vector3 const& X, Vector3 const& L, Vector3 const& U) const
-    {
-        using namespace pbat::math::linalg::mini;
-        Vector3 const DX = Min(U, Max(L, X)) - X;
+        using namespace mini;
+        SVector<GpuScalar, 3> const DX = Min(U, Max(L, X)) - X;
         return SquaredNorm(DX);
     }
-    PBAT_DEVICE GpuScalar MinMaxDistance(Vector3 const& X, Vector3 const& L, Vector3 const& U) const
+    PBAT_DEVICE GpuScalar MinMaxDistance(
+        mini::SVector<GpuScalar, 3> const& X,
+        mini::SVector<GpuScalar, 3> const& L,
+        mini::SVector<GpuScalar, 3> const& U) const
     {
-        using namespace pbat::math::linalg::mini;
-        Vector3 const DXL = Squared(L - X);
-        Vector3 const DXU = Squared(U - X);
-        Vector3 const rm  = Min(DXL, DXU);
-        Vector3 const rM  = Max(DXL, DXU);
-        std::array<GpuScalar, 3> const d{
+        using namespace mini;
+        SVector<GpuScalar, 3> DXL = Squared(L - X);
+        SVector<GpuScalar, 3> DXU = Squared(U - X);
+        SVector<GpuScalar, 3> rm  = Min(DXL, DXU);
+        SVector<GpuScalar, 3> rM  = Max(DXL, DXU);
+        SVector<GpuScalar, 3> d{
             rm(0) + rM(1) + rM(2),
             rM(0) + rm(1) + rM(2),
             rM(0) + rM(1) + rm(2),
         };
-        return min(d[0], min(d[1], d[2]));
+        return Min(d);
     }
-    PBAT_DEVICE GpuScalar Distance(Vector3 const& P, GpuIndex s) const
+    PBAT_DEVICE GpuScalar Distance(mini::SVector<GpuScalar, 3> const& P, GpuIndex s) const
     {
-        using namespace pbat::math::linalg::mini;
-        SMatrix<GpuScalar, 3, 3> ABC;
-        auto A = ABC.Col(0);
-        auto B = ABC.Col(1);
-        auto C = ABC.Col(2);
-        A      = Position(targetInds[0][s]);
-        B      = Position(targetInds[1][s]);
-        C      = Position(targetInds[2][s]);
-
-        /**
-         * Ericson, Christer. Real-time collision detection. Crc Press, 2004. section 5.1.5
-         */
-
-        // Check if P in vertex region outside A
-        Vector3 const AB                     = B - A;
-        Vector3 const AC                     = C - A;
-        Vector3 const AP                     = P - A;
-        GpuScalar const d1                   = Dot(AB, AP);
-        GpuScalar const d2                   = Dot(AC, AP);
-        bool const bIsInVertexRegionOutsideA = d1 <= GpuScalar{0} and d2 <= GpuScalar{0};
-        if (bIsInVertexRegionOutsideA)
-        {
-            // barycentric coordinates (1,0,0)
-            return SquaredNorm(P - A);
-        }
-
-        // Check if P in vertex region outside B
-        Vector3 const BP                     = P - B;
-        GpuScalar const d3                   = Dot(AB, BP);
-        GpuScalar const d4                   = Dot(AC, BP);
-        bool const bIsInVertexRegionOutsideB = d3 >= GpuScalar{0} and d4 <= d3;
-        if (bIsInVertexRegionOutsideB)
-        {
-            // barycentric coordinates (0,1,0)
-            return SquaredNorm(P - B);
-        }
-
-        // Check if P in edge region of AB, if so return projection of P onto AB
-        GpuScalar const vc = d1 * d4 - d3 * d2;
-        bool const bIsInEdgeRegionOfAB =
-            vc <= GpuScalar{0} and d1 >= GpuScalar{0} and d3 <= GpuScalar{0};
-        if (bIsInEdgeRegionOfAB)
-        {
-            GpuScalar const v = d1 / (d1 - d3);
-            // barycentric coordinates (1-v,v,0)
-            return SquaredNorm(P - ((GpuScalar{1} - v) * A + v * B));
-        }
-
-        // Check if P in vertex region outside C
-        Vector3 const CP                     = P - C;
-        GpuScalar const d5                   = Dot(AB, CP);
-        GpuScalar const d6                   = Dot(AC, CP);
-        bool const bIsInVertexRegionOutsideC = d6 >= GpuScalar{0} and d5 <= d6;
-        if (bIsInVertexRegionOutsideC)
-        {
-            // barycentric coordinates (0,0,1)
-            return SquaredNorm(P - C);
-        }
-
-        // Check if P in edge region of AC, if so return projection of P onto AC
-        GpuScalar const vb = d5 * d2 - d1 * d6;
-        bool const bIsInEdgeRegionOfAC =
-            (vb <= GpuScalar{0} and d2 >= GpuScalar{0} and d6 <= GpuScalar{0});
-        if (bIsInEdgeRegionOfAC)
-        {
-            GpuScalar const w = d2 / (d2 - d6);
-            // barycentric coordinates (1-w,0,w)
-            return SquaredNorm(P - (GpuScalar{1} - w) * A + w * C);
-        }
-        // Check if P in edge region of BC, if so return projection of P onto BC
-        GpuScalar const va = d3 * d6 - d5 * d4;
-        bool const bIsInEdgeRegionOfBC =
-            va <= GpuScalar{0} and (d4 - d3) >= GpuScalar{0} and (d5 - d6) >= GpuScalar{0};
-        if (bIsInEdgeRegionOfBC)
-        {
-            GpuScalar const w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-            // barycentric coordinates (0,1-w,w)
-            return SquaredNorm(P - ((GpuScalar{1} - w) * B + w * C));
-        }
-        // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
-        GpuScalar const denom = GpuScalar{1} / (va + vb + vc);
-        SMatrix<GpuScalar, 3> uvw;
-        uvw(1) = vb * denom;
-        uvw(2) = vc * denom;
-        uvw(0) = GpuScalar{1} - uvw(1) - uvw(2);
-        return SquaredNorm(P - ABC * uvw);
+        using namespace mini;
+        auto A = FromBuffers<3, 1>(x, targetInds[0][s]);
+        auto B = FromBuffers<3, 1>(x, targetInds[1][s]);
+        auto C = FromBuffers<3, 1>(x, targetInds[2][s]);
+        using pbat::geometry::DistanceQueries::PointTriangle;
+        return PointTriangle(P, A, B, C);
     }
 
     struct BoxOrSimplex
@@ -342,14 +230,19 @@ struct FContactPairs
     struct BranchAndBound
     {
         PBAT_DEVICE
-        BranchAndBound(Vector3 const& X, GpuScalar R, GpuScalar dzero, GpuIndex sv, GpuIndex v)
+        BranchAndBound(
+            mini::SVector<GpuScalar, 3> const& X,
+            GpuScalar R,
+            GpuScalar dzero,
+            GpuIndex sv,
+            GpuIndex v)
             : stack{}, nearest{}, X(X), R(R), dzero(dzero), sv(sv), v(v)
         {
         }
 
         common::Stack<BoxOrSimplex, 64> stack;
         common::Queue<GpuIndex, 8> nearest;
-        Vector3 X;
+        mini::SVector<GpuScalar, 3> X;
         GpuScalar R;
         GpuScalar dzero;
         GpuIndex sv;
@@ -396,10 +289,14 @@ struct FContactPairs
 
     PBAT_DEVICE void operator()(OverlapType const& o)
     {
+        using namespace mini;
         // Branch and bound over BVH
         GpuIndex const v = queryInds[0][o.first];
-        BranchAndBound traversal{Position(v), R, dzero, o.first, v};
-        Push(traversal, 0, MinDistance(traversal.X, Lower(0), Upper(0)));
+        BranchAndBound traversal{FromBuffers<3, 1>(x, v), R, dzero, o.first, v};
+        Push(
+            traversal,
+            0,
+            MinDistance(traversal.X, FromBuffers<3, 1>(b, 0), FromBuffers<3, 1>(e, 0)));
         do
         {
             assert(not traversal.stack.IsFull());
@@ -410,10 +307,10 @@ struct FContactPairs
             GpuIndex const lc = child[0][bos.node];
             GpuIndex const rc = child[1][bos.node];
 
-            Vector3 const LL = Lower(lc);
-            Vector3 const LU = Upper(lc);
-            Vector3 const RL = Lower(rc);
-            Vector3 const RU = Upper(rc);
+            mini::SVector<GpuScalar, 3> const LL = FromBuffers<3, 1>(b, lc);
+            mini::SVector<GpuScalar, 3> const LU = FromBuffers<3, 1>(e, lc);
+            mini::SVector<GpuScalar, 3> const RL = FromBuffers<3, 1>(b, rc);
+            mini::SVector<GpuScalar, 3> const RU = FromBuffers<3, 1>(e, rc);
 
             GpuScalar Ldmin    = MinDistance(traversal.X, LL, LU);
             GpuScalar Rdmin    = MinDistance(traversal.X, RL, RU);
diff --git a/source/pbat/math/linalg/mini/Matrix.h b/source/pbat/math/linalg/mini/Matrix.h
@@ -258,10 +258,9 @@ template <CMatrix TMatrix, class IndexType>
 PBAT_HOST_DEVICE void
 ToFlatBuffer(TMatrix const& A, typename TMatrix::ScalarType* buf, IndexType bi)
 {
-    using ScalarType                          = typename TMatrix::ScalarType;
-    auto constexpr M                          = TMatrix::kRows;
-    auto constexpr N                          = TMatrix::kCols;
-    FromFlatBuffer<M, N, ScalarType>(buf, bi) = A;
+    auto constexpr M              = TMatrix::kRows;
+    auto constexpr N              = TMatrix::kCols;
+    FromFlatBuffer<M, N>(buf, bi) = A;
 }
 
 template <CMatrix TMatrix, CMatrix TIndexMatrix>
@@ -296,7 +295,7 @@ PBAT_HOST_DEVICE auto FromBuffers(std::array<TScalar*, M> buf, IndexType bi)
     using ScalarType = std::remove_const_t<TScalar>;
     SMatrix<ScalarType, M, N> A{};
     using pbat::common::ForRange;
-    ForRange<0, M>([&]<auto i>() { A.Row(i) = FromFlatBuffer<1, N, ScalarType>(buf[i], bi); });
+    ForRange<0, M>([&]<auto i>() { A.Row(i) = FromFlatBuffer<1, N>(buf[i], bi); });
     return A;
 }
 
@@ -310,8 +309,7 @@ PBAT_HOST_DEVICE auto FromBuffers(std::array<TScalar*, K> buf, TIndexMatrix cons
     using ScalarType = std::remove_cvref_t<TScalar>;
     SMatrix<ScalarType, K * M, N> A{};
     using pbat::common::ForRange;
-    ForRange<0, K>(
-        [&]<auto k>() { A.Slice<M, N>(k * M, 0) = FromFlatBuffer<ScalarType>(buf[k], inds); });
+    ForRange<0, K>([&]<auto k>() { A.Slice<M, N>(k * M, 0) = FromFlatBuffer(buf[k], inds); });
     return A;
 }
 
@@ -321,9 +319,8 @@ ToBuffers(TMatrix const& A, std::array<typename TMatrix::ScalarType*, M> buf, In
 {
     static_assert(M == TMatrix::kRows, "A must have same rows as number of buffers");
     auto constexpr N = TMatrix::kCols;
-    using ScalarType = typename TMatrix::ScalarType;
     using pbat::common::ForRange;
-    ForRange<0, M>([&]<auto i>() { FromFlatBuffer<1, N, ScalarType>(buf[i], bi) = A.Row(i); });
+    ForRange<0, M>([&]<auto i>() { FromFlatBuffer<1, N>(buf[i], bi) = A.Row(i); });
 }
 
 template <CMatrix TMatrix, CMatrix TIndexMatrix, int K>
diff --git a/source/pbat/math/linalg/mini/UnaryOperations.h b/source/pbat/math/linalg/mini/UnaryOperations.h
