CableSegmentDrag.h

#ifndef CABLE_SEGMENT_DRAG_H
#define CABLE_SEGMENT_DRAG_H
 
#include <cmath>
#include <fhsim_environment/hydrodynamics/CylinderHydro.h>
 
namespace cable_segment_drag
{
inline bool SegmentAxis(const double* pk, const double* pl, double& length, double axis[3])
{
   const double d[3] = {pl[0] - pk[0], pl[1] - pk[1], pl[2] - pk[2]};
   length            = std::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
   if (length < 1e-10)
      return false;
   for (int j = 0; j < 3; ++j)
      axis[j] = d[j] / length;
   return true;
}
 
inline void Force(const double* pk, const double* pl, const double* vk, const double* current, double rho, double D, double force[3])
{
   force[0] = force[1] = force[2] = 0.0;
   double length, axis[3];
   if (!SegmentAxis(pk, pl, length, axis))
      return;
   const double w[3] = {vk[0] - current[0], vk[1] - current[1], vk[2] - current[2]};
   namespace cylinder_hydro = environment::cylinder_hydro;
   cylinder_hydro::DragForce(rho, D, 0.5 * length, cylinder_hydro::kWaterKinematicViscosity, w, axis, cylinder_hydro::DragFormulation::Independence, force);
}
 
inline void AddJacobian(const double* pk, const double* pl, const double* vk, const double* current, double rho, double D,
    double* dF_dpk, double* dF_dpl, double* dF_dvk)
{
   double length, axis[3];
   if (!SegmentAxis(pk, pl, length, axis))
      return;
   const double w[3] = {vk[0] - current[0], vk[1] - current[1], vk[2] - current[2]};
   namespace cylinder_hydro = environment::cylinder_hydro;
   const double halfLength  = 0.5 * length;
   const double nu          = cylinder_hydro::kWaterKinematicViscosity;
   const auto   formulation = cylinder_hydro::DragFormulation::Independence;
   double force[3], dF_dw[9], dF_daxis[9];
   cylinder_hydro::DragForce(rho, D, halfLength, nu, w, axis, formulation, force);
   cylinder_hydro::DragForceJacobian(rho, D, halfLength, nu, w, axis, formulation, dF_dw, dF_daxis);
 
   for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 3; ++k) {
         // F is linear in the length |d| / 2: dF/d|d| = F / |d|.
         double dF_dd = force[j] * axis[k] / length;
         for (int m = 0; m < 3; ++m)
            dF_dd += dF_daxis[j * 3 + m] * (((m == k) ? 1.0 : 0.0) - axis[m] * axis[k]) / length;
         dF_dvk[j * 3 + k] += dF_dw[j * 3 + k];
         dF_dpl[j * 3 + k] += dF_dd;
         dF_dpk[j * 3 + k] -= dF_dd;
      }
   }
}
} // namespace cable_segment_drag
 
#endif