MultibodyMeshMixedCcdDcd.h

Go to the documentation of this file.

 
#ifndef PBAT_SIM_CONTACT_MULTIBODYMESHMIXEDCCDDCD_H
#define PBAT_SIM_CONTACT_MULTIBODYMESHMIXEDCCDDCD_H
 
#include "PhysicsBasedAnimationToolkitExport.h"
#include "pbat/Aliases.h"
#include "pbat/common/CountingSort.h"
#include "pbat/geometry/AabbKdTreeHierarchy.h"
#include "pbat/geometry/AabbRadixTreeHierarchy.h"
#include "pbat/geometry/DistanceQueries.h"
#include "pbat/geometry/EdgeEdgeCcd.h"
#include "pbat/geometry/IntersectionQueries.h"
#include "pbat/geometry/OverlapQueries.h"
#include "pbat/geometry/PointTriangleCcd.h"
#include "pbat/math/linalg/mini/Eigen.h"
#include "pbat/profiling/Profiling.h"
 
#include <algorithm>
#include <utility>
#include <vector>
 
namespace pbat::sim::contact {

class MultibodyMeshMixedCcdDcd
{
  public:
    static auto constexpr kDims = 3; 
    using BodyBvh               = geometry::AabbRadixTreeHierarchy<kDims>; 
    using VertexBvh             = geometry::AabbKdTreeHierarchy<kDims>; 
    using EdgeBvh               = geometry::AabbKdTreeHierarchy<kDims>; 
    using TriangleBvh           = geometry::AabbKdTreeHierarchy<kDims>; 
    using TetrahedronBvh = geometry::AabbKdTreeHierarchy<kDims>; 

    struct VertexTriangleContact
    {
        using TriangleVertices = math::linalg::mini::SVector<Index, 3>; 
 
        Index i;              
        Index f;              
        TriangleVertices fjs; 
    };

    struct EdgeEdgeContact
    {
        using EdgeBarycentricCoordinates =
            math::linalg::mini::SVector<Scalar, 2>;                 
        using EdgeVertices = math::linalg::mini::SVector<Index, 2>; 
 
        Index ei;         
        Index ej;         
        EdgeVertices eis; 
        EdgeVertices ejs; 
        EdgeBarycentricCoordinates
            beta; 
    };

    template <class TDerivedX>
    MultibodyMeshMixedCcdDcd(
        Eigen::DenseBase<TDerivedX> const& X,
        Eigen::Ref<IndexVectorX const> const& V,
        Eigen::Ref<IndexMatrix<2, Eigen::Dynamic> const> const& E,
        Eigen::Ref<IndexMatrix<3, Eigen::Dynamic> const> const& F,
        Eigen::Ref<IndexMatrix<4, Eigen::Dynamic> const> const& T,
        Eigen::Ref<IndexVectorX const> const& VP,
        Eigen::Ref<IndexVectorX const> const& EP,
        Eigen::Ref<IndexVectorX const> const& FP,
        Eigen::Ref<IndexVectorX const> const& TP);
    template <
        class FOnVertexTriangleContactPair,
        class FOnEdgeEdgeContactPair,
        class TDerivedXT,
        class TDerivedX,
        class TDerivedXK>
    void UpdateActiveSet(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X,
        Eigen::DenseBase<TDerivedXK> const& XK,
        FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
        FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair);
    template <class FOnVertexTriangleContactPair, class TDerivedX>
    void UpdateDcdActiveSet(
        Eigen::DenseBase<TDerivedX> const& X,
        FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair);
    template <
        class FOnVertexTriangleContactPair,
        class FOnEdgeEdgeContactPair,
        class TDerivedXT,
        class TDerivedX>
    void UpdateCcdActiveSet(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X,
        FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
        FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair);

    [[maybe_unused]] auto const& GetBodyAabbs() const { return mBodyAabbs; }
    [[maybe_unused]] auto const& GetVertexAabbs() const { return mVertexAabbs; }
    [[maybe_unused]] auto const& GetEdgeAabbs() const { return mEdgeAabbs; }
    [[maybe_unused]] auto const& GetTriangleAabbs() const { return mTriangleAabbs; }
    [[maybe_unused]] auto const& GetTetrahedronAabbs() const { return mTetrahedronAabbs; }

    template <class TDerivedX>
    void ComputeVertexAabbs(Eigen::DenseBase<TDerivedX> const& X);
    template <class TDerivedXT, class TDerivedX>
    void ComputeVertexAabbs(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X);
    template <class TDerivedXT, class TDerivedX>
    void
    ComputeEdgeAabbs(Eigen::DenseBase<TDerivedXT> const& XT, Eigen::DenseBase<TDerivedX> const& X);
    template <class TDerivedX>
    void ComputeTriangleAabbs(Eigen::DenseBase<TDerivedX> const& X);
    template <class TDerivedXT, class TDerivedX>
    void ComputeTriangleAabbs(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X);


    template <class TDerivedX>
    void ComputeTetrahedronAabbs(Eigen::DenseBase<TDerivedX> const& X);
    PBAT_API void ComputeBodyAabbs();
    PBAT_API void UpdateMeshVertexBvhs();
    PBAT_API void UpdateMeshEdgeBvhs();
    PBAT_API void UpdateMeshTriangleBvhs();
    PBAT_API void UpdateMeshTetrahedronBvhs();
    PBAT_API void RecomputeBodyBvh();
    template <class FOnBodyPair>
    void ForEachBodyPair(FOnBodyPair&& fOnBodyPair) const;
    template <class FOnPenetratingVertex, class TDerivedX>
    void ForEachPenetratingVertex(
        Eigen::DenseBase<TDerivedX> const& X,
        FOnPenetratingVertex&& fOnPenetratingVertex);
    [[maybe_unused]] auto VertexCount() const { return mVertexAabbs.cols(); }
    [[maybe_unused]] auto EdgeCount() const { return mEdgeAabbs.cols(); } 
    [[maybe_unused]] auto TriangleCount() const { return mTriangleAabbs.cols(); }
    [[maybe_unused]] auto TetrahedronCount() const { return mTetrahedronAabbs.cols(); }
    [[maybe_unused]] auto BodyCount() const { return mBodyAabbs.cols(); }
 
  protected:
    template <class FOnVertexTriangleContactPair, class TDerivedX>
    void HandleDcdPairs(
        Eigen::DenseBase<TDerivedX> const& X,
        FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair);
    template <
        class FOnVertexTriangleContactPair,
        class FOnEdgeEdgeContactPair,
        class TDerivedXT,
        class TDerivedX>
    void HandleCcdPairs(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X,
        FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
        FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair);
    template <class FOnVertexTriangleContactPair, class TDerivedXT, class TDerivedX>
    void ReportVertexTriangleCcdContacts(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X,
        Index oi,
        Index oj,
        FOnVertexTriangleContactPair fOnVertexTriangleContactPair) const;
    template <class FOnEdgeEdgeContactPair, class TDerivedXT, class TDerivedX>
    void ReportEdgeEdgeCcdContacts(
        Eigen::DenseBase<TDerivedXT> const& XT,
        Eigen::DenseBase<TDerivedX> const& X,
        Index oi,
        Index oj,
        FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair) const;
 
  private:
 
    Eigen::Ref<IndexVectorX const>
        mVP; 
    Eigen::Ref<IndexVectorX const> mEP; 
    Eigen::Ref<IndexVectorX const>
        mFP; 
    Eigen::Ref<IndexVectorX const>
        mTP; 

 
    Eigen::Ref<IndexVectorX const>
        mV; 
    Eigen::Ref<IndexMatrix<2, Eigen::Dynamic> const>
        mE; 
    Eigen::Ref<IndexMatrix<3, Eigen::Dynamic> const>
        mF; 
    Eigen::Ref<IndexMatrix<4, Eigen::Dynamic> const>
        mT; 

 
    Matrix<2 * kDims, Eigen::Dynamic> mVertexAabbs; 
    Matrix<2 * kDims, Eigen::Dynamic> mEdgeAabbs; 
    Matrix<2 * kDims, Eigen::Dynamic>
        mTriangleAabbs; 
    Matrix<2 * kDims, Eigen::Dynamic>
        mTetrahedronAabbs; 

 
    std::vector<VertexBvh> mVertexBvhs;           
    std::vector<EdgeBvh> mEdgeBvhs;               
    std::vector<TriangleBvh> mTriangleBvhs;       
    std::vector<TetrahedronBvh> mTetrahedronBvhs; 

 
    Matrix<2 * kDims, Eigen::Dynamic>
        mBodyAabbs;   
    BodyBvh mBodyBvh; 
 
    Eigen::Vector<bool, Eigen::Dynamic> mPenetratingVertexMask; 
};
 
template <class TDerivedX>
inline MultibodyMeshMixedCcdDcd::MultibodyMeshMixedCcdDcd(
    Eigen::DenseBase<TDerivedX> const& X,
    Eigen::Ref<IndexVectorX const> const& V,
    Eigen::Ref<IndexMatrix<2, Eigen::Dynamic> const> const& E,
    Eigen::Ref<IndexMatrix<3, Eigen::Dynamic> const> const& F,
    Eigen::Ref<IndexMatrix<4, Eigen::Dynamic> const> const& T,
    Eigen::Ref<IndexVectorX const> const& VP,
    Eigen::Ref<IndexVectorX const> const& EP,
    Eigen::Ref<IndexVectorX const> const& FP,
    Eigen::Ref<IndexVectorX const> const& TP)
    : mVP(VP), mEP(EP), mFP(FP), mTP(TP), mV(V), mE(E), mF(F), mT(T)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.Construct");
#include "pbat/warning/Push.h"
#include "pbat/warning/SignConversion.h"
    // Allocate memory for AABBs and active sets
    mVertexAabbs.resize(2 * kDims, V.size());
    mEdgeAabbs.resize(2 * kDims, E.cols());
    mTriangleAabbs.resize(2 * kDims, F.cols());
    mTetrahedronAabbs.resize(2 * kDims, T.cols());
    mBodyAabbs.resize(2 * kDims, VP.size() - 1);
    mPenetratingVertexMask.resize(X.cols());
    mPenetratingVertexMask.setConstant(false);
    // Allocate memory for mesh BVHs
    auto const nObjects = mVP.size() - 1;
    mVertexBvhs.resize(nObjects);
    mEdgeBvhs.resize(nObjects);
    mTriangleBvhs.resize(nObjects);
    mTetrahedronBvhs.resize(nObjects);
    // Compute static mesh primitive AABBs for tree construction
    ComputeVertexAabbs(X.derived());
    ComputeEdgeAabbs(X.derived(), X.derived());
    ComputeTriangleAabbs(X.derived());
    ComputeTetrahedronAabbs(X.derived());
    // Construct mesh BVHs
    for (auto o = 0; o < nObjects; ++o)
    {
        auto VB = mVertexAabbs(Eigen::placeholders::all, Eigen::seq(mVP(o), mVP(o + 1) - 1));
        auto EB = mEdgeAabbs(Eigen::placeholders::all, Eigen::seq(mEP(o), mEP(o + 1) - 1));
        auto FB = mTriangleAabbs(Eigen::placeholders::all, Eigen::seq(mFP(o), mFP(o + 1) - 1));
        auto TB = mTetrahedronAabbs(Eigen::placeholders::all, Eigen::seq(mTP(o), mTP(o + 1) - 1));
        // Construct object o's mesh BVH tree topology
        mVertexBvhs[o].Construct(VB.topRows<kDims>(), VB.bottomRows<kDims>());
        mEdgeBvhs[o].Construct(EB.topRows<kDims>(), EB.bottomRows<kDims>());
        mTriangleBvhs[o].Construct(FB.topRows<kDims>(), FB.bottomRows<kDims>());
        mTetrahedronBvhs[o].Construct(TB.topRows<kDims>(), TB.bottomRows<kDims>());
        // Compute object o's AABB
        auto XVo = X(Eigen::placeholders::all, mV(Eigen::seq(mVP(o), mVP(o + 1) - 1)));
        mBodyAabbs.col(o).head<kDims>() = XVo.rowwise().minCoeff();
        mBodyAabbs.col(o).tail<kDims>() = XVo.rowwise().maxCoeff();
    }
    // Construct body BVH to allocate memory
    mBodyBvh.Construct(mBodyAabbs.topRows<kDims>(), mBodyAabbs.bottomRows<kDims>());
#include "pbat/warning/Pop.h"
}
 
template <
    class FOnVertexTriangleContactPair,
    class FOnEdgeEdgeContactPair,
    class TDerivedXT,
    class TDerivedX,
    class TDerivedXK>
inline void MultibodyMeshMixedCcdDcd::UpdateActiveSet(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X,
    Eigen::DenseBase<TDerivedXK> const& XK,
    FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
    FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.UpdateActiveSet");
    UpdateDcdActiveSet(
        XK,
        std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair));
    UpdateCcdActiveSet(
        XT,
        X,
        std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair),
        std::forward<FOnEdgeEdgeContactPair>(fOnEdgeEdgeContactPair));
}
 
template <class FOnVertexTriangleContactPair, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::UpdateDcdActiveSet(
    Eigen::DenseBase<TDerivedX> const& X,
    FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.UpdateDcdActiveSet");
    // Update AABBs for discrete collision detection
    ComputeVertexAabbs(X);
    ComputeTriangleAabbs(X);
    ComputeTetrahedronAabbs(X);
    // Update BVH bounding boxes
    UpdateMeshVertexBvhs();
    UpdateMeshTriangleBvhs();
    UpdateMeshTetrahedronBvhs();
    // Rebuild world tree
    ComputeBodyAabbs();
    RecomputeBodyBvh();
    // Add DCD vertex-triangle pairs to active set
    HandleDcdPairs(X, std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair));
}
 
template <
    class FOnVertexTriangleContactPair,
    class FOnEdgeEdgeContactPair,
    class TDerivedXT,
    class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::UpdateCcdActiveSet(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X,
    FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
    FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.UpdateCcdActiveSet");
    // Compute AABBs for linearly swept vertices, edges and triangles
    ComputeVertexAabbs(XT, X);
    ComputeEdgeAabbs(XT, X);
    ComputeTriangleAabbs(XT, X);
    // Update BVHs
    UpdateMeshVertexBvhs();
    UpdateMeshEdgeBvhs();
    UpdateMeshTriangleBvhs();
    // Rebuild world tree
    ComputeBodyAabbs();
    RecomputeBodyBvh();
    // Report all vertex-triangle and edge-edge CCD contacts
    HandleCcdPairs(
        XT,
        X,
        std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair),
        std::forward<FOnEdgeEdgeContactPair>(fOnEdgeEdgeContactPair));
}
 
template <class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeVertexAabbs(Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeVertexAabbs");
    auto XV                          = X(Eigen::placeholders::all, mV);
    mVertexAabbs.topRows<kDims>()    = XV;
    mVertexAabbs.bottomRows<kDims>() = XV;
}
 
template <class TDerivedXT, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeVertexAabbs(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeVertexAabbs");
    auto XTV = XT.template topRows<kDims>()(Eigen::placeholders::all, mV);
    auto XV  = X.template topRows<kDims>()(Eigen::placeholders::all, mV);
    for (auto v = 0; v < mV.cols(); ++v)
    {
        auto i = mV(v);
        auto L = mVertexAabbs.col(v).head<kDims>();
        auto U = mVertexAabbs.col(v).tail<kDims>();
        L      = XTV.col(i).cwiseMin(XV.col(i));
        U      = XTV.col(i).cwiseMin(XV.col(i));
    }
}
 
template <class TDerivedXT, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeEdgeAabbs(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeEdgeAabbs");
    for (auto e = 0; e < mE.cols(); ++e)
    {
        Matrix<kDims, 2 * 2> XE;
        XE.leftCols<2>()  = XT(Eigen::placeholders::all, mE.col(e)).template block<kDims, 2>(0, 0);
        XE.rightCols<2>() = X(Eigen::placeholders::all, mE.col(e)).template block<kDims, 2>(0, 0);
        auto L            = mEdgeAabbs.col(e).head<kDims>();
        auto U            = mEdgeAabbs.col(e).tail<kDims>();
        L                 = XE.rowwise().minCoeff();
        U                 = XE.rowwise().maxCoeff();
    }
}
 
template <class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeTriangleAabbs(Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeTriangleAabbs");
#include "pbat/warning/Push.h"
#include "pbat/warning/SignConversion.h"
    for (auto f = 0; f < mF.cols(); ++f)
    {
        Matrix<kDims, 3> XF;
        XF     = X(Eigen::placeholders::all, mF.col(f)).template block<kDims, 3>(0, 0);
        auto L = mTriangleAabbs.col(f).head<kDims>();
        auto U = mTriangleAabbs.col(f).tail<kDims>();
        L      = XF.rowwise().minCoeff();
        U      = XF.rowwise().maxCoeff();
    }
#include "pbat/warning/Pop.h"
}
 
template <class TDerivedXT, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeTriangleAabbs(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeTriangleAabbs");
    for (auto f = 0; f < mF.cols(); ++f)
    {
        Matrix<kDims, 2 * 3> XF;
        XF.leftCols<3>()  = XT(Eigen::placeholders::all, mF.col(f)).template block<kDims, 3>(0, 0);
        XF.rightCols<3>() = X(Eigen::placeholders::all, mF.col(f)).template block<kDims, 3>(0, 0);
        auto L            = mTriangleAabbs.col(f).head<kDims>();
        auto U            = mTriangleAabbs.col(f).tail<kDims>();
        L                 = XF.rowwise().minCoeff();
        U                 = XF.rowwise().maxCoeff();
    }
}
 
template <class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ComputeTetrahedronAabbs(Eigen::DenseBase<TDerivedX> const& X)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ComputeTetrahedronAabbs");
    for (auto t = 0; t < mT.cols(); ++t)
    {
        Matrix<kDims, 4> XT = X(Eigen::placeholders::all, mT.col(t)).template block<kDims, 4>(0, 0);
        auto L              = mTetrahedronAabbs.col(t).template head<kDims>();
        auto U              = mTetrahedronAabbs.col(t).template tail<kDims>();
        L                   = XT.rowwise().minCoeff();
        U                   = XT.rowwise().maxCoeff();
    }
}
 
template <class FOnBodyPair>
inline void MultibodyMeshMixedCcdDcd::ForEachBodyPair(FOnBodyPair&& fOnBodyPair) const
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ForEachBodyPair");
    mBodyBvh.SelfOverlaps(
        [&](Index o1, Index o2) {
            using math::linalg::mini::FromEigen;
            auto L1 = mBodyAabbs.col(o1).template head<kDims>();
            auto U1 = mBodyAabbs.col(o1).template tail<kDims>();
            auto L2 = mBodyAabbs.col(o2).template head<kDims>();
            auto U2 = mBodyAabbs.col(o2).template tail<kDims>();
            return geometry::OverlapQueries::AxisAlignedBoundingBoxes(
                FromEigen(L1),
                FromEigen(U1),
                FromEigen(L2),
                FromEigen(U2));
        },
        [fOnBodyPair = std::forward<FOnBodyPair>(
             fOnBodyPair)](Index oi, Index oj, [[maybe_unused]] Index k) { fOnBodyPair(oi, oj); });
}
 
template <class FOnPenetratingVertex, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ForEachPenetratingVertex(
    Eigen::DenseBase<TDerivedX> const& X,
    FOnPenetratingVertex&& fOnPenetratingVertex)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ForEachPenetratingVertex");
    auto const fVisitPenetratingVerticesOf =
        [&,
         fOnPenetratingVertex =
             std::forward<FOnPenetratingVertex>(fOnPenetratingVertex)](Index oi, Index oj) {
            Index i;
            Vector<kDims> xi;
            mVertexBvhs[static_cast<std::size_t>(oi)].Overlaps(
                mTetrahedronBvhs[static_cast<std::size_t>(oj)],
                [&](Index v, Index t) {
                    // Global vertex and tetrahedron indices
                    v = mVP(oi) + v;
                    t = mTP(oj) + t;
                    i = mV(v);
                    // Skip test, if we already know this vertex is penetrating
                    if (mPenetratingVertexMask[i])
                        return false;
                    // Run overlap test
                    xi                   = X.col(i);
                    IndexVector<4> tinds = mT.col(t);
                    Matrix<kDims, 4> XT  = X(Eigen::placeholders::all, tinds);
                    using math::linalg::mini::FromEigen;
                    // Mark vertex as penetrating so that it doesn't participate in CCD
                    bool const bIsPenetrating = geometry::OverlapQueries::PointTetrahedron3D(
                        FromEigen(xi),
                        FromEigen(XT.col(0)),
                        FromEigen(XT.col(1)),
                        FromEigen(XT.col(2)),
                        FromEigen(XT.col(3)));
                    // Cache vertex penetrating status if applicable
                    if (bIsPenetrating)
                        mPenetratingVertexMask[i] = true;
                    return bIsPenetrating;
                },
                [&]([[maybe_unused]] Index v, [[maybe_unused]] Index t, [[maybe_unused]] Index k) {
                    fOnPenetratingVertex(i, oj);
                });
        };
    // Reset penetrating vertex mask
    mPenetratingVertexMask.setConstant(false);
    // Only check for relevant body pairs where vertices may penetrate
    ForEachBodyPair([&](Index oi, Index oj) {
        fVisitPenetratingVerticesOf(oi, oj);
        fVisitPenetratingVerticesOf(oj, oi);
    });
}
 
template <class FOnVertexTriangleContactPair, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::HandleDcdPairs(
    Eigen::DenseBase<TDerivedX> const& X,
    FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair)
{
#include "pbat/warning/Push.h"
#include "pbat/warning/SignConversion.h"
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.HandleDcdPairs");
    // Report vertex-triangle contacts for each DCD penetrating vertex
    ForEachPenetratingVertex(X.derived(), [&](Index i, Index o) {
        // Contact point is vertex position
        Vector<kDims> const XC = X.col(i);
        // Define point-aabb and point-triangle distance functions for vertex i
        auto const fDistanceToBox = [&]<class TL, class TU>(TL const& L, TU const& U) {
            using math::linalg::mini::FromEigen;
            return geometry::DistanceQueries::PointAxisAlignedBoundingBox(
                FromEigen(XC),
                FromEigen(L),
                FromEigen(U));
        };
        auto const fDistanceToTriangle = [&](Index f) {
            using math::linalg::mini::FromEigen;
            Matrix<kDims, 3> XF = X(Eigen::placeholders::all, mF.col(f));
            return geometry::DistanceQueries::PointTriangle(
                FromEigen(XC),
                FromEigen(XF.col(0)),
                FromEigen(XF.col(1)),
                FromEigen(XF.col(2)));
        };
        // Find nearest surface point (i.e. triangle)
        mTriangleBvhs[o].NearestNeighbours(
            fDistanceToBox,
            fDistanceToTriangle,
            [&, fReport = std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair)](
                Index f,
                [[maybe_unused]] Scalar d,
                [[maybe_unused]] Index k) {
                using math::linalg::mini::FromEigen;
                f = mFP(o) + f;
                fReport(VertexTriangleContact{i, f, FromEigen(mF.col(f))});
            });
    });
#include "pbat/warning/Pop.h"
}
 
template <
    class FOnVertexTriangleContactPair,
    class FOnEdgeEdgeContactPair,
    class TDerivedXT,
    class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::HandleCcdPairs(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X,
    FOnVertexTriangleContactPair&& fOnVertexTriangleContactPair,
    FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair)
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.HandleCcdPairs");
    ForEachBodyPair([&](Index oi, Index oj) {
        ReportVertexTriangleCcdContacts(
            XT,
            X,
            oi,
            oj,
            std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair));
        ReportVertexTriangleCcdContacts(
            XT,
            X,
            oj,
            oi,
            std::forward<FOnVertexTriangleContactPair>(fOnVertexTriangleContactPair));
        ReportEdgeEdgeCcdContacts(
            XT,
            X,
            oi,
            oj,
            std::forward<FOnEdgeEdgeContactPair>(fOnEdgeEdgeContactPair));
    });
}
 
template <class FOnVertexTriangleContactPair, class TDerivedXT, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ReportVertexTriangleCcdContacts(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X,
    Index oi,
    Index oj,
    FOnVertexTriangleContactPair fOnVertexTriangleContactPair) const
{
    PBAT_PROFILE_NAMED_SCOPE(
        "pbat.sim.contact.MultibodyMeshMixedCcdDcd.ReportVertexTriangleCcdContacts");
    mVertexBvhs[static_cast<std::size_t>(oi)].Overlaps(
        mTriangleBvhs[static_cast<std::size_t>(oj)],
        [&,
         fOnVertexTriangleContactPair = std::forward<FOnVertexTriangleContactPair>(
             fOnVertexTriangleContactPair)](Index v, Index f) {
            // Global vertex and triangle indices
            v       = mVP(oi) + v;
            f       = mFP(oj) + f;
            Index i = mV(v);
            if (mPenetratingVertexMask[i])
                return false;
            // Run inexact CCD
            Vector<kDims> XTV        = XT.col(i);
            Vector<kDims> XV         = X.col(i);
            IndexVector<kDims> finds = mF.col(f);
            Matrix<kDims, 3> XTF     = XT(Eigen::placeholders::all, finds);
            Matrix<kDims, 3> XF      = X(Eigen::placeholders::all, finds);
            using namespace math::linalg::mini;
            SVector<Scalar, 4> tbeta = geometry::PointTriangleCcd(
                FromEigen(XTV),
                FromEigen(XTF.col(0)),
                FromEigen(XTF.col(1)),
                FromEigen(XTF.col(2)),
                FromEigen(XV),
                FromEigen(XF.col(0)),
                FromEigen(XF.col(1)),
                FromEigen(XF.col(2)));
            // Report if contact found
            auto const t           = tbeta[0];
            bool const bIntersects = t >= Scalar(0);
            if (bIntersects)
            {
                // Report contact to caller
                fOnVertexTriangleContactPair(VertexTriangleContact{i, f, FromEigen(finds)});
            }
            return bIntersects;
        },
        [](auto, auto, auto) {});
}
 
template <class FOnEdgeEdgeContactPair, class TDerivedXT, class TDerivedX>
inline void MultibodyMeshMixedCcdDcd::ReportEdgeEdgeCcdContacts(
    Eigen::DenseBase<TDerivedXT> const& XT,
    Eigen::DenseBase<TDerivedX> const& X,
    Index oi,
    Index oj,
    FOnEdgeEdgeContactPair&& fOnEdgeEdgeContactPair) const
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.sim.contact.MultibodyMeshMixedCcdDcd.ReportEdgeEdgeCcdContacts");
    mEdgeBvhs[static_cast<std::size_t>(oi)].Overlaps(
        mEdgeBvhs[static_cast<std::size_t>(oj)],
        [&,
         fOnEdgeEdgeContactPair =
             std::forward<FOnEdgeEdgeContactPair>(fOnEdgeEdgeContactPair)](Index ei, Index ej) {
            // Global edge indices
            ei = mEP(oi) + ei;
            ej = mEP(oj) + ej;
            // Global edge vertex indices
            IndexVector<2> eindsi = mE.col(ei);
            IndexVector<2> eindsj = mE.col(ej);
            // Do not report contact if any vertex is already penetrating
            if (mPenetratingVertexMask[eindsi[0]] or mPenetratingVertexMask[eindsi[1]] or
                mPenetratingVertexMask[eindsj[0]] or mPenetratingVertexMask[eindsj[1]])
                return false;
            // Run inexact CCD
            Matrix<kDims, 2> XTei = XT(Eigen::placeholders::all, eindsi);
            Matrix<kDims, 2> Xei  = X(Eigen::placeholders::all, eindsi);
            Matrix<kDims, 2> XTej = XT(Eigen::placeholders::all, eindsj);
            Matrix<kDims, 2> Xej  = X(Eigen::placeholders::all, eindsj);
            using namespace math::linalg::mini;
            SVector<Scalar, 3> tbeta = geometry::EdgeEdgeCcd(
                FromEigen(XTei.col(0)),
                FromEigen(XTei.col(1)),
                FromEigen(XTej.col(0)),
                FromEigen(XTej.col(1)),
                FromEigen(Xei.col(0)),
                FromEigen(Xei.col(1)),
                FromEigen(Xej.col(0)),
                FromEigen(Xej.col(1)));
            // Report if contact found
            auto const t           = tbeta[0];
            bool const bIntersects = t >= Scalar(0);
            if (bIntersects)
            {
                auto uv = tbeta.Slice<2, 1>(1, 0);
                fOnEdgeEdgeContactPair(
                    EdgeEdgeContact{ei, ej, FromEigen(eindsi), FromEigen(eindsj), uv});
            }
            return bIntersects;
        },
        [](auto, auto, auto) {});
}
 
} // namespace pbat::sim::contact
 
#endif // PBAT_SIM_CONTACT_MULTIBODYMESHMIXEDCCDDCD_H