NetElement3NForces.h

#pragma once
 
#include <sfh/linalg/matrix_ops.h>
#include <sfh/math/array.h>
#include <sfh/math/math.h>
#include <sfh/sim/kinematics.h>
#include <sfh/util/diagnostic.h>
 
#include <cmath>
#include <iostream>
#include <type_traits>
 
namespace net_element_forces
{
 
const double kMinMeshBarLength = 1.0e-10;
const double kMinHydDynVel = 1.0e-10;
const double kMaxHydDynVel = 1.0e10;
const double kMaxNormalDrag     = 1.0e100;
const double kMaxTangentialDrag = 1.0e100;
const double kSaturationVel = 500.0;
 
 
struct PanelParams
{
   double nodesDistAB_mesh[2]; 
   double nodesDistAC_mesh[2]; 
   double nodesDistBC_mesh[2]; 
   double meshDet;             
   double barL0;               
   double eATwine;             
   double barD;                
   double ny_water;            
   double TurbLimit;           
   double LamLimit;            
   double CnLinearLimitAngle;  
   double CnLinear;            
   double CnTurb;              
   double CnLam;               
   double kTwineDragNormal[2]; 
   double Ct;                  
   double kKnot;               
   double NodeWeight;          
   double dampingCoeff;        
};
 
 
template<class T>
void CalcLocalUVComponents(const PanelParams& params, const T distAB_panel[3],
      const T distAC_panel[3], T meshBarComp[6], T meshBarLength[2])
{
   using std::sqrt;
 
   meshBarComp[0] = (params.nodesDistAC_mesh[1] * distAB_panel[0] - params.nodesDistAB_mesh[1] * distAC_panel[0]) / params.meshDet;
   meshBarComp[1] = (params.nodesDistAC_mesh[1] * distAB_panel[1] - params.nodesDistAB_mesh[1] * distAC_panel[1]) / params.meshDet;
   meshBarComp[2] = T(0.0);
   meshBarComp[3] = (params.nodesDistAB_mesh[0] * distAC_panel[0] - params.nodesDistAC_mesh[0] * distAB_panel[0]) / params.meshDet;
   meshBarComp[4] = (params.nodesDistAB_mesh[0] * distAC_panel[1] - params.nodesDistAC_mesh[0] * distAB_panel[1]) / params.meshDet;
   meshBarComp[5] = T(0.0);
 
   meshBarLength[0] = sfh::math::Max(
         sqrt(meshBarComp[0] * meshBarComp[0] + meshBarComp[1] * meshBarComp[1]),
         T(kMinMeshBarLength));
   meshBarLength[1] = sfh::math::Max(
         sqrt(meshBarComp[3] * meshBarComp[3] + meshBarComp[4] * meshBarComp[4]),
         T(kMinMeshBarLength));
}
 
 
template<class T>
void CalcTensionForces(const PanelParams& params, const T meshBarComp[6],
      const T meshBarLength[2], T nodeATension_panel[3], T nodeBTension_panel[3],
      T nodeCTension_panel[3])
{
   // The original multiplied by the `bool` of this comparison. That promotes to 1.0 or 0.0
   // for a built-in type but discards the derivative of a differentiable one, so the branch
   // is written out. The retained branch is the original expression unchanged, and
   // multiplying it by an exact 1.0 was exact, so the `double` value is unaffected.
   T twineTension[2];
   twineTension[0] = (meshBarLength[0] > params.barL0)
                           ? params.eATwine * (meshBarLength[0] - params.barL0) / params.barL0
                           : T(0.0);
   twineTension[1] = (meshBarLength[1] > params.barL0)
                           ? params.eATwine * (meshBarLength[1] - params.barL0) / params.barL0
                           : T(0.0);
   twineTension[0] = sfh::math::Max(twineTension[0], T(0.0));
   twineTension[1] = sfh::math::Max(twineTension[1], T(0.0));
 
   nodeATension_panel[0] = params.nodesDistBC_mesh[1] * twineTension[0] * meshBarComp[0] / (2 * meshBarLength[0]) - params.nodesDistBC_mesh[0] * twineTension[1] * meshBarComp[3] / (2 * meshBarLength[1]);
   nodeATension_panel[1] = params.nodesDistBC_mesh[1] * twineTension[0] * meshBarComp[1] / (2 * meshBarLength[0]) - params.nodesDistBC_mesh[0] * twineTension[1] * meshBarComp[4] / (2 * meshBarLength[1]);
   nodeATension_panel[2] = T(0.0);
 
   nodeBTension_panel[0] = -params.nodesDistAC_mesh[1] * twineTension[0] * meshBarComp[0] / (2 * meshBarLength[0]) + params.nodesDistAC_mesh[0] * twineTension[1] * meshBarComp[3] / (2 * meshBarLength[1]);
   nodeBTension_panel[1] = -params.nodesDistAC_mesh[1] * twineTension[0] * meshBarComp[1] / (2 * meshBarLength[0]) + params.nodesDistAC_mesh[0] * twineTension[1] * meshBarComp[4] / (2 * meshBarLength[1]);
   nodeBTension_panel[2] = T(0.0);
 
   nodeCTension_panel[0] = params.nodesDistAB_mesh[1] * twineTension[0] * meshBarComp[0] / (2 * meshBarLength[0]) - params.nodesDistAB_mesh[0] * twineTension[1] * meshBarComp[3] / (2 * meshBarLength[1]);
   nodeCTension_panel[1] = params.nodesDistAB_mesh[1] * twineTension[0] * meshBarComp[1] / (2 * meshBarLength[0]) - params.nodesDistAB_mesh[0] * twineTension[1] * meshBarComp[4] / (2 * meshBarLength[1]);
   nodeCTension_panel[2] = T(0.0);
}
 
 
template<class T>
T CalcVelocities(const PanelParams& params, const T& meshOpeningAngle,
      const T R_ned2panel[3][3], const T* const velA_ned, const T* const velB_ned,
      const T* const velC_ned, const T* const waterVel_ned, T elementVel_ned[3],
      T hydDynVel_ned[3], T hydDynVel_panel[3])
{
   using std::cos;
   using std::sin;
   using std::sqrt;
 
   for (int i = 0; i < 3; i++) {
      elementVel_ned[i] = (velA_ned[i] + velB_ned[i] + velC_ned[i]) / 3.0;
   }
 
   const T speed = sfh::math::Norm(elementVel_ned, 3);
   if (speed > kSaturationVel) {
      sfh::math::Normalize3(elementVel_ned);
      for (unsigned short i = 0; i < 3; i++) {
         elementVel_ned[i] = elementVel_ned[i] * kSaturationVel;
      }
   }
 
   // Default value for the effect of knots on solidity; should be specified when applicable.
   const double knotFactor = 1.1;
 
   T Sn = knotFactor * (params.barD) / (params.barL0 * sin(meshOpeningAngle) * cos(meshOpeningAngle));
 
   // Keeps Sn finite and within the bounds given by Kristiansen and Faltinsen (2012). The
   // comparisons are on the value, so they select a branch and say nothing about any
   // derivative the scalar type carries; in the clamped branches the solidity is a constant
   // and its derivative is correctly zero.
   if (!(Sn == Sn) || Sn + 1.0 == Sn) Sn = T(0.1); // Check for NaN or infinity.
   if (Sn > 0.5) Sn = T(0.5);
   if (Sn <= 0) Sn = T(0.0001);
 
   const T snFactor = sqrt(2 - Sn) / (sqrt(2.0) * (1 - Sn));
 
   T waterVelModified_ned[3];
   waterVelModified_ned[0] = waterVel_ned[0] * snFactor;
   waterVelModified_ned[1] = waterVel_ned[1] * snFactor;
   waterVelModified_ned[2] = waterVel_ned[2] * snFactor;
 
   sfh::math::ArraySubtract(elementVel_ned, waterVelModified_ned, hydDynVel_ned, 3ul);
   sfh::sim::RotateToLocal(hydDynVel_ned, R_ned2panel, hydDynVel_panel);
 
   return sfh::math::Bound(sfh::math::Norm(hydDynVel_panel, 3), T(kMinHydDynVel),
         T(kMaxHydDynVel));
}
 
 
template<class T>
void CalcHydroDynamicForces(const PanelParams& params, const T& meshOpeningAngle,
      const T meshBarComp[6], const T meshBarLength[2], const T R_ned2panel[3][3],
      const T* const velA_ned, const T* const velB_ned, const T* const velC_ned,
      const T* const waterVel_ned, T elementVel_ned[3], T totalDragPerNode_panel[3])
{
   using std::asin;
   using std::log10;
 
   T       hydDynVel_ned[3];
   T       hydDynVel_panel[3];
   const T hydDynVel_absolute = CalcVelocities(params, meshOpeningAngle, R_ned2panel,
         velA_ned, velB_ned, velC_ned, waterVel_ned, elementVel_ned, hydDynVel_ned,
         hydDynVel_panel);
 
   // The local z-component of the mesh bars is zero, the element being plane, so the local
   // z-component of the velocity does not contribute to these products.
   T hydDynVel_dot_meshBarComp[2];
   hydDynVel_dot_meshBarComp[0] = sfh::linalg::Dot(hydDynVel_panel, meshBarComp, 2ul);
   hydDynVel_dot_meshBarComp[1] = sfh::linalg::Dot(hydDynVel_panel, meshBarComp + 3, 2ul);
 
   const T k0 = hydDynVel_dot_meshBarComp[0] / (meshBarLength[0] * meshBarLength[0]);
   const T k1 = hydDynVel_dot_meshBarComp[1] / (meshBarLength[1] * meshBarLength[1]);
 
   T hydDynVel_tangential_panel[6];
   for (int i = 0; i < 3; i++) {
      hydDynVel_tangential_panel[i]     = k0 * meshBarComp[i];
      hydDynVel_tangential_panel[i + 3] = k1 * meshBarComp[i + 3];
   }
 
   T hydDynVel_tangential_absolute[2];
   hydDynVel_tangential_absolute[0] = sfh::math::Max(T(kMinHydDynVel),
         sfh::math::Norm(hydDynVel_tangential_panel, 3));
   hydDynVel_tangential_absolute[1] = sfh::math::Max(T(kMinHydDynVel),
         sfh::math::Norm(hydDynVel_tangential_panel + 3, 3));
 
   T hydDynVel_normal_panel[6];
   sfh::math::ArraySubtract(hydDynVel_panel, hydDynVel_tangential_panel,
         hydDynVel_normal_panel, 3ul);
   sfh::math::ArraySubtract(hydDynVel_panel, hydDynVel_tangential_panel + 3,
         hydDynVel_normal_panel + 3, 3ul);
 
   T hydDynVel_normal_absolute[2];
   hydDynVel_normal_absolute[0] = sfh::math::Max(T(kMinHydDynVel),
         sfh::math::Norm(hydDynVel_normal_panel, 3));
   hydDynVel_normal_absolute[1] = sfh::math::Max(T(kMinHydDynVel),
         sfh::math::Norm(hydDynVel_normal_panel + 3, 3));
 
   // The angle of attack between the effective hydrodynamic velocity and each bar family.
   T sin_hydDynAngle[2];
   sin_hydDynAngle[0] = hydDynVel_normal_absolute[0] / hydDynVel_absolute;
   sin_hydDynAngle[1] = hydDynVel_normal_absolute[1] / hydDynVel_absolute;
 
   for (int i = 0; i < 2; i++) {
      if (sin_hydDynAngle[i] > 1) {
         sin_hydDynAngle[i] = T(1);
      } else if (sin_hydDynAngle[i] < -1) {
         sin_hydDynAngle[i] = T(-1);
      }
   }
 
   T hydDynAngle[2];
   hydDynAngle[0] = asin(sin_hydDynAngle[0]);
   hydDynAngle[1] = asin(sin_hydDynAngle[1]);
 
   // Reynolds number from the absolute velocity and the twine diameter, not accounting for
   // the angle between the twines and the effective hydrodynamic velocity.
   const T nominalReynoldsNumber = hydDynVel_absolute * params.barD / params.ny_water;
 
   T Cn[2];
   if ((nominalReynoldsNumber >= 32) && (nominalReynoldsNumber < 10000)) {
      // Seventh order polynomial fit of the drag coefficient of a smooth cylinder against
      // the Reynolds number (Kristiansen and Faltinsen, 2012).
      const T log10Re = log10(nominalReynoldsNumber);
      const T pow2Re  = log10Re * log10Re;
      const T pow3Re  = pow2Re * log10Re;
      const T pow4Re  = pow2Re * pow2Re;
      const T pow5Re  = pow2Re * pow3Re;
      const T pow6Re  = pow3Re * pow3Re;
      const T pow7Re  = pow3Re * pow4Re;
 
      const double A  = -78.46675;
      const double B1 = 254.73873;
      const double B2 = -327.8864;
      const double B3 = 223.64577;
      const double B4 = -87.92234;
      const double B5 = 20.00769;
      const double B6 = -2.44894;
      const double B7 = 0.12479;
 
      Cn[0] = A + B1 * log10Re + B2 * pow2Re + B3 * pow3Re + B4 * pow4Re + B5 * pow5Re + B6 * pow6Re + B7 * pow7Re;
      Cn[0] = Cn[0] * sin_hydDynAngle[0];
      Cn[1] = A + B1 * log10Re + B2 * pow2Re + B3 * pow3Re + B4 * pow4Re + B5 * pow5Re + B6 * pow6Re + B7 * pow7Re;
      Cn[1] = Cn[1] * sin_hydDynAngle[1];
   } else {
      // A linear model below four degrees and the modified cross-flow principle above it,
      // for the Reynolds numbers where the polynomial fit is invalid. Both limits account
      // for the twines' angle to the flow lengthening the hydrodynamic length.
      const T angleTurb = asin(nominalReynoldsNumber / params.TurbLimit);
      const T angleLam  = asin(nominalReynoldsNumber / params.LamLimit);
 
      for (int i = 0; i < 2; i++) {
         if (hydDynAngle[i] < params.CnLinearLimitAngle) {
            Cn[i] = T(params.CnLinear);
         } else if (hydDynAngle[i] < angleTurb) {
            Cn[i] = params.CnTurb * sin_hydDynAngle[i];
         } else if (hydDynAngle[i] <= angleLam) {
            Cn[i] = (params.CnTurb + (hydDynAngle[i] - angleTurb) / (angleLam - angleTurb) * (params.CnLam - params.CnTurb)) * sin_hydDynAngle[i];
         } else {
            Cn[i] = params.CnLam * sin_hydDynAngle[i];
         }
      }
   }
 
   T twineDragNormal[2];
   T twineDragTangential[2];
   twineDragNormal[0]     = params.kTwineDragNormal[0] * hydDynVel_absolute * Cn[0];
   twineDragNormal[1]     = params.kTwineDragNormal[1] * hydDynVel_absolute * Cn[1];
   twineDragTangential[0] = params.kTwineDragNormal[0] * hydDynVel_absolute * params.Ct;
   twineDragTangential[1] = params.kTwineDragNormal[1] * hydDynVel_absolute * params.Ct;
 
   twineDragNormal[0] =
         sfh::math::Bound(twineDragNormal[0], T(-kMaxNormalDrag), T(kMaxNormalDrag));
   twineDragNormal[1] =
         sfh::math::Bound(twineDragNormal[1], T(-kMaxNormalDrag), T(kMaxNormalDrag));
   twineDragTangential[0] = sfh::math::Bound(twineDragTangential[0],
         T(-kMaxTangentialDrag), T(kMaxTangentialDrag));
   twineDragTangential[1] = sfh::math::Bound(twineDragTangential[1],
         T(-kMaxTangentialDrag), T(kMaxTangentialDrag));
 
   // Drag on the knots acts along the effective hydrodynamic velocity, with no tangential
   // component.
   const T knotDrag = params.kKnot * hydDynVel_absolute * hydDynVel_absolute;
 
   T twineDragNormal_panel[6];
   T twineDragTangential_panel[6];
   for (int i = 0; i < 3; i++) {
      twineDragNormal_panel[i]     = -twineDragNormal[0] * hydDynVel_normal_panel[i];
      twineDragNormal_panel[i + 3] = -twineDragNormal[1] * hydDynVel_normal_panel[i + 3];
      twineDragTangential_panel[i] =
            -twineDragTangential[0] * hydDynVel_tangential_panel[i];
      twineDragTangential_panel[i + 3] =
            -twineDragTangential[1] * hydDynVel_tangential_panel[i + 3];
   }
 
   T knotDrag_panel[3];
   knotDrag_panel[0] = -knotDrag * hydDynVel_panel[0] / hydDynVel_absolute;
   knotDrag_panel[1] = -knotDrag * hydDynVel_panel[1] / hydDynVel_absolute;
   knotDrag_panel[2] = -knotDrag * hydDynVel_panel[2] / hydDynVel_absolute;
 
   for (unsigned short i = 0; i < 3; i++) {
      totalDragPerNode_panel[i] = (twineDragNormal_panel[i] + twineDragNormal_panel[i + 3] + twineDragTangential_panel[i] + twineDragTangential_panel[i + 3] + knotDrag_panel[i]) / 3;
   }
 
   // Retained because the original computed them and a reader will look for them; the
   // tangential magnitudes are not used in the force itself.
   (void)hydDynVel_tangential_absolute;
}
 
 
template<class T>
void CalcDampingForces(const PanelParams& params, const T elementVel_ned[3],
      const T* const velA_ned, const T* const velB_ned, const T* const velC_ned,
      T dampingForcesNodeA_ned[3], T dampingForcesNodeB_ned[3],
      T dampingForcesNodeC_ned[3], double addedLinearDrag)
{
   for (unsigned short i = 0; i < 3; i++) {
      dampingForcesNodeA_ned[i] = params.dampingCoeff * (elementVel_ned[i] - velA_ned[i]);
      dampingForcesNodeB_ned[i] = params.dampingCoeff * (elementVel_ned[i] - velB_ned[i]);
      dampingForcesNodeC_ned[i] = params.dampingCoeff * (elementVel_ned[i] - velC_ned[i]);
   }
 
   if (addedLinearDrag > 0) {
      for (unsigned short i = 0; i < 3; i++) {
         dampingForcesNodeA_ned[i] -= addedLinearDrag / 3 * velA_ned[i];
         dampingForcesNodeB_ned[i] -= addedLinearDrag / 3 * velB_ned[i];
         dampingForcesNodeC_ned[i] -= addedLinearDrag / 3 * velC_ned[i];
      }
   }
}
 
 
template<class T>
void AddNodeForces(const PanelParams& params, const T* const posA_ned,
      const T* const posB_ned, const T* const posC_ned, const T* const velA_ned,
      const T* const velB_ned, const T* const velC_ned, const T* const waterVel_ned,
      T* const nodeAforce_ned, T* const nodeBforce_ned, T* const nodeCforce_ned,
      double hydroForcesRatio = 1.0, double addedLinearDrag = 0.0)
{
   using std::acos;
   using std::sqrt;
 
   // The panel x-axis lies along the AB side, the y-axis spans towards C, and the z-axis
   // completes a right-handed system.
   T R_ned2panel[3][3];
   sfh::sim::RMatrixFromThreePoints(posA_ned, posB_ned, posC_ned, R_ned2panel);
 
   // The panel-frame separations. They are held as 3-vectors although the third component
   // is always zero, so that the standard rotation and array routines apply.
   T distAB_ned[3], distAC_ned[3];
   T distAB_panel[3], distAC_panel[3];
   sfh::math::ArraySubtract(posB_ned, posA_ned, distAB_ned, 3ul);
   sfh::math::ArraySubtract(posC_ned, posA_ned, distAC_ned, 3ul);
   sfh::sim::RotateToLocal(distAB_ned, R_ned2panel, distAB_panel);
   sfh::sim::RotateToLocal(distAC_ned, R_ned2panel, distAC_panel);
 
   T meshBarComp_panel[6];
   T meshBarLength[2];
   CalcLocalUVComponents(params, distAB_panel, distAC_panel, meshBarComp_panel,
         meshBarLength);
 
   // The mesh half opening angle, from the scalar product of the u- and v-bar vectors
   // (Priour, 2001, p. 232). The positive mesh directions must be defined so that the half
   // angle is the one between bars that would naturally tend to close.
   T normBarCompU = T(0);
   T normBarCompV = T(0);
   for (int i = 0; i < 3; i++) {
      normBarCompU += meshBarComp_panel[i] * meshBarComp_panel[i];
      normBarCompV += meshBarComp_panel[i + 3] * meshBarComp_panel[i + 3];
   }
   normBarCompU = sqrt(normBarCompU);
   normBarCompV = sqrt(normBarCompV);
 
   T meshOpeningAngle = 0.5 * acos(sfh::linalg::Dot(meshBarComp_panel, meshBarComp_panel + 3, 2ul) / (normBarCompU * normBarCompV));
   if (!(meshOpeningAngle == meshOpeningAngle)) meshOpeningAngle = T(0);
 
   T nodeATension_panel[3] = {T(0), T(0), T(0)};
   T nodeBTension_panel[3] = {T(0), T(0), T(0)};
   T nodeCTension_panel[3] = {T(0), T(0), T(0)};
   CalcTensionForces(params, meshBarComp_panel, meshBarLength, nodeATension_panel,
         nodeBTension_panel, nodeCTension_panel);
 
   T elementVel_ned[3];
   T totalDragPerNode_panel[3];
   CalcHydroDynamicForces(params, meshOpeningAngle, meshBarComp_panel, meshBarLength,
         R_ned2panel, velA_ned, velB_ned, velC_ned, waterVel_ned, elementVel_ned,
         totalDragPerNode_panel);
 
   T dampingForcesNodeA_ned[3] = {T(0), T(0), T(0)};
   T dampingForcesNodeB_ned[3] = {T(0), T(0), T(0)};
   T dampingForcesNodeC_ned[3] = {T(0), T(0), T(0)};
   CalcDampingForces(params, elementVel_ned, velA_ned, velB_ned, velC_ned,
         dampingForcesNodeA_ned, dampingForcesNodeB_ned, dampingForcesNodeC_ned,
         addedLinearDrag);
 
   // The hydrodynamic force is shared equally between the three nodes. Local z-components
   // of the tension are zero, the element being plane.
   T nodeAforce_panel[3];
   T nodeBforce_panel[3];
   T nodeCforce_panel[3];
   for (int i = 0; i < 3; i++) {
      nodeAforce_panel[i] = nodeATension_panel[i];
      nodeBforce_panel[i] = nodeBTension_panel[i];
      nodeCforce_panel[i] = nodeCTension_panel[i];
 
      nodeAforce_panel[i] += hydroForcesRatio * totalDragPerNode_panel[i];
      nodeBforce_panel[i] += hydroForcesRatio * totalDragPerNode_panel[i];
      nodeCforce_panel[i] += hydroForcesRatio * totalDragPerNode_panel[i];
   }
 
   sfh::sim::RotateFromLocalAndAdd(nodeAforce_panel, R_ned2panel, nodeAforce_ned);
   sfh::sim::RotateFromLocalAndAdd(nodeBforce_panel, R_ned2panel, nodeBforce_ned);
   sfh::sim::RotateFromLocalAndAdd(nodeCforce_panel, R_ned2panel, nodeCforce_ned);
 
   // The weight in water of the netting, distributed equally between the three nodes.
   nodeAforce_ned[2] += params.NodeWeight;
   nodeBforce_ned[2] += params.NodeWeight;
   nodeCforce_ned[2] += params.NodeWeight;
 
   for (unsigned short i = 0; i < 3; i++) {
      nodeAforce_ned[i] += dampingForcesNodeA_ned[i];
      nodeBforce_ned[i] += dampingForcesNodeB_ned[i];
      nodeCforce_ned[i] += dampingForcesNodeC_ned[i];
   }
 
   // The diagnostic below reads raw `double` arrays, so it applies only to the `double`
   // instantiation. It is unchanged in that case.
   if constexpr (std::is_same_v<T, double>) {
      unsigned long bad;
      if ((bad = sfh::util::FindBadValues(nodeAforce_ned, 3)) != (unsigned long)-1)
         std::cout << "Bad element in nodeAforce_ned: " << bad << std::endl;
      if ((bad = sfh::util::FindBadValues(nodeBforce_ned, 3)) != (unsigned long)-1)
         std::cout << "Bad element in nodeBforce_ned: " << bad << std::endl;
      if ((bad = sfh::util::FindBadValues(nodeCforce_ned, 3)) != (unsigned long)-1)
         std::cout << "Bad element in nodeCforce_ned: " << bad << std::endl;
   }
}
 
} // namespace net_element_forces