Primitive.h

Go to the documentation of this file.

#ifndef PBAT_GEOMETRY_SDF_PRIMITIVE_H
#define PBAT_GEOMETRY_SDF_PRIMITIVE_H
 
#include "TypeDefs.h"
#include "pbat/HostDevice.h"
#include "pbat/common/Concepts.h"
#include "pbat/math/linalg/mini/Mini.h"
 
#include <algorithm>
#include <cmath>
 
namespace pbat::geometry::sdf {
template <common::CArithmetic TScalar>
TScalar sign(TScalar x)
{
    return static_cast<TScalar>(x > 0) - static_cast<TScalar>(x < 0);
};

struct Primitive
{
};

template <common::CArithmetic TScalar>
struct Sphere : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType R{1};            
    Sphere() = default;
    explicit Sphere(ScalarType R_)
        : R(R_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const { return Norm(p) - R; }
};

template <common::CArithmetic TScalar>
struct Box : public Primitive
{
    using ScalarType = TScalar; 
    Vec3<ScalarType> he{
        TScalar(0.5),
        TScalar(0.5),
        TScalar(0.5)}; 

    Box() = default;
    explicit Box(Vec3<ScalarType> const& he_)
        : he(he_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        Vec3<ScalarType> q = Abs(p) - he;
        Zero3<ScalarType> constexpr zero3{};
        using namespace std;
        return Norm(Max(q, zero3)) + min(max(q(0), max(q(1), q(2))), ScalarType(0));
    }
};

template <common::CArithmetic TScalar>
struct BoxFrame : public Primitive
{
    using ScalarType = TScalar; 
    Vec3<ScalarType> he{
        TScalar(0.5),
        TScalar(0.5),
        TScalar(0.5)};          
    ScalarType t{TScalar(0.1)}; 
    BoxFrame() = default;
    explicit BoxFrame(Vec3<ScalarType> const& he_, ScalarType t_)
        : he(he_),
          t(t_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        p                  = Abs(p) - he;
        Vec3<ScalarType> q = Abs(p + t) - t;
        Zero3<ScalarType> constexpr zero3{};
        ScalarType constexpr zero{0};
        using namespace std;
        // clang-format off
        return min(
            min(
                Norm(Max(Vec3<ScalarType>{p(0),q(1),q(2)},zero3)) + min(max(p(0),max(q(1),q(2))), zero),
                Norm(Max(Vec3<ScalarType>{q(0),p(1),q(2)},zero3)) + min(max(q(0),max(p(1),q(2))), zero)
            ),
            Norm(Max(Vec3<ScalarType>{q(0),q(1),p(2)},zero3)) + min(max(q(0),max(q(1),p(2))), zero)
        );
        // clang-format on
    }
};

template <common::CArithmetic TScalar>
struct Torus : public Primitive
{
    using ScalarType = TScalar;                   
    Vec2<ScalarType> t{TScalar(1), TScalar(0.5)}; 
    Torus() = default;
    explicit Torus(Vec2<ScalarType> const& t_)
        : t(t_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        Vec2<ScalarType> q = Vec2<ScalarType>{Norm(Vec2<ScalarType>{p(0), p(2)}) - t(0), p(1)};
        return Norm(q) - t(1);
    }
};

template <common::CArithmetic TScalar>
struct CappedTorus : public Primitive
{
    using ScalarType = TScalar;                           
    Vec2<ScalarType> sc{TScalar(0.833), TScalar(-0.545)}; 
    ScalarType ra{1.0};                                   
    ScalarType rb{0.2};                                   
    CappedTorus() = default;
    explicit CappedTorus(Vec2<ScalarType> const& sc_, ScalarType ra_, ScalarType rb_)
        : sc(sc_),
          ra(ra_),
          rb(rb_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        using namespace std;
        p(0)          = abs(p(0));
        bool const bk = sc(1) * p(0) > sc(0) * p(1);
        auto pxy      = p.template Slice<2, 1>(0, 0);
        // NOTE: Not sure if better to do branchless but compute a norm (i.e. expensive sqrt), or
        // use ternary operator and add divergent branching, but save the sqrt when possible.
        ScalarType k = bk * (Dot(pxy, sc)) + (not bk) * Norm(pxy);
        return sqrt(SquaredNorm(p) + ra * ra - ScalarType(2) * ra * k) - rb;
    }
};

template <common::CArithmetic TScalar>
struct Link : public Primitive
{
    using ScalarType = TScalar;                   
    Vec2<ScalarType> t{TScalar(1), TScalar(0.5)}; 
    ScalarType le{TScalar(1)};                    
    Link() = default;
    explicit Link(Vec2<ScalarType> const& t_, ScalarType le_)
        : t(t_),
          le(le_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType constexpr zero{0};
        Vec3<ScalarType> q = Vec3<ScalarType>{p(0), max(abs(p(1)) - le, zero), p(2)};
        auto qxy           = q.template Slice<2, 1>(0, 0);
        return Norm(Vec2<ScalarType>{Norm(qxy) - t(0), q(2)}) - t(1);
    }
};

template <common::CArithmetic TScalar>
struct InfiniteCylinder : public Primitive
{
    using ScalarType = TScalar; 
    Vec3<ScalarType> c{
        TScalar(0),
        TScalar(0),
        TScalar(0.5)}; 

    InfiniteCylinder() = default;
    explicit InfiniteCylinder(Vec3<ScalarType> const& c_)
        : c(c_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        Vec2<ScalarType> d{p(0) - c(0), p(2) - c(1)};
        return Norm(d) - c(2);
    }
};

template <common::CArithmetic TScalar>
struct Cone : public Primitive
{
    using ScalarType = TScalar;                      
    Vec2<ScalarType> sc{TScalar(0.5), TScalar(0.5)}; 
    ScalarType h{TScalar(1)};                        
    Cone() = default;
    explicit Cone(Vec2<ScalarType> const& sc_, ScalarType h_)
        : sc(sc_),
          h(h_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
        Vec2<ScalarType> q = h * Vec2<ScalarType>{sc(0) / sc(1), ScalarType(-1)};
        Vec2<ScalarType> w{Norm(Vec2<ScalarType>{p(0), p(2)}), p(1)};
        Vec2<ScalarType> a = w - q * clamp(Dot(w, q) / Dot(q, q), zero, one);
        Vec2<ScalarType> b = w - q * Vec2<ScalarType>{clamp(w(0) / q(0), zero, one), one};
        ScalarType k       = sign(q(1));
        ScalarType d       = min(Dot(a, a), Dot(b, b));
        ScalarType s       = max(k * (w(0) * q(1) - w(1) * q(0)), k * (w(1) - q(1)));
        return sqrt(d) * sign(s);
    }
};

template <common::CArithmetic TScalar>
struct InfiniteCone : public Primitive
{
    using ScalarType = TScalar;                      
    Vec2<ScalarType> sc{TScalar(0.5), TScalar(0.5)}; 
    InfiniteCone() = default;
    explicit InfiniteCone(Vec2<ScalarType> const& sc_)
        : sc(sc_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
        Vec2<ScalarType> q = Vec2<ScalarType>{Norm(Vec2<ScalarType>{p(0), p(2)}), -p(1)};
        ScalarType d       = Norm(q - max(Dot(q, sc), zero) * sc);
        bool bd            = (q(0) * sc(1) - q(1) * sc(0) < zero);
        return d * (bd * (-one) + (not bd) * one);
    }
};

template <common::CArithmetic TScalar>
struct Plane : public Primitive
{
    using ScalarType = TScalar; 
    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        // n^T (p - o)
        return p(2);
    }
};

template <common::CArithmetic TScalar>
struct HexagonalPrism : public Primitive
{
    using ScalarType = TScalar; 
    Vec2<ScalarType> h{
        TScalar(1),
        TScalar(1)}; 

    HexagonalPrism() = default;
    explicit HexagonalPrism(Vec2<ScalarType> const& h_)
        : h(h_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        using namespace std;
        Vec3<ScalarType> const k{ScalarType{-0.8660254}, ScalarType{0.5}, ScalarType{0.57735}};
        p        = Abs(p);
        auto pxy = p.template Slice<2, 1>(0, 0);
        auto kxy = k.template Slice<2, 1>(0, 0);
        ScalarType constexpr zero{0};
        pxy -= ScalarType(2) * min(Dot(kxy, pxy), zero) * kxy;
        Vec2<ScalarType> d = Vec2<ScalarType>{
            Norm(pxy - Vec2<ScalarType>{clamp(p(0), -k(2) * h(0), k(2) * h(0)), h(0)}) *
            sign(p(1) - h(0)),
            p(2) - h(1)};
        return min(max(d(0), d(1)), zero) + Norm(Max(d, Zero2<ScalarType>{}));
    }
};

template <common::CArithmetic TScalar>
struct Capsule : public Primitive
{
    using ScalarType = TScalar;                              
    Vec3<ScalarType> a{TScalar(-1), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> b{TScalar(1), TScalar(0), TScalar(0)};  
    ScalarType r{TScalar(0.2)};                              
    Capsule() = default;
    explicit Capsule(Vec3<ScalarType> const& a_, Vec3<ScalarType> const& b_, ScalarType r_)
        : a(a_),
          b(b_),
          r(r_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec3<ScalarType> pa = p - a;
        Vec3<ScalarType> ba = b - a;
        ScalarType h        = clamp(Dot(pa, ba) / Dot(ba, ba), ScalarType(0), ScalarType(1));
        return Norm(pa - h * ba) - r;
    }
};

template <common::CArithmetic TScalar>
struct VerticalCapsule : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType h{TScalar(1)};   
    ScalarType r{TScalar(0.2)}; 
    VerticalCapsule() = default;
    explicit VerticalCapsule(ScalarType h_, ScalarType r_)
        : h(h_),
          r(r_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        using namespace std;
        p(1) -= clamp(p(1), ScalarType(0), h);
        return Norm(p) - r;
    }
};

template <common::CArithmetic TScalar>
struct CappedCylinder : public Primitive
{
    using ScalarType = TScalar;                              
    Vec3<ScalarType> a{TScalar(-1), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> b{TScalar(1), TScalar(0), TScalar(0)};  
    ScalarType r{TScalar(0.2)};                              
    CappedCylinder() = default;
    explicit CappedCylinder(Vec3<ScalarType> const& a_, Vec3<ScalarType> const& b_, ScalarType r_)
        : a(a_),
          b(b_),
          r(r_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec3<ScalarType> ba = b - a;
        Vec3<ScalarType> pa = p - a;
        ScalarType baba     = Dot(ba, ba);
        ScalarType paba     = Dot(pa, ba);
        ScalarType x        = Norm(baba * pa - paba * ba) - r * baba;
        ScalarType y        = abs(paba - baba * ScalarType(0.5)) - baba * ScalarType(0.5);
        ScalarType x2       = x * x;
        ScalarType y2       = y * y * baba;
        ScalarType constexpr zero{0};
        ScalarType d = (max(x, y) < zero) ?
                           -min(x2, y2) :
                           (((x > zero) ? x2 : zero) + ((y > zero) ? y2 : zero));
        return sign(d) * sqrt(abs(d)) / baba;
    }
};

template <common::CArithmetic TScalar>
struct VerticalCappedCylinder : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType h{TScalar(1)};   
    ScalarType r{TScalar(0.2)}; 
    VerticalCappedCylinder() = default;
    explicit VerticalCappedCylinder(ScalarType h_, ScalarType r_)
        : h(h_),
          r(r_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec2<ScalarType> pxz{p(0), p(2)};
        Vec2<ScalarType> d = Abs(Vec2<ScalarType>{Norm(pxz), p(1)}) - Vec2<ScalarType>{r, h};
        ScalarType constexpr zero{0};
        return min(max(d(0), d(1)), zero) + Norm(Max(d, Zero2<ScalarType>{}));
    }
};

template <common::CArithmetic TScalar>
struct RoundedCylinder : public Primitive
{
    using ScalarType = TScalar;   
    ScalarType h{TScalar(1)};     
    ScalarType ra{TScalar(0.2)};  
    ScalarType rb{TScalar(0.05)}; 
    RoundedCylinder() = default;
    explicit RoundedCylinder(ScalarType h_, ScalarType ra_, ScalarType rb_)
        : h(h_),
          ra(ra_),
          rb(rb_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec2<ScalarType> pxz{p(0), p(2)};
        Vec2<ScalarType> d{Norm(pxz) - ScalarType(2) * ra + rb, abs(p(1)) - h};
        ScalarType constexpr zero{0};
        Zero2<ScalarType> constexpr zero2{};
        return min(max(d(0), d(1)), zero) + Norm(Max(d, zero2)) - rb;
    }
};

template <common::CArithmetic TScalar>
struct VerticalCappedCone : public Primitive
{
    using ScalarType = TScalar;  
    ScalarType h{TScalar(1)};    
    ScalarType r1{TScalar(0.2)}; 
    ScalarType r2{TScalar(0.4)}; 
    VerticalCappedCone() = default;
    explicit VerticalCappedCone(ScalarType h_, ScalarType r1_, ScalarType r2_)
        : h(h_),
          r1(r1_),
          r2(r2_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
        Vec2<ScalarType> pxz{p(0), p(2)};
        Vec2<ScalarType> q{Norm(pxz), p(1)};
        Vec2<ScalarType> k1{r2, h};
        Vec2<ScalarType> k2{r2 - r1, ScalarType(2) * h};
        bool const brqy = (q(1) < zero);
        ScalarType rqy  = brqy * r1 + (not brqy) * r2;
        Vec2<ScalarType> ca{q(0) - min(q(0), rqy), abs(q(1)) - h};
        Vec2<ScalarType> cb = q - k1 + k2 * clamp(Dot(k1 - q, k2) / SquaredNorm(k2), zero, one);
        bool const bs       = (cb(0) < zero and ca(1) < zero);
        ScalarType s        = bs * (-one) + (not bs) * one;
        return s * sqrt(min(SquaredNorm(ca), SquaredNorm(cb)));
    }
};

template <common::CArithmetic TScalar>
struct CutHollowSphere : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType r{TScalar(1)};   
    ScalarType h{TScalar(0.5)}; 
    ScalarType t{TScalar(0.1)}; 
    CutHollowSphere() = default;
    explicit CutHollowSphere(ScalarType r_, ScalarType h_, ScalarType t_)
        : r(r_),
          h(h_),
          t(t_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType w = sqrt(r * r - h * h);
        Vec2<ScalarType> pxz{p(0), p(2)};
        Vec2<ScalarType> q{Norm(pxz), p(1)};
        bool b = (h * q(0) < w * q(1));
        return b * (Norm(q - Vec2<ScalarType>{w, h})) + (not b) * (abs(Norm(q) - r) - t);
    }
};

template <common::CArithmetic TScalar>
struct VerticalRoundCone : public Primitive
{
    using ScalarType = TScalar;  
    ScalarType h{TScalar(1)};    
    ScalarType r1{TScalar(0.2)}; 
    ScalarType r2{TScalar(0.4)}; 
    VerticalRoundCone() = default;
    explicit VerticalRoundCone(ScalarType h_, ScalarType r1_, ScalarType r2_)
        : h(h_),
          r1(r1_),
          r2(r2_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        ScalarType b = (r1 - r2) / h;
        ScalarType a = sqrt(ScalarType(1) - b * b);
        Vec2<ScalarType> pxz{p(0), p(2)};
        Vec2<ScalarType> q{Norm(pxz), p(1)};
        ScalarType k = Dot(q, Vec2<ScalarType>{-b, a});
        ScalarType constexpr zero{0};
        if (k < zero)
            return Norm(q) - r1;
        if (k > a * h)
            return Norm(q - Vec2<ScalarType>{zero, h}) - r2;
        return Dot(q, Vec2<ScalarType>{a, b}) - r1;
    }
};

template <common::CArithmetic TScalar>
struct Octahedron : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType s{TScalar(1)};   
    Octahedron() = default;
    explicit Octahedron(ScalarType s_)
        : s(s_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        using namespace std;
        p            = Abs(p);
        ScalarType m = p(0) + p(1) + p(2) - s;
        ScalarType constexpr three{3};
        Vec3<ScalarType> q;
        if (three * p(0) < m)
            q = p;
        else if (three * p(1) < m)
            q = Vec3<ScalarType>{p(1), p(2), p(0)};
        else if (three * p(2) < m)
            q = Vec3<ScalarType>{p(2), p(0), p(1)};
        else
            return m * ScalarType(0.57735027);
        ScalarType constexpr zero{0};
        ScalarType k = clamp(ScalarType(0.5) * (q(2) - q(1) + s), zero, s);
        return Norm(Vec3<ScalarType>{q(0), q(1) - s + k, q(2) - k});
    }
};

template <common::CArithmetic TScalar>
struct Pyramid : public Primitive
{
    using ScalarType = TScalar; 
    ScalarType h{TScalar(1)};   
    Pyramid() = default;
    explicit Pyramid(ScalarType h_)
        : h(h_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> p) const
    {
        using namespace std;
        ScalarType constexpr quarter{0.25};
        ScalarType constexpr half{0.5};
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
 
        ScalarType m2 = h * h + quarter;
 
        ScalarType apx = abs(p(0));
        ScalarType apz = abs(p(2));
        bool bzx       = (apz > apx);
        p(0)           = bzx * apz + (not bzx) * apx - half;
        p(2)           = bzx * apx + (not bzx) * apz - half;
 
        Vec3<ScalarType> q{p(2), h * p(1) - half * p(0), h * p(0) + half * p(1)};
 
        ScalarType s = max(-q(0), zero);
        ScalarType t = clamp((q(1) - half * p(2)) / (m2 + quarter), zero, one);
 
        ScalarType a = m2 * (q(0) + s) * (q(0) + s) + q(1) * q(1);
        ScalarType b =
            m2 * (q(0) + half * t) * (q(0) + half * t) + (q(1) - m2 * t) * (q(1) - m2 * t);
 
        bool bd2      = min(q(1), -q(0) * m2 - q(1) * half) > zero;
        ScalarType d2 = bd2 * zero + (not bd2) * min(a, b);
 
        return sqrt((d2 + q(2) * q(2)) / m2) * sign(max(q(2), -p(1)));
    }
};

template <common::CArithmetic TScalar>
struct Triangle : public Primitive
{
    using ScalarType = TScalar;                             
    Vec3<ScalarType> a{TScalar(0), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> b{TScalar(1), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> c{TScalar(0), TScalar(0), TScalar(1)}; 
    Triangle() = default;
    explicit Triangle(
        Vec3<ScalarType> const& a_,
        Vec3<ScalarType> const& b_,
        Vec3<ScalarType> const& c_)
        : a(a_),
          b(b_),
          c(c_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec3<ScalarType> ba  = b - a;
        Vec3<ScalarType> pa  = p - a;
        Vec3<ScalarType> cb  = c - b;
        Vec3<ScalarType> pb  = p - b;
        Vec3<ScalarType> ac  = a - c;
        Vec3<ScalarType> pc  = p - c;
        Vec3<ScalarType> nor = Cross(ba, ac);
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
        ScalarType constexpr two{2};
        bool bs =
        (sign(Dot(Cross(ba, nor), pa)) + sign(Dot(Cross(cb, nor), pb)) +
         sign(Dot(Cross(ac, nor), pc)) <
         two);
        return sqrt(
            bs ?
                min(
                    min(
                        SquaredNorm(ba * clamp(Dot(ba, pa) / SquaredNorm(ba), zero, one) - pa),
                        SquaredNorm(cb * clamp(Dot(cb, pb) / SquaredNorm(cb), zero, one) - pb)),
                    SquaredNorm(ac * clamp(Dot(ac, pc) / SquaredNorm(ac), zero, one) - pc)) :
                Dot(nor, pa) * Dot(nor, pa) / SquaredNorm(nor));
    }
};

template <common::CArithmetic TScalar>
struct Quadrilateral : public Primitive
{
    using ScalarType = TScalar;                             
    Vec3<ScalarType> a{TScalar(0), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> b{TScalar(1), TScalar(0), TScalar(0)}; 
    Vec3<ScalarType> c{TScalar(1), TScalar(0), TScalar(1)}; 
    Vec3<ScalarType> d{TScalar(0), TScalar(0), TScalar(1)}; 
    Quadrilateral() = default;
    explicit Quadrilateral(
        Vec3<ScalarType> const& a_,
        Vec3<ScalarType> const& b_,
        Vec3<ScalarType> const& c_,
        Vec3<ScalarType> const& d_)
        : a(a_),
          b(b_),
          c(c_),
          d(d_)
    {
    }

    PBAT_HOST_DEVICE ScalarType Eval(Vec3<ScalarType> const& p) const
    {
        using namespace std;
        Vec3<ScalarType> ba  = b - a;
        Vec3<ScalarType> pa  = p - a;
        Vec3<ScalarType> cb  = c - b;
        Vec3<ScalarType> pb  = p - b;
        Vec3<ScalarType> dc  = d - c;
        Vec3<ScalarType> pc  = p - c;
        Vec3<ScalarType> ad  = a - d;
        Vec3<ScalarType> pd  = p - d;
        Vec3<ScalarType> nor = Cross(ba, ad);
        ScalarType constexpr zero{0};
        ScalarType constexpr one{1};
        ScalarType constexpr three{3};
        bool bs =
        (sign(Dot(Cross(ba, nor), pa)) + sign(Dot(Cross(cb, nor), pb)) +
         sign(Dot(Cross(dc, nor), pc)) + sign(Dot(Cross(ad, nor), pd)) <
         three);
        return sqrt(
            bs ?
                min(
                    min(
                        min(
                            SquaredNorm(ba * clamp(Dot(ba, pa) / SquaredNorm(ba), zero, one) - pa),
                            SquaredNorm(cb * clamp(Dot(cb, pb) / SquaredNorm(cb), zero, one) - pb)),
                        SquaredNorm(dc * clamp(Dot(dc, pc) / SquaredNorm(dc), zero, one) - pc)),
                    SquaredNorm(ad * clamp(Dot(ad, pd) / SquaredNorm(ad), zero, one) - pd)) :
                Dot(nor, pa) * Dot(nor, pa) / SquaredNorm(nor));
    }
};
} // namespace pbat::geometry::sdf
 
#endif // PBAT_GEOMETRY_SDF_PRIMITIVE_H