solver_friction.hpp

Go to the documentation of this file.

 
#ifndef DEM_SOLVER_FRICTION_HPP
#define DEM_SOLVER_FRICTION_HPP
 
#include <Kokkos_Core.hpp>
 
#include "dem_portable.hpp"
#include "solver_position.hpp"  // ContactC, detail::computeW, CpExec/CpMem
#include "solver_velocity.hpp"  // detail::genInvMass (the world-frame inverse inertia)
 
namespace peclet::dem {
 
using FrManifoldCounts = Kokkos::View<float* [2], CpMem>;  // per body: .x = plane load, .y = count
 
KOKKOS_INLINE_FUNCTION void contactArmsMid(const ContactC& c, F3& rA, F3& rB) {
  const F4 shift{c.normal.x * c.dist * 0.5f, c.normal.y * c.dist * 0.5f, c.normal.z * c.dist * 0.5f,
                 0.0f};
  rA = F3{c.rA.x - shift.x, c.rA.y - shift.y, c.rA.z - shift.z};
  rB = F3{c.rB.x + shift.x, c.rB.y + shift.y, c.rB.z + shift.z};
}
 
inline void accumulateNormalImpulseKokkos(Kokkos::View<ContactC*, CpMem> contacts, int numContacts,
                                          Kokkos::View<const float*, CpMem> invMass,
                                          Kokkos::View<const float* [3], CpMem> invInertia,
                                          Kokkos::View<const float* [3], CpMem> velPred,
                                          Kokkos::View<const float* [3], CpMem> angVelPred,
                                          Kokkos::View<const int*, CpMem> realIdx, float growthRate,
                                          bool dedupTwins = true) {
  using detail::computeW;
  CpExec space;
  Kokkos::parallel_for(
      "peclet::dem::fric_accum", Kokkos::RangePolicy<CpExec>(space, 0, numContacts),
      KOKKOS_LAMBDA(int idx) {
        ContactC c = contacts(idx);
        if (c.bodyB < 0) {
          // Wall (boundary): accumulate the transmitted normal load on the owned grain (immovable
          // wall -> A term only), relative to the wall's surface velocity. Indexed like
          // computePlaneLoad (c.bodyA directly reads the ghost-augmented velPred slot).
          const int idA = c.bodyA;
          const float invMA = invMass(idA);
          if (invMA <= 0.0f)
            return;
          const F3 invIA = ldF3(invInertia, idA);
          const F3 rA{c.rA.x, c.rA.y, c.rA.z}, n{c.normal.x, c.normal.y, c.normal.z};
          const F3 vAc = add3(ldF3(velPred, idA), cross3v(ldF3(angVelPred, idA), rA));
          const F3 vWall{c.boundaryVel.x, c.boundaryVel.y, c.boundaryVel.z};
          const float approach = -dot3(sub3(vAc, vWall), n);
          if (approach <= 0.0f)
            return;
          const float w_n = computeW(rA, n, invMA, invIA);
          if (w_n > 1e-6f)
            contacts(idx).friction_lambda_n += approach / w_n;
          return;
        }
        const int realA = realIdx(c.bodyA), realB = realIdx(c.bodyB);
        if (dedupTwins && realA > realB)
          return;  // periodic dedup (single rank; §4.2 item 2)
        const float invMA = invMass(realA), invMB = invMass(realB);
        const F3 invIA = ldF3(invInertia, realA), invIB = ldF3(invInertia, realB);
        F3 rA, rB;
        contactArmsMid(c, rA, rB);  // single application point
        const F3 n{c.normal.x, c.normal.y, c.normal.z};
        const F3 vAc = add3(ldF3(velPred, realA), cross3v(ldF3(angVelPred, realA), rA));
        const F3 vBc = add3(ldF3(velPred, realB), cross3v(ldF3(angVelPred, realB), rB));
        const F3 vrel = add3(sub3(vAc, vBc), scale3(sub3(rA, rB), growthRate));
        const float approach = -dot3(vrel, n);
        if (approach <= 0.0f)
          return;
        const float w_n = computeW(rA, n, invMA, invIA) + computeW(rB, n, invMB, invIB);
        if (w_n > 1e-6f)
          contacts(idx).friction_lambda_n += approach / w_n;
      });
  space.fence();
}
 
inline void computePlaneLoadKokkos(Kokkos::View<ContactC*, CpMem> contacts, int numContacts,
                                   Kokkos::View<const float*, CpMem> invMass,
                                   Kokkos::View<const float* [3], CpMem> invInertia,
                                   Kokkos::View<const float* [3], CpMem> velPred,
                                   Kokkos::View<const float* [3], CpMem> angVelPred,
                                   FrManifoldCounts planeFriction) {
  using detail::computeW;
  CpExec space;
  Kokkos::deep_copy(space, planeFriction, 0.0f);
  Kokkos::parallel_for(
      "peclet::dem::fric_plane_load", Kokkos::RangePolicy<CpExec>(space, 0, numContacts),
      KOKKOS_LAMBDA(int idx) {
        ContactC c = contacts(idx);
        if (c.bodyB >= 0)
          return;
        const int idA = c.bodyA;
        const float invMA = invMass(idA);
        if (invMA <= 0.0f)
          return;
        const F3 invIA = ldF3(invInertia, idA);
        const F3 rA{c.rA.x, c.rA.y, c.rA.z}, n{c.normal.x, c.normal.y, c.normal.z};
        const F3 vAc = add3(ldF3(velPred, idA), cross3v(ldF3(angVelPred, idA), rA));
        // Relative to the wall's surface velocity (0 for a static plane), so a wall pressing into a
        // grain along its normal (a vibrating tray) registers the normal load that bounds friction.
        const F3 vWall{c.boundaryVel.x, c.boundaryVel.y, c.boundaryVel.z};
        const float approach = -dot3(sub3(vAc, vWall), n);
        if (approach <= 0.0f)
          return;
        const float w_n = computeW(rA, n, invMA, invIA);
        contacts(idx).friction_lambda_n = (w_n > 1e-6f) ? approach / w_n : 0.0f;
        Kokkos::atomic_max(&planeFriction(idA, 0), approach / invMA);
      });
  space.fence();
}
 
inline void countFrictionContactsKokkos(Kokkos::View<const ContactC*, CpMem> contacts,
                                        int numContacts, Kokkos::View<const int*, CpMem> realIdx,
                                        FrManifoldCounts planeFriction) {
  CpExec space;
  Kokkos::parallel_for(
      "peclet::dem::fric_count", Kokkos::RangePolicy<CpExec>(space, 0, numContacts),
      KOKKOS_LAMBDA(int idx) {
        const ContactC c = contacts(idx);
        if (c.friction_lambda_n <= 0.0f)
          return;
        Kokkos::atomic_add(&planeFriction(realIdx(c.bodyA), 1), 1.0f);
        if (c.bodyB >= 0)
          Kokkos::atomic_add(&planeFriction(realIdx(c.bodyB), 1), 1.0f);
      });
  space.fence();
}
 
inline void solveContactFrictionKokkos(
    Kokkos::View<const ContactC*, CpMem> contacts, int numContacts,
    Kokkos::View<const float*, CpMem> invMass, Kokkos::View<const float* [3], CpMem> invInertia,
    Kokkos::View<const float* [4], CpMem> quat, Kokkos::View<const float* [3], CpMem> velPred,
    Kokkos::View<const float* [3], CpMem> angVelPred, Kokkos::View<const int*, CpMem> realIdx,
    FrManifoldCounts planeFriction, float frictionDynamic, Kokkos::View<float* [3], CpMem> deltaVel,
    Kokkos::View<float* [3], CpMem> deltaAngVel) {
  CpExec space;
  Kokkos::deep_copy(space, deltaVel, 0.0f);
  Kokkos::deep_copy(space, deltaAngVel, 0.0f);
  Kokkos::parallel_for(
      "peclet::dem::fric_solve", Kokkos::RangePolicy<CpExec>(space, 0, numContacts),
      KOKKOS_LAMBDA(int idx) {
        const ContactC c = contacts(idx);
        const float lambda_n = c.friction_lambda_n;
        if (lambda_n <= 0.0f)
          return;
        const int idA = c.bodyA, idB = c.bodyB;
        const int realA = realIdx(idA);
        const int realB = (idB >= 0) ? realIdx(idB) : -1;
        const float invMA = invMass(realA);
        const float invMB = (idB >= 0) ? invMass(realB) : 0.0f;
        const F3 invIA = ldF3(invInertia, realA);
        const F3 invIB = (idB >= 0) ? ldF3(invInertia, realB) : F3{0, 0, 0};
        F3 rA{c.rA.x, c.rA.y, c.rA.z}, rB{c.rB.x, c.rB.y, c.rB.z};
        if (idB >= 0)
          contactArmsMid(c, rA, rB);  // single application point; a wall keeps its arm
        const F3 n{c.normal.x, c.normal.y, c.normal.z};
 
        const F3 vAc = add3(ldF3(velPred, realA), cross3v(ldF3(angVelPred, realA), rA));
        F3 vBc{0, 0, 0};
        if (idB >= 0)
          vBc = add3(ldF3(velPred, realB), cross3v(ldF3(angVelPred, realB), rB));
        else
          vBc =
              F3{c.boundaryVel.x, c.boundaryVel.y, c.boundaryVel.z};  // moving wall drags the grain
        const F3 vrel = sub3(vAc, vBc);
        const float vn = dot3(vrel, n);
        const F3 vt = sub3(vrel, scale3(n, vn));
        const float vt_len = Kokkos::sqrt(dot3(vt, vt));
        if (vt_len < 1e-7f)
          return;
        const F3 t = scale3(vt, 1.0f / vt_len);
 
        const F3 rnA = cross3v(rA, t), rnB = cross3v(rB, t);
        // Isotropic inverse inertia at an end (spheres; a wall end is 0): no principal frame.
        const bool isoA = invIA.x == invIA.y && invIA.y == invIA.z;
        const bool isoB = invIB.x == invIB.y && invIB.y == invIB.z;
        F4 qA{0, 0, 0, 1}, qB{0, 0, 0, 1};
        if (!isoA)
          qA = F4{quat(realA, 0), quat(realA, 1), quat(realA, 2), quat(realA, 3)};
        if (!isoB)
          qB = F4{quat(realB, 0), quat(realB, 1), quat(realB, 2), quat(realB, 3)};
        const float w_t = (isoA && isoB)
                              ? invMA + invMB + rnA.x * rnA.x * invIA.x + rnA.y * rnA.y * invIA.y +
                                    rnA.z * rnA.z * invIA.z + rnB.x * rnB.x * invIB.x +
                                    rnB.y * rnB.y * invIB.y + rnB.z * rnB.z * invIB.z
                              : invMA + invMB + detail::genInvMass(rnA, invIA, qA) +
                                    detail::genInvMass(rnB, invIB, qB);
        if (w_t < 1e-6f)
          return;
 
        // friction_lambda_n now carries the accumulated force-chain load for BOTH body-body and
        // wall contacts (see accumulateNormalImpulseKokkos), so it is the Coulomb bound in both
        // cases.
        const float bound = lambda_n;
        const float nA = planeFriction(realA, 1);
        const float nB = (idB >= 0) ? planeFriction(realB, 1) : 0.0f;
        const float inv_n = 1.0f / Kokkos::fmax(Kokkos::fmax(nA, nB), 1.0f);
        float lt = -vt_len / w_t;
        // Per-wall AND per-pair friction (a < 0 sentinel keeps the global material — planes and
        // body-body contacts without a pair table).
        const float mu = (c.boundaryFriction >= 0.0f) ? c.boundaryFriction : frictionDynamic;
        const float maxf = mu * bound;
        if (lt < -maxf)
          lt = -maxf;
        lt *= inv_n;
 
        Kokkos::atomic_add(&deltaVel(realA, 0), t.x * lt * invMA);
        Kokkos::atomic_add(&deltaVel(realA, 1), t.y * lt * invMA);
        Kokkos::atomic_add(&deltaVel(realA, 2), t.z * lt * invMA);
        if (isoA) {
          Kokkos::atomic_add(&deltaAngVel(realA, 0), rnA.x * invIA.x * lt);
          Kokkos::atomic_add(&deltaAngVel(realA, 1), rnA.y * invIA.y * lt);
          Kokkos::atomic_add(&deltaAngVel(realA, 2), rnA.z * invIA.z * lt);
        } else {  // I_world^-1 (r x t) lt = R diag(invI) R^T (r x t) lt
          const F3 Jl = invRotateVector(qA, scale3(rnA, lt));
          const F3 dww = rotateVector(qA, F3{Jl.x * invIA.x, Jl.y * invIA.y, Jl.z * invIA.z});
          Kokkos::atomic_add(&deltaAngVel(realA, 0), dww.x);
          Kokkos::atomic_add(&deltaAngVel(realA, 1), dww.y);
          Kokkos::atomic_add(&deltaAngVel(realA, 2), dww.z);
        }
        if (idB >= 0) {
          Kokkos::atomic_add(&deltaVel(realB, 0), -t.x * lt * invMB);
          Kokkos::atomic_add(&deltaVel(realB, 1), -t.y * lt * invMB);
          Kokkos::atomic_add(&deltaVel(realB, 2), -t.z * lt * invMB);
          if (isoB) {
            Kokkos::atomic_add(&deltaAngVel(realB, 0), -rnB.x * invIB.x * lt);
            Kokkos::atomic_add(&deltaAngVel(realB, 1), -rnB.y * invIB.y * lt);
            Kokkos::atomic_add(&deltaAngVel(realB, 2), -rnB.z * invIB.z * lt);
          } else {
            const F3 Jl = invRotateVector(qB, scale3(rnB, -lt));
            const F3 dww = rotateVector(qB, F3{Jl.x * invIB.x, Jl.y * invIB.y, Jl.z * invIB.z});
            Kokkos::atomic_add(&deltaAngVel(realB, 0), dww.x);
            Kokkos::atomic_add(&deltaAngVel(realB, 1), dww.y);
            Kokkos::atomic_add(&deltaAngVel(realB, 2), dww.z);
          }
        }
      });
  space.fence();
}
 
}  // namespace peclet::dem
 
#endif  // DEM_SOLVER_FRICTION_HPP