GaussPointDrag.h

#pragma once
 
#include "WetCylinderWater.h"
 
#include <fhsim_environment/ElementFlowInteraction.h>
#include <fhsim_environment/hydrodynamics/WetCylinderLaw.h>
 
#include <Eigen/Eigen>
 
#include <algorithm>
#include <array>
#include <cmath>
#include <vector>
 
namespace wet_cylinder_water
{
using environment::wet_cylinder::EndWater; 
 
struct GaussDragLoad
{
   Eigen::Vector3d force  = Eigen::Vector3d::Zero(); 
   Eigen::Vector3d moment = Eigen::Vector3d::Zero(); 
};
 
class ChainJoints
{
public:
   void Begin(double length, bool closed = false)
   {
      m_length = length;
      m_closed = closed;
      m_centres.clear();
      m_axes.clear();
   }
 
   void Add(const Eigen::Vector3d& centre, const Eigen::Vector3d& axis)
   {
      m_centres.push_back(centre);
      m_axes.push_back(axis);
   }
 
   void Build()
   {
      const int numElements = NumElements();
      m_ends.resize(numElements);
      const int numJoints = m_closed ? numElements : numElements + 1;
      m_positions.assign(numElements > 0 ? numJoints : 0, Eigen::Vector3d::Zero());
      if (numElements == 0)
         return;
      if (m_closed) {
         for (int i = 0; i < numElements; i++)
            AssignEnds(i, NearerEnd(i, m_centres[(i + 1) % numElements]), i, (i + numElements - 1) % numElements);
         for (int i = 0; i < numElements; i++)
            for (int end = 0; end < 2; end++)
               m_positions[m_ends[i][end]] += EndPosition(i, end);
         for (Eigen::Vector3d& position : m_positions)
            position *= 0.5; // two ends meet at every joint: their midpoint
         return;
      }
      if (numElements == 1) {
         m_ends[0] = {0, 1};
      } else {
         for (int i = 0; i < numElements - 1; i++)
            AssignEnds(i, NearerEnd(i, m_centres[i + 1]), i + 1, i);
         const int last = numElements - 1;
         AssignEnds(last, NearerEnd(last, m_centres[last - 1]), last, numElements);
      }
      for (int i = 0; i < numElements; i++)
         for (int end = 0; end < 2; end++)
            m_positions[m_ends[i][end]] += EndPosition(i, end);
      for (int joint = 1; joint < numElements; joint++)
         m_positions[joint] *= 0.5; // two ends meet: their midpoint
   }
 
   int NumElements() const { return static_cast<int>(m_centres.size()); }
 
   int NumJoints() const { return static_cast<int>(m_positions.size()); }
 
   const Eigen::Vector3d& JointPosition(int joint) const { return m_positions[joint]; }
 
   int JointOfEnd(int element, int end) const { return m_ends[element][end]; }
 
   void AppendJointPositions(std::vector<double>& points) const
   {
      points.reserve(points.size() + 3 * m_positions.size());
      for (const Eigen::Vector3d& position : m_positions)
         points.insert(points.end(), position.data(), position.data() + 3);
   }
 
   void QueryWater(const environment::wake::ElementFlowInteraction& flow, double T, int firstPoint)
   {
      m_water.resize(m_positions.size());
      for (int joint = 0; joint < NumJoints(); joint++) {
         m_water[joint].setZero();
         flow.WaterVelocity(T, firstPoint + joint, m_positions[joint].data(), m_water[joint].data());
      }
   }
 
   EndWater WaterOf(int element) const
   {
      const Eigen::Vector3d& minus = m_water[m_ends[element][0]];
      const Eigen::Vector3d& plus  = m_water[m_ends[element][1]];
      return {{minus(0), minus(1), minus(2)}, {plus(0), plus(1), plus(2)}};
   }
 
private:
   Eigen::Vector3d EndPosition(int element, int end) const { return m_centres[element] + (end == 1 ? 0.5 : -0.5) * m_length * m_axes[element]; }
 
   int NearerEnd(int element, const Eigen::Vector3d& target) const
   {
      return (EndPosition(element, 1) - target).squaredNorm() < (EndPosition(element, 0) - target).squaredNorm() ? 1 : 0;
   }
 
   void AssignEnds(int element, int touchingEnd, int touchingJoint, int otherJoint)
   {
      m_ends[element][touchingEnd]     = touchingJoint;
      m_ends[element][1 - touchingEnd] = otherJoint;
   }
 
   double                              m_length = 0.0;
   bool                                m_closed = false; 
   std::vector<Eigen::Vector3d>        m_centres;   
   std::vector<Eigen::Vector3d>        m_axes;      
   std::vector<std::array<int, 2>>     m_ends;      
   std::vector<Eigen::Vector3d>        m_positions; 
   std::vector<Eigen::Vector3d>        m_water;     
};
 
template<class DragModel = environment::wet_cylinder::ReynoldsDrag>
inline GaussDragLoad GaussPointDragLoad(const environment::wet_cylinder::WetCylinder& cylinder, double f, const Eigen::Vector3d& V, const Eigen::Vector3d& W,
      const Eigen::Vector3d& axis, const EndWater& water, const DragModel& model = DragModel {})
{
   const auto parts = environment::wet_cylinder::GaussPointDragLoad(cylinder, model, f, {V(0), V(1), V(2)}, {W(0), W(1), W(2)}, {axis(0), axis(1), axis(2)}, water);
   return {Eigen::Vector3d(parts.force.data()), Eigen::Vector3d(parts.moment.data())};
}
} // namespace wet_cylinder_water