CableSegmentStructuralJacobian.h

#ifndef CABLE_SEGMENT_STRUCTURAL_JACOBIAN_H
#define CABLE_SEGMENT_STRUCTURAL_JACOBIAN_H
 
#include <Eigen/Core>
#include <cmath>
 
struct CableSegmentStructuralForces
{
   Eigen::Vector3d spring;
   Eigen::Vector3d damper;
};
 
inline CableSegmentStructuralForces CableSegmentStructuralForce(const Eigen::Vector3d& pk, const Eigen::Vector3d& pl,
    const Eigen::Vector3d& vk, const Eigen::Vector3d& vl, double restLength, double springCoef, double damperCoef)
{
   Eigen::Vector3d pklbar = pl - pk;
   pklbar.normalize();
 
   CableSegmentStructuralForces force;
   if ((pl - pk).norm() < restLength)
      force.spring = Eigen::Vector3d::Zero();
   else
      force.spring = springCoef * ((pl - pk).norm() - restLength) * pklbar;
   force.damper = damperCoef * ((vl - vk).dot(pklbar)) * pklbar;
   return force;
}
 
inline void SetCableSegmentStructuralJacobian(const double* pk, const double* pl, const double* vk, const double* vl,
    double restLength, double springCoef, double damperCoef, double* dF_dpk, double* dF_dpl, double* dF_dvk,
    double* dF_dvl)
{
   const double d[3] = {pk[0] - pl[0], pk[1] - pl[1], pk[2] - pl[2]};
   const double L2   = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
 
   for (int i = 0; i < 9; ++i)
      dF_dpk[i] = dF_dpl[i] = dF_dvk[i] = dF_dvl[i] = 0.0;
   if (L2 < 1e-20)
      return;
 
   const double L    = std::sqrt(L2);
   const double Linv = 1.0 / L;
   const double Q[3] = {d[0] * Linv, d[1] * Linv, d[2] * Linv};
 
   const double dV[3]  = {vk[0] - vl[0], vk[1] - vl[1], vk[2] - vl[2]};
   const double QdotdV = Q[0] * dV[0] + Q[1] * dV[1] + Q[2] * dV[2];
 
   const bool inTension = (L > restLength);
   const double tension = inTension ? springCoef * (L - restLength) : 0.0;
   const double TD      = tension + damperCoef * QdotdV;
 
   const double extension = inTension ? (L - restLength) / restLength : 0.0;
   const double dT_dL     = (extension > 1e-8) ? springCoef : 0.0;
 
   // F[j] = -TD * Q[j]: dF[j]/dpk[k] = -dTD/dpk[k] * Q[j] - TD * dQ[j]/dpk[k]
   for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 3; ++k) {
         const double dTD_dpk = dT_dL * Q[k] + damperCoef * (dV[k] - QdotdV * Q[k]) * Linv;
         const double dQj_dpk = ((j == k ? 1.0 : 0.0) - Q[j] * Q[k]) * Linv;
 
         dF_dpk[j * 3 + k] = -dTD_dpk * Q[j] - TD * dQj_dpk;
         dF_dpl[j * 3 + k] = -dF_dpk[j * 3 + k]; // d = pk - pl
         dF_dvk[j * 3 + k] = -damperCoef * Q[k] * Q[j];
         dF_dvl[j * 3 + k] = damperCoef * Q[k] * Q[j];
      }
   }
}
 
#endif