narrowphase.hpp

Go to the documentation of this file.

 
#ifndef DEM_NARROWPHASE_HPP
#define DEM_NARROWPHASE_HPP
 
#include <Kokkos_Core.hpp>
 
#include "contact_preprocessing.hpp"  // ContactC, CpExec/CpMem
#include "dem_portable.hpp"
#include "peclet/core/geom/scene.hpp"
 
namespace peclet::dem {
 
struct ShapeDesc {
  int type;         // peclet::dem::ShapeKind
  F4 params;        // analytic parameters (see sdf_analytic); grid: params.x = bounding radius
  int shellOffset;  // start index into the flat shell-points View
  int numPoints;    // shell size; 0 => analytic single-probe (sphere center)
  // --- grid-SDF descriptor (type == SHAPE_GRID_SDF); default/zero for analytic shapes ---
  // Carries core's descriptor DIRECTLY rather than loose fields the sampler marshals per call.
  // That is not cosmetic: building a GridDesc inside the sampler shifted nvcc's FMA contraction in
  // the trilinear chain and moved 185/4000 CUDA results by ~1 ULP (Layer 0 rung 3). Passing a
  // prebuilt descriptor through is bit-identical to the pre-port code on every backend.
  peclet::core::geom::GridDesc<float> grid;  // extension = kObject (a body)
  // --- composed-analytic descriptor (type == SHAPE_SCENE); null/-1 for everything else ---
  // A raw pointer into the Simulation's pooled device node View (same pattern as WallSdf.nodes:
  // the owning View lives on the Simulation so ShapeDesc stays POD and device-copyable). Child
  // indices are ABSOLUTE into the pool (rebased at add_scene_shape, like addAnalyticWall).
  const peclet::core::geom::ShapeNode<float>* nodes = nullptr;
  int nodeCount = 0;
  int shapeRoot = -1;
};
 
struct PlaneP {
  F3 point;
  F3 normal;
};
 
struct WallSdf {
  // world-space grid SDF samples live in Particles::wallGrid at [offset, offset + nx*ny*nz),
  // x-fastest (idx = x + y*nx + z*nx*ny), at nodes q = origin + (x,y,z)/invSpacing.
  // extension = kContainer -- see the sign discussion on sampleWallSdf below.
  peclet::core::geom::GridDesc<float> grid;
  // ANALYTIC alternative (Layer 1): when shapeRoot >= 0 the wall is a core shape tree evaluated
  // exactly, not a sampled field -- a stirrer becomes a CSG expression instead of a voxel grid,
  // with no resolution limit and no per-rank replicated sample pool. Raw pointer (like
  // core::geom::SceneView) so WallSdf stays POD and device-copyable with no signature churn at the
  // call sites. `sign` is -1 for the usual case of a CONTAINER built from an object tree: core's
  // leaves are negative-inside-solid, while a wall must read positive in the void where the grains
  // live.
  const peclet::core::geom::ShapeNode<float>* nodes = nullptr;
  int nodeCount = 0;
  int shapeRoot = -1;
  float sign = 1.0f;
  // rigid-body surface velocity field v(x) = linVel + angVel × (x − center) (set from the host).
  F3 linVel{0, 0, 0};
  F3 angVel{0, 0, 0};
  F3 center{0, 0, 0};
  // binary particle–wall material (independent of the body-body material).
  float restitution = 0.0f;
  float friction = 0.0f;
  // Optional material id: >= 0 and a pair table set -> particle-wall (e, mu) comes from the
  // pair table row (materialId(particle), materialId) instead of the binary values above.
  int materialId = -1;
};
 
constexpr int kMaxMaterials = 8;
using PairTableView = Kokkos::View<const float*, CpMem>;
using MatIdView = Kokkos::View<const unsigned char*, CpMem>;
 
// Convenience aliases for the SoA particle views the narrow-phase reads.
using PosView = Kokkos::View<const float* [3], CpMem>;
using QuatView = Kokkos::View<const float* [4], CpMem>;
using ScalarF = Kokkos::View<const float*, CpMem>;
using ScalarI = Kokkos::View<const int*, CpMem>;
using ShellView = Kokkos::View<const float* [3], CpMem>;
using GridView = Kokkos::View<const float*, CpMem>;
 
KOKKOS_INLINE_FUNCTION F3 loadF3(PosView v, int i) {
  return F3{v(i, 0), v(i, 1), v(i, 2)};
}
KOKKOS_INLINE_FUNCTION F4 loadF4(QuatView v, int i) {
  return F4{v(i, 0), v(i, 1), v(i, 2), v(i, 3)};
}
 
KOKKOS_INLINE_FUNCTION float sampleGridSdf(F3 p, const ShapeDesc& d, GridView grid) {
  return peclet::core::geom::sampleGrid(toCoreVec(p), d.grid, grid);
}
 
KOKKOS_INLINE_FUNCTION float sampleWallSdf(F3 p, const WallSdf& w, GridView grid) {
  if (w.shapeRoot >= 0)  // analytic wall: exact, no lattice
    return w.sign * peclet::core::geom::evalTree<float>(
                        peclet::core::geom::TablePtr<peclet::core::geom::ShapeNode<float>>{w.nodes},
                        w.nodeCount, w.shapeRoot, toCoreVec(p),
                        peclet::core::geom::TablePtr<peclet::core::geom::GridDesc<float>>{nullptr},
                        peclet::core::geom::PoolPtr<float>{nullptr});
  return peclet::core::geom::sampleGrid(toCoreVec(p), w.grid, grid);
}
 
KOKKOS_INLINE_FUNCTION float sdfEvalShape(F3 p, const ShapeDesc& d, GridView grid) {
  if (d.type == SHAPE_GRID_SDF)
    return sampleGridSdf(p, d, grid);
  if (d.type == SHAPE_SCENE)  // composed analytic tree, exact in canonical body space
    return peclet::core::geom::evalTree<float>(
        peclet::core::geom::TablePtr<peclet::core::geom::ShapeNode<float>>{d.nodes}, d.nodeCount,
        d.shapeRoot, toCoreVec(p),
        peclet::core::geom::TablePtr<peclet::core::geom::GridDesc<float>>{nullptr},
        peclet::core::geom::PoolPtr<float>{nullptr});
  return sdfEval(p, d.type, d.params);
}
 
KOKKOS_INLINE_FUNCTION F3 sdfGradShape(F3 p, const ShapeDesc& d, GridView grid) {
  namespace prim = peclet::core::geom::prim;
  const peclet::core::Vec3<float> q = toCoreVec(p);
  peclet::core::Vec3<float> g;
  switch (d.type) {
    case SPHERE:
      g = prim::Sphere<float>{d.params.x}.grad(q);
      break;
    case BOX:
      g = prim::Box<float>{d.params.x, d.params.y, d.params.z}.grad(q);
      break;
    case HOLLOW_CYLINDER:
      g = prim::HollowCylinder<float>{d.params.x, d.params.y, d.params.z}.grad(q);
      break;
    case SHAPE_SCENE: {  // composed tree: ANALYTIC gradient (core evalTreeGrad -- the chain rule
                         // costs one rotation per node; at CSG ridges this is the active face's
                         // EXACT normal where a central difference smears the two faces into an
                         // average that belongs to neither. Measured 3.2x faster than eval+FD too.)
      peclet::core::Vec3<float> gt;
      (void)peclet::core::geom::evalTreeGrad<float>(
          peclet::core::geom::TablePtr<peclet::core::geom::ShapeNode<float>>{d.nodes}, d.nodeCount,
          d.shapeRoot, toCoreVec(p),
          peclet::core::geom::TablePtr<peclet::core::geom::GridDesc<float>>{nullptr},
          peclet::core::geom::PoolPtr<float>{nullptr}, gt);
      return F3{gt.x, gt.y, gt.z};
    }
    default: {  // SHAPE_GRID_SDF: sampled field, central difference (fixed-eps caveat, Layer 1)
      const float eps = 1e-4f;
      return F3{sdfEvalShape(F3{p.x + eps, p.y, p.z}, d, grid) -
                    sdfEvalShape(F3{p.x - eps, p.y, p.z}, d, grid),
                sdfEvalShape(F3{p.x, p.y + eps, p.z}, d, grid) -
                    sdfEvalShape(F3{p.x, p.y - eps, p.z}, d, grid),
                sdfEvalShape(F3{p.x, p.y, p.z + eps}, d, grid) -
                    sdfEvalShape(F3{p.x, p.y, p.z - eps}, d, grid)};
    }
  }
  return F3{g.x, g.y, g.z};
}
 
inline void detectContactsKokkos(Kokkos::View<const int* [2], CpMem> pairs, int numPairs,
                                 PosView pos, QuatView quat, ScalarF scale, ScalarI shapeId,
                                 Kokkos::View<const ShapeDesc*, CpMem> shapes, ShellView shell,
                                 float globalScale, float margin,
                                 Kokkos::View<ContactC*, CpMem> outContacts,
                                 Kokkos::View<int, CpMem> outCount,
                                 Kokkos::View<float, CpMem> maxOverlap,
                                 GridView sdfGrid = GridView{}, MatIdView matId = MatIdView{},
                                 PairTableView pairTable = PairTableView{}, bool twoWay = false) {
  CpExec space;
  const int maxContacts = static_cast<int>(outContacts.extent(0));
  Kokkos::parallel_for(
      "peclet::dem::np::contacts", Kokkos::RangePolicy<CpExec>(space, 0, numPairs),
      KOKKOS_LAMBDA(int idx) {
        const int idA = pairs(idx, 0), idB = pairs(idx, 1);
        const ShapeDesc dA = shapes(shapeId(idA));
        const ShapeDesc dB = shapes(shapeId(idB));
        const F3 posA = loadF3(pos, idA), posB = loadF3(pos, idB);
        const F4 qA = loadF4(quat, idA), qB = loadF4(quat, idB);
        // Canonical->world size = per-particle scale * global scale. globalScale must appear in the
        // shell placement, the canonical remap of B, and the distance rescale exactly as it does in
        // the sphere probe radius below — else a non-unit global_scale makes A's and B's radii
        // disagree (grains a real diameter apart read a huge penetration and the solver explodes).
        // The plane/wall boundary kernels already fold globalScale into their `s =
        // scale*globalScale`.
        const float effScaleA = scale(idA) * globalScale, effScaleB = scale(idB) * globalScale;
 
        const int countA = dA.numPoints;
        const bool sphereA = (dA.type == SPHERE);
        const int iter = (countA > 0) ? countA : 1;
        // Two-way only: B's probes against A's SDF. A shell A against a sphere B swaps to B's
        // exact sphere probe (and drops A's forward probes below); a shell pair adds B's shell.
        const int countB = dB.numPoints;
        const bool sphereProbeB = twoWay && countA > 0 && countB == 0 && dB.type == SPHERE;
        const bool shellProbeB = twoWay && countA > 0 && countB > 0;
 
        for (int k = 0; k < (sphereProbeB ? 0 : iter); ++k) {
          F3 pLocalA{0, 0, 0};
          float pointRadius = 0.0f;
          if (countA > 0) {
            const int s = dA.shellOffset + k;
            pLocalA = F3{shell(s, 0), shell(s, 1), shell(s, 2)};
          } else if (sphereA) {
            pointRadius = dA.params.x * effScaleA;
          }
 
          const F3 pWorld = add3(posA, rotateVector(qA, scale3(pLocalA, effScaleA)));
          const F3 pLocalB = invRotateVector(qB, sub3(pWorld, posB));
          const F3 pCanB = scale3(pLocalB, 1.0f / effScaleB);
 
          const float dist = sdfEvalShape(pCanB, dB, sdfGrid) * effScaleB;
          const float effDist = dist - pointRadius;
          if (effDist >= margin)
            continue;
 
          if (effDist < 0.0f)
            Kokkos::atomic_max(&maxOverlap(), -effDist);
          const int slot = Kokkos::atomic_fetch_add(&outCount(), 1);
          if (slot >= maxContacts) {
            Kokkos::atomic_add(&outCount(), -1);
            continue;
          }
 
          F3 nLoc = sdfGradShape(pCanB, dB, sdfGrid);
          const float len = len3(nLoc);
          nLoc = (len > 1e-9f) ? scale3(nLoc, 1.0f / len) : F3{0, 1, 0};
 
          const F3 nWorld = rotateVector(qB, nLoc);
          const F3 pSurfA = sub3(pWorld, scale3(nWorld, pointRadius));
          const F3 rA = sub3(pSurfA, posA);
          const F3 rB = sub3(sub3(pSurfA, scale3(nWorld, effDist)), posB);
 
          ContactC c{};
          c.bodyA = idA;
          c.bodyB = idB;
          c.normal = F4{nWorld.x, nWorld.y, nWorld.z, 0.0f};
          c.rA = F4{rA.x, rA.y, rA.z, 0.0f};
          c.rB = F4{rB.x, rB.y, rB.z, 0.0f};
          c.dist = effDist;
          c.friction_lambda_n = 0.0f;
          c.weight = 0.0f;
          if (pairTable.extent(0) > 0) {  // per-pair body-body material
            const int t = (int(matId(idA)) * kMaxMaterials + int(matId(idB))) * 2;
            c.boundaryRestitution = pairTable(t);
            c.boundaryFriction = pairTable(t + 1);
          }
          outContacts(slot) = c;
        }
 
        // Reverse direction: B's probe points against A's SDF, emitted in the canonical (A, B)
        // orientation. The geometry mirrors the forward block with the roles of A and B exchanged;
        // only the emission differs (normal = -gradient of A, the surface points swap arms).
        const int iterB = sphereProbeB ? 1 : (shellProbeB ? countB : 0);
        for (int k = 0; k < iterB; ++k) {
          F3 pLocalB{0, 0, 0};
          float pointRadius = 0.0f;
          if (shellProbeB) {
            const int s = dB.shellOffset + k;
            pLocalB = F3{shell(s, 0), shell(s, 1), shell(s, 2)};
          } else {
            pointRadius = dB.params.x * effScaleB;
          }
 
          const F3 pWorld = add3(posB, rotateVector(qB, scale3(pLocalB, effScaleB)));
          const F3 pLocalA = invRotateVector(qA, sub3(pWorld, posA));
          const F3 pCanA = scale3(pLocalA, 1.0f / effScaleA);
 
          const float dist = sdfEvalShape(pCanA, dA, sdfGrid) * effScaleA;
          const float effDist = dist - pointRadius;
          if (effDist >= margin)
            continue;
 
          if (effDist < 0.0f)
            Kokkos::atomic_max(&maxOverlap(), -effDist);
          const int slot = Kokkos::atomic_fetch_add(&outCount(), 1);
          if (slot >= maxContacts) {
            Kokkos::atomic_add(&outCount(), -1);
            continue;
          }
 
          F3 mLoc = sdfGradShape(pCanA, dA, sdfGrid);
          const float len = len3(mLoc);
          mLoc = (len > 1e-9f) ? scale3(mLoc, 1.0f / len) : F3{0, 1, 0};
 
          const F3 mWorld = rotateVector(qA, mLoc);  // outward normal of A at B's probe
          const F3 pSurfB = sub3(pWorld, scale3(mWorld, pointRadius));  // on B's surface
          const F3 pSurfA = sub3(pSurfB, scale3(mWorld, effDist));      // on A's surface
          const F3 rA = sub3(pSurfA, posA);
          const F3 rB = sub3(pSurfB, posB);
 
          // Canonical orientation: the normal is B's outward direction, -mWorld, so that
          // pSurfA = pSurfB + normal * dist as for a forward contact. It is A's surface normal,
          // so normal.w = 1 marks it (normalOfA): the position phase rotates it with A's spin.
          ContactC c{};
          c.bodyA = idA;
          c.bodyB = idB;
          c.normal = F4{-mWorld.x, -mWorld.y, -mWorld.z, 1.0f};
          c.rA = F4{rA.x, rA.y, rA.z, 0.0f};
          c.rB = F4{rB.x, rB.y, rB.z, 0.0f};
          c.dist = effDist;
          c.friction_lambda_n = 0.0f;
          c.weight = 0.0f;
          if (pairTable.extent(0) > 0) {  // per-pair body-body material (the pair's own row)
            const int t = (int(matId(idA)) * kMaxMaterials + int(matId(idB))) * 2;
            c.boundaryRestitution = pairTable(t);
            c.boundaryFriction = pairTable(t + 1);
          }
          outContacts(slot) = c;
        }
      });
  space.fence();
}
 
inline void detectWallSdfKokkos(
    int numReal, int numWalls, PosView pos, QuatView quat, ScalarF scale, ScalarI shapeId,
    Kokkos::View<const ShapeDesc*, CpMem> shapes, ShellView shell,
    Kokkos::View<const WallSdf*, CpMem> walls, GridView wallGrid, float globalScale, float margin,
    Kokkos::View<ContactC*, CpMem> outContacts, Kokkos::View<int, CpMem> outCount,
    Kokkos::View<float, CpMem> maxOverlap, MatIdView matId = MatIdView{},
    PairTableView pairTable = PairTableView{}, int wallIdBase = 0) {
  CpExec space;
  const int maxContacts = static_cast<int>(outContacts.extent(0));
  Kokkos::parallel_for(
      "peclet::dem::np::wallsdf", Kokkos::RangePolicy<CpExec>(space, 0, numReal),
      KOKKOS_LAMBDA(int i) {
        const F3 posA = loadF3(pos, i);
        const float s = scale(i) * globalScale;
        const ShapeDesc d = shapes(shapeId(i));
        float baseR = d.params.x;
        if (baseR == 0.0f)
          baseR = 1.0f;
        const float radius = baseR * s;  // analytic sphere radius (numPts == 0)
        const int numPts = d.numPoints;
        const F4 qA = loadF4(quat, i);
        const float eps = 1e-4f;
 
        for (int wi = 0; wi < numWalls; ++wi) {
          const WallSdf w = walls(wi);
          const int iter = (numPts > 0) ? numPts : 1;
          for (int k = 0; k < iter; ++k) {
            // World surface probe: a shell point (rotated + scaled) or, for a sphere, the centre.
            F3 rA{0, 0, 0};
            if (numPts > 0) {
              const int si = d.shellOffset + k;
              rA = rotateVector(qA, scale3(F3{shell(si, 0), shell(si, 1), shell(si, 2)}, s));
            }
            const F3 pw = add3(posA, rA);
            const float sdf = sampleWallSdf(pw, w, wallGrid);
            // Outward SDF gradient = push-out normal (wall -> void). ANALYTIC for a tree wall
            // (evalTreeGrad; walls are CSG containers whose EDGES are exactly where the old
            // central difference smeared the normal -- the deliberate numerics change of this
            // commit, mirroring Layer 1's exact particle normals). Grid walls keep the FD.
            F3 n;
            if (w.shapeRoot >= 0) {
              peclet::core::Vec3<float> gt;
              (void)peclet::core::geom::evalTreeGrad<float>(
                  peclet::core::geom::TablePtr<peclet::core::geom::ShapeNode<float>>{w.nodes},
                  w.nodeCount, w.shapeRoot, toCoreVec(pw),
                  peclet::core::geom::TablePtr<peclet::core::geom::GridDesc<float>>{nullptr},
                  peclet::core::geom::PoolPtr<float>{nullptr}, gt);
              n = F3{w.sign * gt.x, w.sign * gt.y, w.sign * gt.z};
            } else {
              n = F3{sampleWallSdf(F3{pw.x + eps, pw.y, pw.z}, w, wallGrid) -
                         sampleWallSdf(F3{pw.x - eps, pw.y, pw.z}, w, wallGrid),
                     sampleWallSdf(F3{pw.x, pw.y + eps, pw.z}, w, wallGrid) -
                         sampleWallSdf(F3{pw.x, pw.y - eps, pw.z}, w, wallGrid),
                     sampleWallSdf(F3{pw.x, pw.y, pw.z + eps}, w, wallGrid) -
                         sampleWallSdf(F3{pw.x, pw.y, pw.z - eps}, w, wallGrid)};
            }
            const float ln = len3(n);
            n = (ln > 1e-9f) ? scale3(n, 1.0f / ln) : F3{0, 1, 0};
 
            // For a sphere the nearest surface point is one radius toward the wall (−normal); its
            // signed gap to the wall is sdf − radius.
            const float dist = (numPts > 0) ? sdf : sdf - radius;
            if (dist >= margin)
              continue;
            const F3 rAeff = (numPts > 0) ? rA : scale3(n, -radius);
            const F3 pSurfA = add3(posA, rAeff);             // particle surface point
            const F3 pWall = sub3(pSurfA, scale3(n, dist));  // point on the wall along the normal
 
            if (dist < 0.0f)
              Kokkos::atomic_max(&maxOverlap(), -dist);
            const int slot = Kokkos::atomic_fetch_add(&outCount(), 1);
            if (slot >= maxContacts) {
              Kokkos::atomic_add(&outCount(), -1);
              continue;
            }
            // Rigid-body wall surface velocity at the contact point: linVel + angVel × (r −
            // center).
            const F3 r = sub3(pWall, w.center);
            const F3 vWall = add3(w.linVel, cross3v(w.angVel, r));
 
            ContactC c{};
            c.bodyA = i;
            c.bodyB = wallContactId(wallIdBase + wi);  // SDF walls follow the planes
            c.normal = F4{n.x, n.y, n.z, 0.0f};
            c.rA = F4{rAeff.x, rAeff.y, rAeff.z, 0.0f};
            c.rB = F4{pWall.x, pWall.y, pWall.z, 0.0f};
            c.dist = dist;
            c.friction_lambda_n = 0.0f;
            c.weight = 0.0f;
            c.boundaryVel = F4{vWall.x, vWall.y, vWall.z, 0.0f};
            if (w.materialId >= 0 && pairTable.extent(0) > 0) {
              const int t = (int(matId(i)) * kMaxMaterials + w.materialId) * 2;
              c.boundaryRestitution = pairTable(t);
              c.boundaryFriction = pairTable(t + 1);
            } else {
              c.boundaryRestitution = w.restitution;
              c.boundaryFriction = w.friction;
            }
            outContacts(slot) = c;
          }
        }
      });
  space.fence();
}
 
inline void detectBoundaryKokkos(int numReal, int numPlanes, PosView pos, QuatView quat,
                                 ScalarF scale, ScalarI shapeId,
                                 Kokkos::View<const ShapeDesc*, CpMem> shapes, ShellView shell,
                                 Kokkos::View<const PlaneP*, CpMem> planes, float globalScale,
                                 float margin, Kokkos::View<ContactC*, CpMem> outContacts,
                                 Kokkos::View<int, CpMem> outCount,
                                 Kokkos::View<float, CpMem> maxOverlap) {
  CpExec space;
  const int maxContacts = static_cast<int>(outContacts.extent(0));
  Kokkos::parallel_for(
      "peclet::dem::np::boundary", Kokkos::RangePolicy<CpExec>(space, 0, numReal),
      KOKKOS_LAMBDA(int i) {
        const F3 posA = loadF3(pos, i);
        const float s = scale(i) * globalScale;
        const ShapeDesc d = shapes(shapeId(i));
        float baseR = d.params.x;
        if (baseR == 0.0f)
          baseR = 1.0f;
        const float radius = baseR * s;
        const int numPts = d.numPoints;
        const F4 qA = loadF4(quat, i);
 
        for (int pi = 0; pi < numPlanes; ++pi) {
          const PlaneP pl = planes(pi);
          // emit helper, inlined (no lambda capture of mutable counters across backends).
          if (numPts > 0) {
            for (int k = 0; k < numPts; ++k) {
              const int si = d.shellOffset + k;
              const F3 rA =
                  rotateVector(qA, scale3(F3{shell(si, 0), shell(si, 1), shell(si, 2)}, s));
              const F3 pwk = add3(posA, rA);
              const float dist = dot3(sub3(pwk, pl.point), pl.normal);
              if (dist >= margin)
                continue;
              if (dist < 0.0f)
                Kokkos::atomic_max(&maxOverlap(), -dist);
              const int slot = Kokkos::atomic_fetch_add(&outCount(), 1);
              if (slot >= maxContacts) {
                Kokkos::atomic_add(&outCount(), -1);
                continue;
              }
              ContactC c{};
              c.bodyA = i;
              c.bodyB = wallContactId(pi);
              c.normal = F4{pl.normal.x, pl.normal.y, pl.normal.z, 0.0f};
              c.rA = F4{rA.x, rA.y, rA.z, 0.0f};
              c.rB = F4{pl.point.x, pl.point.y, pl.point.z, 0.0f};
              c.dist = dist;
              c.friction_lambda_n = 0.0f;
              c.weight = 0.0f;
              outContacts(slot) = c;
            }
          } else {
            const float dist = dot3(sub3(posA, pl.point), pl.normal) - radius;
            if (dist >= margin)
              continue;
            if (dist < 0.0f)
              Kokkos::atomic_max(&maxOverlap(), -dist);
            const int slot = Kokkos::atomic_fetch_add(&outCount(), 1);
            if (slot >= maxContacts) {
              Kokkos::atomic_add(&outCount(), -1);
              continue;
            }
            const F3 rA = scale3(pl.normal, -radius);
            ContactC c{};
            c.bodyA = i;
            c.bodyB = wallContactId(pi);
            c.normal = F4{pl.normal.x, pl.normal.y, pl.normal.z, 0.0f};
            c.rA = F4{rA.x, rA.y, rA.z, 0.0f};
            c.rB = F4{pl.point.x, pl.point.y, pl.point.z, 0.0f};
            c.dist = dist;
            c.friction_lambda_n = 0.0f;
            c.weight = 0.0f;
            outContacts(slot) = c;
          }
        }
      });
  space.fence();
}
 
}  // namespace peclet::dem
 
#endif  // DEM_NARROWPHASE_HPP