TriangleAabbHierarchy.h

Go to the documentation of this file.

 
#ifndef PBAT_GEOMETRY_TRIANGLEAABBHIERARCHY_H
#define PBAT_GEOMETRY_TRIANGLEAABBHIERARCHY_H
 
#include "AxisAlignedBoundingBox.h"
#include "BoundingVolumeHierarchy.h"
#include "DistanceQueries.h"
#include "OverlapQueries.h"
#include "PhysicsBasedAnimationToolkitExport.h"
#include "pbat/Aliases.h"
#include "pbat/common/Eigen.h"
#include "pbat/math/linalg/mini/Eigen.h"
#include "pbat/profiling/Profiling.h"
 
#include <limits>
#include <tbb/parallel_for.h>
#include <utility>
 
namespace pbat::geometry {

class TriangleAabbHierarchy3D : public BoundingVolumeHierarchy<
                                    TriangleAabbHierarchy3D,
                                    AxisAlignedBoundingBox<3>,
                                    IndexVector<3>,
                                    3>
{
  public:
    static auto constexpr kDims = 3;                       
    using SelfType              = TriangleAabbHierarchy3D; 
    using BaseType              = BoundingVolumeHierarchy<
                     SelfType,
                     AxisAlignedBoundingBox<kDims>,
                     IndexVector<3>,
                     kDims>; 

    PBAT_API TriangleAabbHierarchy3D(
        Eigen::Ref<MatrixX const> const& V,
        Eigen::Ref<IndexMatrixX const> const& C,
        Index maxPointsInLeaf = 10);
    PBAT_API PrimitiveType Primitive(Index p) const;
    PBAT_API Vector<kDims> PrimitiveLocation(PrimitiveType const& primitive) const;
    template <class RPrimitiveIndices>
    BoundingVolumeType BoundingVolumeOf(RPrimitiveIndices&& pinds) const;
    PBAT_API void Update();
    PBAT_API IndexMatrixX OverlappingPrimitives(SelfType const& bvh, std::size_t reserve = 1000ULL) const;
    template <class TDerivedP>
    IndexVectorX PrimitivesContainingPoints(
        Eigen::MatrixBase<TDerivedP> const& P,
        bool bParallelize = true) const;
    template <class TDerivedP, class FCull>
    auto NearestPrimitivesToPoints(
        Eigen::MatrixBase<TDerivedP> const& P,
        FCull fCull,
        bool bParallelize = true) const -> std::pair<IndexVectorX, VectorX>;
    template <class TDerivedP>
    auto
    NearestPrimitivesToPoints(Eigen::MatrixBase<TDerivedP> const& P, bool bParallelize = true) const
        -> std::pair<IndexVectorX, VectorX>;
    [[maybe_unused]] auto const& GetBoundingVolumes() const { return mBoundingVolumes; }
    template <class TDerivedP>
    void SetV(Eigen::MatrixBase<TDerivedP> const& P)
    {
        V = P;
    }

    [[maybe_unused]] auto GetV() const { return V; }
 
    Eigen::Ref<MatrixX const> V;      
    Eigen::Ref<IndexMatrixX const> C; 
};
 
template <class RPrimitiveIndices>
inline TriangleAabbHierarchy3D::BoundingVolumeType
TriangleAabbHierarchy3D::BoundingVolumeOf(RPrimitiveIndices&& pinds) const
{
    auto vertices = C(Eigen::placeholders::all, common::Slice(pinds)).reshaped();
    return BoundingVolumeType(V(Eigen::placeholders::all, vertices));
}
 
template <class TDerivedP>
inline IndexVectorX TriangleAabbHierarchy3D::PrimitivesContainingPoints(
    Eigen::MatrixBase<TDerivedP> const& P,
    bool bParallelize) const
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.geometry.TriangleAabbHierarchy3D.PrimitivesContainingPoints");
    using math::linalg::mini::FromEigen;
    IndexVectorX p(P.cols());
    p.setConstant(-1);
    auto const FindContainingPrimitive = [&](Index i) {
        std::vector<Index> const intersectingPrimitives = this->PrimitivesIntersecting(
            [&](BoundingVolumeType const& bv) -> bool { return bv.contains(P.col(i)); },
            [&](PrimitiveType const& T) -> bool {
                auto const VT  = V(Eigen::placeholders::all, T);
                Scalar const d = DistanceQueries::PointTriangle(
                    FromEigen(P.col(i).template head<kDims>()),
                    FromEigen(VT.col(0).head<kDims>()),
                    FromEigen(VT.col(1).head<kDims>()),
                    FromEigen(VT.col(2).head<kDims>()));
                auto constexpr eps = 1e-15;
                return d < eps;
            });
        if (not intersectingPrimitives.empty())
        {
            p(i) = intersectingPrimitives.front();
        }
    };
    if (bParallelize)
    {
        tbb::parallel_for(Index{0}, Index{P.cols()}, FindContainingPrimitive);
    }
    else
    {
        for (auto i = 0; i < P.cols(); ++i)
            FindContainingPrimitive(i);
    }
    return p;
}
 
template <class TDerivedP, class FCull>
inline auto TriangleAabbHierarchy3D::NearestPrimitivesToPoints(
    Eigen::MatrixBase<TDerivedP> const& P,
    FCull fCull,
    bool bParallelize) const -> std::pair<IndexVectorX, VectorX>
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.geometry.TriangleAabbHierarchy3D.NearestPrimitivesToPoints");
    using math::linalg::mini::FromEigen;
    IndexVectorX p(P.cols());
    p.setConstant(-1);
    VectorX d(P.cols());
    d.setConstant(std::numeric_limits<Scalar>::max());
    auto const FindNearestPrimitive = [&](Index i) {
        std::size_t constexpr K{1};
        auto const [nearestPrimitives, distances] = this->NearestPrimitivesTo(
            [&](BoundingVolumeType const& bv) -> Scalar {
                return bv.squaredExteriorDistance(P.col(i));
            },
            [&](PrimitiveType const& T) -> Scalar {
                if (fCull(i, T))
                {
                    return std::numeric_limits<Scalar>::max();
                }
                auto const VT = V(Eigen::placeholders::all, T);
                return DistanceQueries::PointTriangle(
                    FromEigen(P.col(i).template head<kDims>()),
                    FromEigen(VT.col(0).head<kDims>()),
                    FromEigen(VT.col(1).head<kDims>()),
                    FromEigen(VT.col(2).head<kDims>()));
            },
            K);
        p(i) = nearestPrimitives.front();
        d(i) = distances.front();
    };
    if (bParallelize)
    {
        tbb::parallel_for(Index{0}, Index{P.cols()}, FindNearestPrimitive);
    }
    else
    {
        for (auto i = 0; i < P.cols(); ++i)
            FindNearestPrimitive(i);
    }
    return {p, d};
}
 
template <class TDerivedP>
inline auto TriangleAabbHierarchy3D::NearestPrimitivesToPoints(
    Eigen::MatrixBase<TDerivedP> const& P,
    bool bParallelize) const -> std::pair<IndexVectorX, VectorX>
{
    return NearestPrimitivesToPoints(P, [](auto, auto) { return false; }, bParallelize);
}

class TriangleAabbHierarchy2D : public BoundingVolumeHierarchy<
                                    TriangleAabbHierarchy2D,
                                    AxisAlignedBoundingBox<2>,
                                    IndexVector<3>,
                                    2>
{
  public:
    static auto constexpr kDims = 2;                       
    using SelfType              = TriangleAabbHierarchy2D; 
    using BaseType              = BoundingVolumeHierarchy<
                     SelfType,
                     AxisAlignedBoundingBox<kDims>,
                     IndexVector<3>,
                     kDims>; 

    PBAT_API TriangleAabbHierarchy2D(
        Eigen::Ref<MatrixX const> const& V,
        Eigen::Ref<IndexMatrixX const> const& C,
        Index maxPointsInLeaf = 10);
    PBAT_API PrimitiveType Primitive(Index p) const;
    PBAT_API Vector<kDims> PrimitiveLocation(PrimitiveType const& primitive) const;
    template <class RPrimitiveIndices>
    BoundingVolumeType BoundingVolumeOf(RPrimitiveIndices&& pinds) const;
    PBAT_API void Update();
    PBAT_API IndexMatrixX OverlappingPrimitives(SelfType const& bvh, std::size_t reserve = 1000ULL) const;
    template <class TDerivedP>
    IndexVectorX PrimitivesContainingPoints(
        Eigen::MatrixBase<TDerivedP> const& P,
        bool bParallelize = true) const;
    template <class TDerivedP>
    auto
    NearestPrimitivesToPoints(Eigen::MatrixBase<TDerivedP> const& P, bool bParallelize = true) const
        -> std::pair<IndexVectorX, VectorX>;
    [[maybe_unused]] auto const& GetBoundingVolumes() const { return mBoundingVolumes; }
    template <class TDerivedP>
    void SetV(Eigen::MatrixBase<TDerivedP> const& P)
    {
        V = P;
    }

    [[maybe_unused]] auto GetV() const { return V; }
 
    Eigen::Ref<MatrixX const> V;      
    Eigen::Ref<IndexMatrixX const> C; 
};
 
template <class RPrimitiveIndices>
inline TriangleAabbHierarchy2D::BoundingVolumeType
TriangleAabbHierarchy2D::BoundingVolumeOf(RPrimitiveIndices&& pinds) const
{
    auto vertices = C(Eigen::placeholders::all, common::Slice(pinds)).reshaped();
    return BoundingVolumeType(V(Eigen::placeholders::all, vertices));
}
 
template <class TDerivedP>
inline IndexVectorX TriangleAabbHierarchy2D::PrimitivesContainingPoints(
    Eigen::MatrixBase<TDerivedP> const& P,
    bool bParallelize) const
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.geometry.TriangleAabbHierarchy2D.PrimitivesContainingPoints");
    using math::linalg::mini::FromEigen;
    IndexVectorX p(P.cols());
    p.setConstant(-1);
    auto const FindContainingPrimitive = [&](Index i) {
        std::vector<Index> const intersectingPrimitives = this->PrimitivesIntersecting(
            [&](BoundingVolumeType const& bv) -> bool { return bv.contains(P.col(i)); },
            [&](PrimitiveType const& T) -> bool {
                auto const VT = V(Eigen::placeholders::all, T);
                return OverlapQueries::PointTriangle(
                    FromEigen(P.col(i).template head<kDims>()),
                    FromEigen(VT.col(0).head<kDims>()),
                    FromEigen(VT.col(1).head<kDims>()),
                    FromEigen(VT.col(2).head<kDims>()));
            });
        if (not intersectingPrimitives.empty())
        {
            p(i) = intersectingPrimitives.front();
        }
    };
    if (bParallelize)
    {
        tbb::parallel_for(Index{0}, Index{P.cols()}, FindContainingPrimitive);
    }
    else
    {
        for (auto i = 0; i < P.cols(); ++i)
            FindContainingPrimitive(i);
    }
    return p;
}
 
template <class TDerivedP>
inline auto TriangleAabbHierarchy2D::NearestPrimitivesToPoints(
    Eigen::MatrixBase<TDerivedP> const& P,
    bool bParallelize) const -> std::pair<IndexVectorX, VectorX>
{
    PBAT_PROFILE_NAMED_SCOPE("pbat.geometry.TriangleAabbHierarchy2D.NearestPrimitivesToPoints");
    using math::linalg::mini::FromEigen;
    IndexVectorX p(P.cols());
    p.setConstant(-1);
    VectorX d(P.cols());
    d.setConstant(std::numeric_limits<Scalar>::max());
    auto const FindNearestPrimitive = [&](Index i) {
        std::size_t constexpr K{1};
        auto const [nearestPrimitives, distances] = this->NearestPrimitivesTo(
            [&](BoundingVolumeType const& bv) -> Scalar {
                return bv.squaredExteriorDistance(P.col(i));
            },
            [&](PrimitiveType const& T) -> Scalar {
                auto const VT = V(Eigen::placeholders::all, T);
                return DistanceQueries::PointTriangle(
                    FromEigen(P.col(i).template head<kDims>()),
                    FromEigen(VT.col(0).head<kDims>()),
                    FromEigen(VT.col(1).head<kDims>()),
                    FromEigen(VT.col(2).head<kDims>()));
            },
            K);
        p(i) = nearestPrimitives.front();
        d(i) = distances.front();
    };
    if (bParallelize)
    {
        tbb::parallel_for(Index{0}, Index{P.cols()}, FindNearestPrimitive);
    }
    else
    {
        for (auto i = 0; i < P.cols(); ++i)
            FindNearestPrimitive(i);
    }
    return std::make_pair(p, d);
}
 
} // namespace pbat::geometry
 
#endif // PBAT_GEOMETRY_TRIANGLEAABBHIERARCHY_H