CableAxialLaw.h

#pragma once
 
#include "sfh/math/math.h"
 
#include <cmath>
 
constexpr double kMaxCompressionSpeedSaturation = 1.0e2;
constexpr double kMinLengthSaturation = 1.0e-3;
constexpr double kMaxLengthSaturation = 1.0e4;
 
namespace cable_axial_law
{
struct Element
{
   double relaxedLength;   
   double stiffness;       
   double stiffnessFactor; 
   double meanTension;     
   double dampingCoeff;    
   double maxTension;      
};
 
struct Tension
{
   double strain;      
   double value;       
   double lengthSlope; 
   double speedSlope;  
   bool   bounded;     
};
 
inline Tension AxialTension(const Element& element, double length, double compressionSpeed)
{
   const double boundedLength = sfh::math::Bound(length, kMinLengthSaturation, kMaxLengthSaturation);
   const double clampedSpeed  = sfh::math::Bound(compressionSpeed, -kMaxCompressionSpeedSaturation, kMaxCompressionSpeedSaturation);
   double       strain        = (boundedLength - element.relaxedLength) / element.relaxedLength;
   if (!(strain == strain))
      strain = 0;
   const double spring  = strain * element.stiffnessFactor * element.stiffness + (1 - element.stiffnessFactor) * element.meanTension;
   const double damping = element.dampingCoeff * clampedSpeed * std::sqrt(element.stiffnessFactor);
   const double sum     = spring + damping;
 
   Tension tension;
   tension.strain       = strain;
   tension.value        = sfh::math::Bound(sum, 0.0, element.maxTension);
   tension.bounded      = sum <= 0.0 || sum >= element.maxTension;
   const bool lengthOut = length < kMinLengthSaturation || length > kMaxLengthSaturation;
   const bool speedOut  = std::fabs(compressionSpeed) >= kMaxCompressionSpeedSaturation;
   tension.lengthSlope  = (tension.bounded || lengthOut) ? 0.0 : element.stiffnessFactor * element.stiffness / element.relaxedLength;
   tension.speedSlope   = (tension.bounded || speedOut) ? 0.0 : element.dampingCoeff * std::sqrt(element.stiffnessFactor);
   return tension;
}
 
inline double SpringTension(const Element& element, double strain)
{
   return sfh::math::Bound(strain * element.stiffnessFactor * element.stiffness + (1.0 - element.stiffnessFactor) * element.meanTension, 0.0,
         element.maxTension);
}
 
inline void EndBlocks(const Tension& tension, const double direction[3], double length, const double relativeVelocity[3], double dFA_dpB[9],
      double dFA_dvA[9])
{
   const double* e = direction;
   double        s = 0.0;
   for (int k = 0; k < 3; k++)
      s += relativeVelocity[k] * e[k];
   double relVelPerp[3];
   for (int k = 0; k < 3; k++)
      relVelPerp[k] = relativeVelocity[k] - e[k] * s;
 
   for (int k = 0; k < 3; k++) {
      for (int j = 0; j < 3; j++) {
         const double dij   = (k == j) ? 1.0 : 0.0;
         dFA_dpB[k * 3 + j] = (dij - e[k] * e[j]) * tension.value / length + e[k] * e[j] * tension.lengthSlope + e[k] * tension.speedSlope * relVelPerp[j] / length;
         dFA_dvA[k * 3 + j] = tension.speedSlope * e[k] * e[j];
      }
   }
}
} // namespace cable_axial_law