VertexTriangleMixedCcdDcd.cuh

#ifndef PBAT_GPU_IMPL_CONTACT_VERTEXTRIANGLEMIXEDCCDDCD_H
#define PBAT_GPU_IMPL_CONTACT_VERTEXTRIANGLEMIXEDCCDDCD_H
 
#include "pbat/geometry/DistanceQueries.h"
#include "pbat/gpu/Aliases.h"
#include "pbat/gpu/impl/common/Buffer.cuh"
#include "pbat/gpu/impl/geometry/Aabb.cuh"
#include "pbat/gpu/impl/geometry/Bvh.cuh"
#include "pbat/gpu/impl/geometry/Morton.cuh"
#include "pbat/math/linalg/mini/Mini.h"
 
namespace pbat::gpu::impl::contact {

class VertexTriangleMixedCcdDcd
{
  public:
    static auto constexpr kDims = 3; 
    static auto constexpr kMaxNeighbours =
        8; 

    VertexTriangleMixedCcdDcd(
        Eigen::Ref<GpuIndexVectorX const> const& B,
        Eigen::Ref<GpuIndexVectorX const> const& V,
        Eigen::Ref<GpuIndexMatrixX const> const& F);
    void InitializeActiveSet(
        common::Buffer<GpuScalar, 3> const& xt,
        common::Buffer<GpuScalar, 3> const& xtp1,
        geometry::Morton::Bound const& wmin,
        geometry::Morton::Bound const& wmax);
    void UpdateActiveSet(common::Buffer<GpuScalar, 3> const& x, bool bComputeBoxes = true);
    void FinalizeActiveSet(common::Buffer<GpuScalar, 3> const& x, bool bComputeBoxes = true);
    template <class FOnNearestNeighbourFound>
    void ForEachNearestNeighbour(
        common::Buffer<GpuScalar, 3> const& x,
        FOnNearestNeighbourFound fOnNearestNeighbourFound);
    void UpdateBvh(common::Buffer<GpuScalar, 3> const& x);
 
  public:
    common::Buffer<GpuIndex> av; 
    GpuIndex nActive;            
    common::Buffer<GpuIndex> nn; 
    common::Buffer<GpuIndex> B;    
    common::Buffer<GpuIndex> V;    
    common::Buffer<GpuIndex, 3> F; 
 
    common::Buffer<GpuIndex> inds; 
    geometry::Morton morton;       
    geometry::Aabb<kDims>
        Paabbs; 
    geometry::Aabb<kDims>
        Faabbs;         
    geometry::Bvh Fbvh; 
    common::Buffer<bool> active;      
    common::Buffer<GpuScalar> dupper; 
    GpuScalar eps;                    
};
 
template <class FOnNearestNeighbourFound>
inline void VertexTriangleMixedCcdDcd::ForEachNearestNeighbour(
    common::Buffer<GpuScalar, 3> const& x,
    FOnNearestNeighbourFound fOnNearestNeighbourFound)
{
    using namespace pbat::math::linalg::mini;
    auto fGetQueryObject = [x = x.Raw(), V = V.Raw(), av = av.Raw()] PBAT_DEVICE(GpuIndex q) {
        GpuIndex const v = av[q];
        return FromBuffers<3, 1>(x, V[v]);
    };
    auto fMinDistanceToBox = [] PBAT_DEVICE(
                                 SVector<GpuScalar, 3> const& xi,
                                 SVector<GpuScalar, 3> const& L,
                                 SVector<GpuScalar, 3> const& U) {
        return pbat::geometry::DistanceQueries::PointAxisAlignedBoundingBox(xi, L, U);
    };
    auto fDistanceToLeaf =
        [x = x.Raw(), V = V.Raw(), F = F.Raw(), B = B.Raw(), av = av.Raw()] PBAT_DEVICE(
            GpuIndex q,
            SVector<GpuScalar, 3> const& xi,
            [[maybe_unused]] GpuIndex leaf,
            GpuIndex f) {
            GpuIndex const i         = V[av[q]];
            auto fv                  = FromBuffers<3, 1>(F, f);
            bool const bFromSameBody = B[i] == B[fv[0]];
            if (bFromSameBody)
                return std::numeric_limits<GpuScalar>::max();
 
            SMatrix<GpuScalar, 3, 3> const xf = FromBuffers(x, fv.Transpose());
            return pbat::geometry::DistanceQueries::PointTriangle(
                xi,
                xf.Col(0),
                xf.Col(1),
                xf.Col(2));
        };
    auto fDistanceUpperBound = [dupper = dupper.Raw(), av = av.Raw()] PBAT_DEVICE(GpuIndex q) {
        // Try warm-starting the NN search!
        return dupper[av[q]];
    };
    Fbvh.NearestNeighbours<
        decltype(fGetQueryObject),
        decltype(fMinDistanceToBox),
        decltype(fDistanceToLeaf),
        decltype(fDistanceUpperBound),
        decltype(fOnNearestNeighbourFound),
        kMaxNeighbours>(
        Faabbs,
        nActive,
        fGetQueryObject,
        fMinDistanceToBox,
        fDistanceToLeaf,
        fDistanceUpperBound,
        fOnNearestNeighbourFound,
        eps);
}
 
} // namespace pbat::gpu::impl::contact
 
#endif // PBAT_GPU_IMPL_CONTACT_VERTEXTRIANGLEMIXEDCCDDCD_H