HydroBody.h

#ifndef HYDRO_BODY_H
#define HYDRO_BODY_H
 
#include "BodyKinematics6Dof.h"
#include "RigidBodyTerms.h"
 
#include <fhsim/simobject/ISimObjectCreator.h>
#include <fhsim_environment/WakeSourceSet.h>
#include <string>
 
namespace environment::wake
{
class BodyFlowInteraction;
}
 
namespace hydro_body
{
struct FlowFrame
{
   const double* pos_n  = nullptr; 
   const double* quater = nullptr; 
   const double* vel_d  = nullptr; 
   const double* omega  = nullptr; 
 
   double R[3][3]       = {}; 
   double vel_n[3]      = {}; 
   double waterVel_n[3] = {}; 
   double waterVel_d[3] = {}; 
   double relVel_d[3]   = {}; 
};
 
struct Wrench
{
   double values[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
   void Add(const double wrench[6]);
 
   void AddForceAt(const FlowFrame& frame, const double pos_d[3], const double force_n[3]);
 
   void AddWeightAndBuoyancy(const FlowFrame& frame, double mass, const double cog_d[3], const double buoyancy_n[3]);
};
 
environment::wake::WakeCastState CastStateOf(const FlowFrame& frame);
} // namespace hydro_body
 
class HydroBody
{
   public:
   HydroBody(ISimObjectCreator* creator, const std::string& angularRateName, double convergenceTime, rigid_body::MunkMoment munk);
 
   void SetInertia(const double rigid[6], const double added[6]);
 
   const BodyKinematics6Dof& Kinematics() const { return m_kinematics; }
 
   const double* RigidInertia() const { return m_rigidInertia; }
 
   const double* AddedMass() const { return m_addedMass; }
 
   double InverseMass(int i) const { return m_massInverse[i][i]; }
 
   hydro_body::FlowFrame Frame(const double* X, const double waterVel_n[3]) const;
 
   hydro_body::FlowFrame Frame(const environment::wake::BodyFlowInteraction& flow, double t, const double* X) const;
 
   hydro_body::FlowFrame LocalFrame(const double* localX, const double waterVel_n[3]) const;
 
   hydro_body::FlowFrame LocalFrame(const environment::wake::BodyFlowInteraction& flow, double t, const double* localX) const;
 
   void Derivatives(const hydro_body::FlowFrame& frame, const hydro_body::Wrench& wrench, const double* X, double* XDot) const;
 
   void AddJacobian(const double* localX, const hydro_body::FlowFrame& frame, double dWrench[6][6], double* J, int nStates) const;
 
   private:
   hydro_body::FlowFrame FrameAtPos(const double* posState, const double waterVel_n[3]) const;
 
   BodyKinematics6Dof m_kinematics;
   rigid_body::MunkMoment m_munk;
   double m_rigidInertia[6]   = {};
   double m_addedMass[6]      = {};
   double m_massInverse[6][6] = {};
};
 
#endif