NetElement3NForces.h

#pragma once
 
#include <fhsim_environment/hydrodynamics/PanelClampLog.h>
#include <fhsim_environment/hydrodynamics/PanelLoadLaw.h>
 
#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 hydrodynamics::FlowReference kFlowReference = hydrodynamics::FlowReference::FreeStream;
 
 
struct PanelParams
{
   double nodesDistAB_mesh[2]; 
   double nodesDistAC_mesh[2]; 
   double nodesDistBC_mesh[2]; 
   double meshDet;             
   double barL0;               
   double eATwine;             
   double barD;                
   double knotDiameter;        
   double numBarsU;            
   double numBarsV;            
   double numKnots;            
   double NodeWeight;          
   double dampingCoeff;        
   bool twineStructure = true;
};
 
 
template<class T>
struct PanelFrame
{
   T R_ned2panel[3][3]; 
   T meshBarComp[6];    
   T meshBarLength[2];  
   T meshOpeningAngle;  
   T area;              
};
 
 
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>
PanelFrame<T> ComputePanelFrame(const PanelParams& params, const T* const posA_ned,
      const T* const posB_ned, const T* const posC_ned)
{
   using std::acos;
   using std::sqrt;
 
   PanelFrame<T> frame;
   sfh::sim::RMatrixFromThreePoints(posA_ned, posB_ned, posC_ned, frame.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, frame.R_ned2panel, distAB_panel);
   sfh::sim::RotateToLocal(distAC_ned, frame.R_ned2panel, distAC_panel);
 
   CalcLocalUVComponents(params, distAB_panel, distAC_panel, frame.meshBarComp,
         frame.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 += frame.meshBarComp[i] * frame.meshBarComp[i];
      normBarCompV += frame.meshBarComp[i + 3] * frame.meshBarComp[i + 3];
   }
   normBarCompU = sqrt(normBarCompU);
   normBarCompV = sqrt(normBarCompV);
 
   frame.meshOpeningAngle = 0.5 * acos(sfh::linalg::Dot(frame.meshBarComp, frame.meshBarComp + 3, 2ul) / (normBarCompU * normBarCompV));
   if (!(frame.meshOpeningAngle == frame.meshOpeningAngle)) frame.meshOpeningAngle = T(0);
 
   // In the panel frame AB lies along x and the element in the xy-plane.
   frame.area = 0.5 * sfh::math::Abs(distAB_panel[0] * distAC_panel[1] - distAB_panel[1] * distAC_panel[0]);
   return frame;
}
 
 
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>
hydrodynamics::PanelLoad<T> EvaluatePanelLoad(const PanelParams& params, const PanelFrame<T>& frame,
      const hydrodynamics::PanelLoadLaw& law, const hydrodynamics::Fluid& fluid, const T& solidity,
      const T elementVel_ned[3], const T* const waterVel_ned)
{
   hydrodynamics::PanelGeometry<T> geometry;
   geometry.area = frame.area;
 
   hydrodynamics::Netting<T> netting;
   netting.solidity         = solidity;
   netting.twineThickness   = params.barD;
   netting.hasBarDirections = params.meshDet != 0;
   netting.barLength        = params.barL0;
   netting.knotDiameter     = params.knotDiameter;
   netting.numBarsU         = params.numBarsU;
   netting.numBarsV         = params.numBarsV;
   netting.numKnots         = params.numKnots;
 
   hydrodynamics::FlowSample<T> flow;
   flow.reference = kFlowReference;
 
   for (int i = 0; i < 3; i++) {
      geometry.normal[i]       = frame.R_ned2panel[i][2];
      flow.relativeVelocity[i] = waterVel_ned[i] - elementVel_ned[i];
 
      // The bars lie in the panel's xy-plane; rotate them to the global frame and normalise.
      const T barU    = frame.R_ned2panel[i][0] * frame.meshBarComp[0] + frame.R_ned2panel[i][1] * frame.meshBarComp[1];
      const T barV    = frame.R_ned2panel[i][0] * frame.meshBarComp[3] + frame.R_ned2panel[i][1] * frame.meshBarComp[4];
      netting.barU[i] = barU / frame.meshBarLength[0];
      netting.barV[i] = barV / frame.meshBarLength[1];
   }
 
   return hydrodynamics::Evaluate(law, geometry, netting, fluid, flow);
}
 
 
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 PanelFrame<T>& frame,
      const hydrodynamics::PanelLoadLaw& law, const hydrodynamics::Fluid& fluid, const T& solidity,
      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 addedLinearDrag = 0.0,
      const hydrodynamics::PanelClampLog* clampLog = nullptr)
{
   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)};
   if (params.twineStructure)
      CalcTensionForces(params, frame.meshBarComp, frame.meshBarLength, nodeATension_panel,
            nodeBTension_panel, nodeCTension_panel);
 
   T elementVel_ned[3];
   for (int i = 0; i < 3; i++) {
      elementVel_ned[i] = (velA_ned[i] + velB_ned[i] + velC_ned[i]) / 3.0;
   }
 
   const hydrodynamics::PanelLoad<T> load =
         EvaluatePanelLoad(params, frame, law, fluid, solidity, elementVel_ned, waterVel_ned);
   if (clampLog)
      clampLog->Note(load.flags);
 
   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)};
   if (params.twineStructure) {
      CalcDampingForces(params, elementVel_ned, velA_ned, velB_ned, velC_ned,
            dampingForcesNodeA_ned, dampingForcesNodeB_ned, dampingForcesNodeC_ned,
            addedLinearDrag);
 
      // Local z-components of the tension are zero, the element being plane.
      sfh::sim::RotateFromLocalAndAdd(nodeATension_panel, frame.R_ned2panel, nodeAforce_ned);
      sfh::sim::RotateFromLocalAndAdd(nodeBTension_panel, frame.R_ned2panel, nodeBforce_ned);
      sfh::sim::RotateFromLocalAndAdd(nodeCTension_panel, frame.R_ned2panel, nodeCforce_ned);
   }
 
   // The hydrodynamic force is shared equally between the three nodes.
   for (int i = 0; i < 3; i++) {
      const T hydroForcePerNode = load.force[i] / 3.0;
      nodeAforce_ned[i] += hydroForcePerNode;
      nodeBforce_ned[i] += hydroForcePerNode;
      nodeCforce_ned[i] += hydroForcePerNode;
   }
 
   // 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;
 
   if (params.twineStructure) {
      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];
      }
   } else if (addedLinearDrag > 0) {
      // Without twine structure only the added linear drag of CalcDampingForces remains.
      for (unsigned short i = 0; i < 3; i++) {
         nodeAforce_ned[i] -= addedLinearDrag / 3 * velA_ned[i];
         nodeBforce_ned[i] -= addedLinearDrag / 3 * velB_ned[i];
         nodeCforce_ned[i] -= addedLinearDrag / 3 * velC_ned[i];
      }
   }
 
   // The diagnostic below reads raw `double` arrays, so it applies only to the `double`
   // instantiation.
   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