ClosestPointQueries.h

Go to the documentation of this file.

 
#ifndef PBAT_GEOMETRY_CLOSESTPOINTQUERIES_H
#define PBAT_GEOMETRY_CLOSESTPOINTQUERIES_H
 
#include "pbat/HostDevice.h"
#include "pbat/math/linalg/mini/Mini.h"
 
#include <algorithm>
#include <cassert>
 
namespace pbat {
namespace geometry {
namespace ClosestPointQueries {
 
namespace mini = math::linalg::mini;

template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixN>
PBAT_HOST_DEVICE auto PointOnPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>;

template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ>
PBAT_HOST_DEVICE auto PointOnLineSegment(TMatrixX const& X, TMatrixP const& P, TMatrixQ const& Q)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>;

template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
PBAT_HOST_DEVICE auto
PointOnAxisAlignedBoundingBox(TMatrixX const& X, TMatrixL const& L, TMatrixU const& U)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>;

template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC>
PBAT_HOST_DEVICE auto
UvwPointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
    -> mini::SVector<typename TMatrixP::ScalarType, 3>;

template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC>
PBAT_HOST_DEVICE auto
PointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
    -> mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>;

template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC,
    mini::CMatrix TMatrixD>
PBAT_HOST_DEVICE auto PointInTetrahedron(
    TMatrixP const& P,
    TMatrixA const& A,
    TMatrixB const& B,
    TMatrixC const& C,
    TMatrixD const& D) -> mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>;

template <
    mini::CMatrix TMatrixP1,
    mini::CMatrix TMatrixQ1,
    mini::CMatrix TMatrixP2,
    mini::CMatrix TMatrixQ2,
    class TScalar = typename TMatrixP1::ScalarType>
PBAT_HOST_DEVICE auto Lines(
    TMatrixP1 const& P1,
    TMatrixQ1 const& Q1,
    TMatrixP2 const& P2,
    TMatrixQ2 const& Q2,
    TScalar eps = std::numeric_limits<TScalar>::min()) -> mini::SVector<TScalar, 2>;
 
template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixN>
PBAT_HOST_DEVICE auto PointOnPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>
{
    using namespace std;
    using ScalarType = typename TMatrixX::ScalarType;
#ifndef NDEBUG
    bool const bIsNormalUnit = abs(SquaredNorm(n) - ScalarType(1)) <= ScalarType(1e-15);
    assert(bIsNormalUnit);
#endif
    ScalarType const t                                = Dot(n, X - P);
    mini::SVector<ScalarType, TMatrixX::kRows> Xplane = X - t * n;
    return Xplane;
}
 
template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ>
PBAT_HOST_DEVICE auto PointOnLineSegment(TMatrixX const& X, TMatrixP const& P, TMatrixQ const& Q)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>
{
    using ScalarType = typename TMatrixX::ScalarType;
    using namespace std;
    mini::SVector<ScalarType, TMatrixX::kRows> const PQ = Q - P;
    // Project X onto PQ, computing parameterized position R(t) = P + t*(Q � P)
    ScalarType t = Dot(X - P, PQ) / SquaredNorm(PQ);
    // If outside segment, clamp t (and therefore d) to the closest endpoint
    t = min(max(t, ScalarType(0)), ScalarType(1));
    // Compute projected position from the clamped t
    auto const Xpq = P + t * PQ;
    return Xpq;
}
 
template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
PBAT_HOST_DEVICE auto
PointOnAxisAlignedBoundingBox(TMatrixX const& P, TMatrixL const& L, TMatrixU const& U)
    -> mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows>
{
    using namespace std;
    mini::SVector<typename TMatrixX::ScalarType, TMatrixX::kRows> X = P;
    pbat::common::ForRange<0, TMatrixX::kRows>(
        [&]<auto i>() { X(i) = min(max(X(i), L(i)), U(i)); });
    return X;
}
 
template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC>
PBAT_HOST_DEVICE auto
UvwPointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
    -> mini::SVector<typename TMatrixP::ScalarType, 3>
{
    using ScalarType = typename TMatrixP::ScalarType;
 
    // Check if P in vertex region outside A
    auto constexpr kRows                      = TMatrixP::kRows;
    mini::SVector<ScalarType, kRows> const AB = B - A;
    mini::SVector<ScalarType, kRows> const AC = C - A;
    mini::SVector<ScalarType, kRows> const AP = P - A;
    ScalarType const d1                       = Dot(AB, AP);
    ScalarType const d2                       = Dot(AC, AP);
    if (d1 <= ScalarType(0) and d2 <= ScalarType(0))
    {
        return mini::Unit<ScalarType, 3>(0); // barycentric coordinates (1,0,0)
    }
 
    // Check if P in vertex region outside B
    mini::SVector<ScalarType, kRows> const BP = P - B;
    ScalarType const d3                       = Dot(AB, BP);
    ScalarType const d4                       = Dot(AC, BP);
    if (d3 >= ScalarType(0) and d4 <= d3)
    {
        return mini::Unit<ScalarType, 3>(1); // barycentric coordinates (0,1,0)
    }
 
    // Check if P in edge region of AB, if so return projection of P onto AB
    ScalarType const vc = d1 * d4 - d3 * d2;
    if (vc <= ScalarType(0) and d1 >= ScalarType(0) and d3 <= ScalarType(0))
    {
        ScalarType const v = d1 / (d1 - d3);
        return mini::SVector<ScalarType, 3>{
            ScalarType(1) - v,
            v,
            ScalarType(0)}; // barycentric coordinates (1-v,v,0)
    }
 
    // Check if P in vertex region outside C
    mini::SVector<ScalarType, kRows> const CP = P - C;
    ScalarType const d5                       = Dot(AB, CP);
    ScalarType const d6                       = Dot(AC, CP);
    if (d6 >= ScalarType(0) and d5 <= d6)
    {
        return mini::Unit<ScalarType, 3>(2); // barycentric coordinates (0,0,1)
    }
 
    // Check if P in edge region of AC, if so return projection of P onto AC
    ScalarType const vb = d5 * d2 - d1 * d6;
    if (vb <= ScalarType(0) and d2 >= ScalarType(0) and d6 <= ScalarType(0))
    {
        ScalarType const w = d2 / (d2 - d6);
        return mini::SVector<ScalarType, 3>{
            ScalarType(1) - w,
            ScalarType(0),
            w}; // barycentric coordinates (1-w,0,w)
    }
    // Check if P in edge region of BC, if so return projection of P onto BC
    ScalarType const va = d3 * d6 - d5 * d4;
    if (va <= ScalarType(0) and (d4 - d3) >= ScalarType(0) and (d5 - d6) >= ScalarType(0))
    {
        ScalarType const w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        return mini::SVector<ScalarType, 3>{
            ScalarType(0),
            ScalarType(1) - w,
            w}; // barycentric coordinates (0,1-w,w)
    }
    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    ScalarType const denom = ScalarType(1) / (va + vb + vc);
    ScalarType const v     = vb * denom;
    ScalarType const w     = vc * denom;
    return mini::SVector<ScalarType, 3>{
        ScalarType(1) - v - w,
        v,
        w}; // = u*a + v*b + w*c, u = va * denom = ScalarType(1)0f-v-w
}
 
template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC>
PBAT_HOST_DEVICE auto
PointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
    -> mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>
{
    auto uvw = UvwPointInTriangle(P, A, B, C);
    return A * uvw(0) + B * uvw(1) + C * uvw(2);
}
 
template <
    mini::CMatrix TMatrixP,
    mini::CMatrix TMatrixA,
    mini::CMatrix TMatrixB,
    mini::CMatrix TMatrixC,
    mini::CMatrix TMatrixD>
PBAT_HOST_DEVICE auto PointInTetrahedron(
    TMatrixP const& P,
    TMatrixA const& A,
    TMatrixB const& B,
    TMatrixC const& C,
    TMatrixD const& D) -> mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>
{
    using ScalarType     = typename TMatrixP::ScalarType;
    auto constexpr kRows = TMatrixP::kRows;
    auto constexpr kDims = 3;
    static_assert(kRows == kDims, "This overlap test is specialized for 3D");

 
    // Start out assuming point inside all halfspaces, so closest to itself
    mini::SVector<ScalarType, kDims> X = P;
    ScalarType d2min                   = std::numeric_limits<ScalarType>::max();
 
    auto const PointOutsidePlane = [](auto const& p, auto const& a, auto const& b, auto const& c) {
        ScalarType const d = Dot(p - a, Cross(b - a, c - a));
        return d > ScalarType(0);
    };
    auto const TestFace = [&](auto const& a, auto const& b, auto const& c) {
        // If point outside face abc then compute closest point on abc
        if (PointOutsidePlane(P, a, b, c))
        {
            auto const Q        = PointInTriangle(P, a, b, c);
            ScalarType const d2 = SquaredNorm(Q - P);
            // Update best closest point if (squared) distance is less than current best
            if (d2 < d2min)
            {
                d2min = d2;
                X     = Q;
            }
        }
    };
    TestFace(A, B, D);
    TestFace(B, C, D);
    TestFace(C, A, D);
    TestFace(A, C, B);
    return X;
}
 
template <
    mini::CMatrix TMatrixP1,
    mini::CMatrix TMatrixQ1,
    mini::CMatrix TMatrixP2,
    mini::CMatrix TMatrixQ2,
    class TScalar>
PBAT_HOST_DEVICE auto Lines(
    TMatrixP1 const& P1,
    TMatrixQ1 const& Q1,
    TMatrixP2 const& P2,
    TMatrixQ2 const& Q2,
    TScalar eps) -> mini::SVector<TScalar, 2>
{
    auto constexpr kDims                     = TMatrixP1::kRows;
    mini::SVector<TScalar, kDims> const d1   = Q1 - P1;
    mini::SVector<TScalar, kDims> const d2   = Q2 - P2;
    mini::SVector<TScalar, kDims> const P2P1 = P1 - P2;
    TScalar const d1sq                       = SquaredNorm(d1);
    TScalar const d2sq                       = SquaredNorm(d2);
    TScalar const d1d2                       = Dot(d1, d2);
    TScalar const similarity                 = d1sq * d2sq - d1d2 * d1d2;
    bool const bIsLine1Degenerate            = d1sq <= eps;
    bool const bIsLine2Degenerate            = d2sq <= eps;
    bool const bAreParallel                  = (-eps <= similarity) and (similarity <= eps);
    TScalar alpha{0}, beta{0};
    if (not(bIsLine1Degenerate and bIsLine2Degenerate))
    {
        if (bIsLine1Degenerate)
        {
            beta = Dot(P2P1, d2) / d2sq;
        }
        else if (bIsLine2Degenerate)
        {
            alpha = -Dot(P2P1, d1) / d1sq;
        }
        else
        {
            if (not bAreParallel)
                alpha = Dot(P2P1, d2 * d1d2 - d2sq * d1) / similarity;
            beta = Dot(P2P1 + alpha * d1, d2) / d2sq;
        }
    }
    return mini::SVector<TScalar, 2>{alpha, beta};
}
 
} // namespace ClosestPointQueries
} // namespace geometry
} // namespace pbat
 
#endif // PBAT_GEOMETRY_CLOSESTPOINTQUERIES_H