Line data    Source code

       1              : #pragma once
       2              : // ============================================================================
       3              : //  Dispatcher.hpp
       4              : //
       5              : //  Routes each shape-pair to the correct CollisionAlgorithm specialisation.
       6              : //  The primary template falls back to GJK + EPA + ContactManifoldGenerator
       7              : //  which gives a proper 1-4 point manifold even for non-analytic pairs.
       8              : //
       9              : //  Analytic specialisations (SphereSphere, SphereBox, BoxBox, …) should
      10              : //  fill the ContactManifold directly and bypass GJK/EPA entirely.
      11              : // ============================================================================
      12              : 
      13              : #include "rbc/shapes/ShapeTypes.hpp"
      14              : #include "rbc/Contact.hpp"
      15              : #include "rbc/gjk/GJK.hpp"
      16              : #include "rbc/gjk/EPA.hpp"
      17              : #include "rbc/gjk/MinkowskiDiff.hpp"
      18              : #include "rbc/gjk/ContactManifoldGenerator.hpp"   // gjk_epa_manifold + generate_manifold
      19              : #include <variant>
      20              : 
      21              : namespace rbc
      22              : {
      23              : 
      24              :     // ── Primary template: GJK/EPA + manifold generation ──────────────────────
      25              :     template <typename A, typename B>
      26              :     struct CollisionAlgorithm
      27              :     {
      28            0 :         static bool test(const A       &a, const m3d::tf &tf_a,
      29              :                          const B       &b, const m3d::tf &tf_b,
      30              :                          ContactManifold &manifold)
      31              :         {
      32            0 :             Shape sa = a, sb = b;
      33            0 :             return gjk_epa_manifold(sa, tf_a, sb, tf_b, manifold);
      34              :         }
      35              :     };
      36              : 
      37              :     // ── Symmetric helper: (B,A) reuses (A,B) and flips the normal ────────────
      38              :     template <typename A, typename B>
      39              :     struct CollisionAlgorithmSym
      40              :     {
      41         8953 :         static bool test(const A       &a, const m3d::tf &tf_a,
      42              :                          const B       &b, const m3d::tf &tf_b,
      43              :                          ContactManifold &manifold)
      44              :         {
      45         8953 :             bool hit = CollisionAlgorithm<B, A>::test(b, tf_b, a, tf_a, manifold);
      46         8953 :             if (hit)
      47         3652 :                 manifold.normal = -manifold.normal;
      48         8953 :             return hit;
      49              :         }
      50              :     };
      51              : 
      52              : } // namespace rbc
      53              : 
      54              : // ── Analytic specialisations ─────────────────────────────────────────────────
      55              : #include "rbc/analytic/SphereSphere.hpp"
      56              : #include "rbc/analytic/SphereBox.hpp"
      57              : #include "rbc/analytic/BoxBox.hpp"
      58              : //#include "rbc/analytic/SphereCapsule.hpp"
      59              : //#include "rbc/analytic/CapsuleCapsule.hpp"
      60              : //#include "rbc/analytic/PlaneCollision.hpp"
      61              : // #include "rbc/analytic/MeshCollision.hpp"
      62              : //#include "rbc/analytic/HeightmapCollision.hpp"
      63              : 
      64              : namespace rbc
      65              : {
      66              :     // ── Dispatch wrapper ──────────────────────────────────────────────────────
      67              :     template <typename A, typename B>
      68         9386 :     bool dispatch_wrapper(const Shape     &a, const m3d::tf &tf_a,
      69              :                           const Shape     &b, const m3d::tf &tf_b,
      70              :                           ContactManifold &out)
      71              :     {
      72         9386 :         return CollisionAlgorithm<A, B>::test(a.get<A>(), tf_a, b.get<B>(), tf_b, out);
      73              :     }
      74              : 
      75              :     using CollisionFunc = bool (*)(const Shape &, const m3d::tf &,
      76              :                                    const Shape &, const m3d::tf &,
      77              :                                    ContactManifold &);
      78              : 
      79              : #define GENERATE_ROW(InnerType, InnerName, FixedOuterType) \
      80              :     dispatch_wrapper<FixedOuterType, InnerType>,
      81              : 
      82              : #define GENERATE_MATRIX_LINE(OuterType, OuterName) \
      83              :     {RBC_SHAPE_LIST_INNER(GENERATE_ROW, OuterType)},
      84              : 
      85              :     static constexpr CollisionFunc DispatchTable
      86              :         [static_cast<int>(ShapeType::Count)]
      87              :         [static_cast<int>(ShapeType::Count)] =
      88              :     {
      89              :         RBC_SHAPE_LIST(GENERATE_MATRIX_LINE)
      90              :     };
      91              : 
      92         9386 :     inline bool dispatch(const Shape     &a, const m3d::tf &tf_a,
      93              :                          const Shape     &b, const m3d::tf &tf_b,
      94              :                          ContactManifold &out)
      95              :     {
      96              :         return DispatchTable
      97         9386 :             [static_cast<int>(a.type)]
      98         9386 :             [static_cast<int>(b.type)](a, tf_a, b, tf_b, out);
      99              :     }
     100              : 
     101              : } // namespace rbc