CableSegmentDragJacobian.h

#ifndef CABLE_SEGMENT_DRAG_JACOBIAN_H
#define CABLE_SEGMENT_DRAG_JACOBIAN_H
 
#include <cmath>
 
inline void AddCableSegmentDragJacobian(const double* pk, const double* pl, const double* vk, const double* current,
    double factor, double crossCoef, double alongCoef, double* dF_dpk, double* dF_dpl, double* dF_dvk)
{
   const double d[3] = {pl[0] - pk[0], pl[1] - pk[1], pl[2] - pk[2]};
   const double w[3] = {vk[0] - current[0], vk[1] - current[1], vk[2] - current[2]};
   const double L    = std::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
   const double W    = std::sqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]);
   if (L < 1e-10 || W <= 0.0)
      return; // degenerate segment, or no relative flow: the derivatives vanish
 
   const double Q[3]       = {d[0] / L, d[1] / L, d[2] / L};
   const double q          = w[0] * Q[0] + w[1] * Q[1] + w[2] * Q[2];
   const double extraAlong = alongCoef - crossCoef;
 
   // F = -factor * W * inner, inner = crossCoef * L * w + extraAlong * q * L * Q
   for (int j = 0; j < 3; ++j) {
      const double inner = crossCoef * L * w[j] + extraAlong * q * L * Q[j];
      for (int k = 0; k < 3; ++k) {
         const double delta = (j == k) ? 1.0 : 0.0;
         // dW/dw = w / W; d(inner)/dw = L * (crossCoef * I + extraAlong * Q Q^T)
         const double dInner_dw = L * (crossCoef * delta + extraAlong * Q[j] * Q[k]);
         const double dF_dw     = -factor * (inner * w[k] / W + W * dInner_dw);
         // d(L w)/dd = w Q^T; d(q L Q)/dd = d((w.d) d / L)/dd = Q w^T + q (I - Q Q^T)
         const double dInner_dd = crossCoef * w[j] * Q[k] + extraAlong * (Q[j] * w[k] + q * (delta - Q[j] * Q[k]));
         const double dF_dd     = -factor * W * dInner_dd;
 
         dF_dvk[j * 3 + k] += dF_dw;
         dF_dpl[j * 3 + k] += dF_dd;
         dF_dpk[j * 3 + k] -= dF_dd;
      }
   }
}
 
#endif