CableSegmentStructuralJacobian.h

#ifndef CABLE_SEGMENT_STRUCTURAL_JACOBIAN_H
#define CABLE_SEGMENT_STRUCTURAL_JACOBIAN_H
 
#include <cmath>
 
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