Auv.h

#pragma once
 
#include "AuvFoil.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <string>
 
class PrintDuringExec;
 
struct AuvHullProperties
{
   double radius      = 0.0; 
   double volume      = 0.0; 
   double frontalArea = 0.0; 
   double mass        = 0.0; 
   double ixx         = 0.0; 
   double iyy         = 0.0; 
   double izz         = 0.0; 
};
 
enum class AuvRudderPlane
{
   Dive, 
   Yaw   
};
 
struct AuvRudderProperties
{
   AuvFoilProperties foil;                              
   double            rho           = 1025.0;            
   double            area          = 0.016;             
   double            pos[3]        = {-0.85, 0.0, 0.0}; 
   double            maxDeflection = 0.35;              
};
 
class Auv : public SimObject
{
public:
   Auv(std::string simObjectName, ISimObjectCreator* creator);
 
   ~Auv() override;
 
   void OdeFcn(const double dT, const double* const adX, double* const adXDot) const override;
 
 
   bool HasJacobians() const override;
   void OdeJacobian(double T, const double* X, double* J, int nStates) override;
   int  GetJacobianSparsity(int nStates, int* rowPtr, int* colIdx) override;
 
   bool HasPortJacobians() const override;
   void InputPortJacobian(const std::string& portName, double T, const double* X,
         double* dF_dInput, int portSize, int nStates) override;
   void OutputPortJacobian(const std::string& portName, double T, const double* X,
         double* dPort_dX, int portSize, int nStates, int index) override;
 
   void FinalSetup(const double dT, const double* const adX, ISimObjectCreator* const creator) override;
 
   static AuvHullProperties HullPropertiesOf(double length, double diameter, double averageDensity);
 
   static double SubmergedFraction(double axisDepth, double radius);
 
   static void AxialDragWrench(const double relVel_d[3], double rho, double frontalArea,
         double axialDragCoeff, double wrench_d[6]);
 
   static const int kSectionCount = 3;
 
   static const double kCrossFlowSpeedEpsilon;
 
   static void CrossFlowStripWrench(const double relVel_d[3], const double omega_d[3],
         double rho, double crossFlowDragCoeff, int sectionCount, const double* sectionX,
         const double* sectionDiameter, const double* sectionLength, double wrench_d[6]);
 
   static void SlenderBodyLiftWrench(const double relVel_d[3], const double omega_d[3], double rho,
         int sectionCount, const double* sectionX, const double* sectionDiameter,
         const double* sectionLength, double wrench_d[6], double (*dWrench_dVelocity)[6] = nullptr);
 
   static double RollDampingMoment(double rollRate, double linear, double quadratic);
 
   static const double kInflowSpeedEpsilon;
 
   static void RudderWrench(AuvRudderPlane plane, const double relVel_d[3], const double omega_d[3],
         double deflection, const AuvRudderProperties& rudder, double wrench_d[6]);
 
   static void RudderWrenchJacobian(AuvRudderPlane plane, const double relVel_d[3],
         const double omega_d[3], double deflection, const AuvRudderProperties& rudder,
         double dWrench_dVelocity[6][6], double dWrench_dDeflection[6]);
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator) override;
 
   void RenderUpdate(const double dT, const double* const adX) override;
#endif
 
protected:
   virtual void CalcFoundation(const double dT, const double* const adX);
 
   virtual void AddExternalForces(const double dT, const double* const adX);
 
   virtual void AddInternalForces(const double dT);
 
   virtual void AddThrustForces(const double dT, const double* const adX);
 
   virtual void AddHullHydroForces();
 
   virtual void AddRudderForces(const double dT, const double* const adX);
 
   void WarnIfTheFinsCannotDive(const std::string& simObjectName, double diveAngle) const;
 
   double AuthorityCheckCruiseSpeed() const;
 
   virtual double ComputeThrust(double propulsionPower, double surgeSpeed) const;
 
   double ClampedPropulsionPower(double commandedPower) const;
 
   virtual void CalcDerivatives(double* const adXDot);
 
   virtual void CalcOutput(const double dT, const double* const adX);
 
   void CalcOutputCommon(const double dT, const double* const adX) const;
 
   const double* OutPos(const double dT, const double* const adX);
   const double* OutVel(const double dT, const double* const adX);
   const double* OutQuater(const double dT, const double* const adX);
   const double* OutEuler(const double dT, const double* const adX);
   const double* OutHeading(const double dT, const double* const adX);
   const double* OutPitch(const double dT, const double* const adX);
   const double* OutDepth(const double dT, const double* const adX);
   const double* OutSpeed(const double dT, const double* const adX);
   const double* OutAltitude(const double dT, const double* const adX);
   const double* OutSeaDepth(const double dT, const double* const adX);
   const double* OutBattery(const double dT, const double* const adX);
   const double* OutBatteryFraction(const double dT, const double* const adX);
   const double* OutThrust(const double dT, const double* const adX);
 
   // Geometry and mass, all derived from Length, Diameter and AverageDensity.
   double            m_length = 0.0;      
   AuvHullProperties m_hull;              
   double            m_cOG[3];            
   double            m_massInverse[6][6]; 
   double            m_rigidInertia[6];   
   double            m_addedMass[6];      
   double            m_rhoWater;          
   double            m_initialBattery;    
 
   // Propulsion, drag and the hotel load.
   double m_thrustPerPower;     
   double m_maxPropulsionPower; 
   double m_hotelPower;         
   double m_thrusterPos[3];     
   double m_axialDragCoeff;     
 
   // Cross-flow strip theory and the roll damping it cannot supply.
   double m_crossFlowDragCoeff;             
   double m_sectionX[kSectionCount];        
   double m_sectionDiameter[kSectionCount]; 
   double m_sectionLength[kSectionCount];   
   double m_rollDampingLinear;              
   double m_rollDampingQuadratic;           
 
   // The fins. One set of properties serves both planes; they differ only in which
   // transverse component of the inflow they see.
   AuvRudderProperties m_rudder; 
 
   environment::EnvironmentProvider* m_environment = nullptr; 
   PrintDuringExec*                  m_print       = nullptr; 
 
   // Input ports
   ISignalPort* m_inThrustPower   = nullptr; 
   ISignalPort* m_inRudderYaw     = nullptr; 
   ISignalPort* m_inRudderDive    = nullptr; 
   ISignalPort* m_inExternalForce = nullptr; 
 
   ICommonComputation* m_commonCalc = nullptr; 
 
   unsigned long m_IStatePos      = 0; 
   unsigned long m_IStateQuater   = 0; 
   unsigned long m_IStateLocalVel = 0; 
   unsigned long m_IStateOmega    = 0; 
   unsigned long m_IStateBattery  = 0; 
   unsigned long m_IStateBase     = 0; 
 
   int LocalIndex(unsigned long globalStateIndex) const
   {
      return static_cast<int>(globalStateIndex - m_IStateBase);
   }
 
   // Working set of the derivative evaluation, in the style of TrawlDoorBase.
   double        ma_adSumForces[6]   = {0, 0, 0, 0, 0, 0};                
   double        ma_mdR[3][3]        = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 
   double        ma_adXVel_n[3]      = {0, 0, 0};                         
   double        ma_adWaterVel_d[3]  = {0, 0, 0};                         
   const double* ma_adXPos_n         = nullptr;                           
   const double* ma_adXQuater        = nullptr;                           
   const double* ma_adXVel_d         = nullptr;                           
   const double* ma_adXOmega         = nullptr;                           
   const double* ma_adXBattery       = nullptr;                           
   double        ma_adRelVel_d[3]    = {0, 0, 0};                         
   double        ma_dPropulsionPower = 0.0;                               
   double        ma_dCommandedPower  = 0.0;                               
   double        ma_dRudderDive      = 0.0;                               
   double        ma_dRudderYaw       = 0.0;                               
 
   // Output port storage, all written by CalcOutput.
   double m_outPos[3]          = {0, 0, 0};    
   double m_outVel[3]          = {0, 0, 0};    
   double m_outQuater[4]       = {1, 0, 0, 0}; 
   double m_outEuler[3]        = {0, 0, 0};    
   double m_outDepth           = 0.0;          
   double m_outSpeed           = 0.0;          
   double m_outAltitude        = 0.0;          
   double m_outSeaDepth        = 0.0;          
   double m_outBatteryFraction = 0.0;          
   double m_outThrust          = 0.0;          
 
#ifdef FH_VISUALIZATION
   double HullRadiusAt(double bodyX) const;
 
   void BuildHullSurface(Ogre::ManualObject* const hull) const;
 
   void BuildFinPlane(Ogre::ManualObject* const fins, AuvRudderPlane plane) const;
 
   Ogre::SceneManager* m_sceneMgr      = nullptr; 
   Ogre::SceneNode*    m_renderNode    = nullptr; 
   Ogre::SceneNode*    m_diveFinNode   = nullptr; 
   Ogre::SceneNode*    m_yawFinNode    = nullptr; 
   Ogre::ManualObject* m_hullObject    = nullptr; 
   Ogre::ManualObject* m_diveFinObject = nullptr; 
   Ogre::ManualObject* m_yawFinObject  = nullptr; 
#endif
};