Auv.h

#pragma once
 
#include "AuvFoil.h"
#include "AuvHullLaw.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_base/body/HydroBody.h>
#include <fhsim_environment/BodyFlowInteraction.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <fhsim_environment/WakeSourceSet.h>
#include <string>
 
class PrintDuringExec;
 
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;
 
   void PreOdeFcn(double T, const double* X) override;
 
#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:
   hydro_body::Wrench SumLoads(double T, const double* X, const hydro_body::FlowFrame& frame, double& propulsionPower) const;
 
   void AddExternalForces(double T, const double* X, const hydro_body::FlowFrame& frame, hydro_body::Wrench& wrench) const;
 
   void AddInternalForces(double T, const hydro_body::FlowFrame& frame, hydro_body::Wrench& wrench) const;
 
   double AddThrustForces(double T, const double* X, const hydro_body::FlowFrame& frame, hydro_body::Wrench& wrench) const;
 
   struct RudderCommands
   {
      double dive = 0.0; 
      double yaw  = 0.0; 
   };
 
   RudderCommands ReadRudderCommands(double T, const double* X) const;
 
   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 CalcOutput(const double dT, const double* const adX);
 
   void CalcThrust(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;      
   auv_hull::Hull    m_hull;              
   double            m_cOG[3];            
   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];     
 
   // Cross-flow strip theory and the roll damping it cannot supply, with the drag coefficients.
   auv_hull::Sections     m_sections;     
   auv_hull::Coefficients m_coefficients; 
 
   // The fins. One set of properties serves both planes; they differ only in which
   // transverse component of the inflow they see.
   auv_hull::Rudder m_rudder; 
 
   class AuvCaster final : public environment::wake::PropelledObject
   {
   public:
      explicit AuvCaster(const Auv& auv)
          : m_auv(auv)
      {
      }
 
      environment::wake::WakeCastState Reference(double T, const double* X) const override;
      void CurrentLoad(double T, const double* X, const double freeStream[3], const double flowDir[3], double force[3], double& frontalArea,
            double rearCentre[3]) const override;
      environment::wake::PropellerJetInput PropellerJet(double T, const double* X) const override;
 
   private:
      const Auv& m_auv;
   };
 
   environment::EnvironmentProvider*      m_environment = nullptr;       
   AuvCaster                              m_caster;                      
   environment::wake::BodyFlowInteraction m_flow;                        
   HydroBody                              m_body;                        
   double                                 m_propellerDiameter = 0.0;     
   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);
   }
 
   struct PortCommands
   {
      double power      = 0.0; 
      double rudderDive = 0.0; 
      double rudderYaw  = 0.0; 
   };
   mutable PortCommands m_lastCommands; 
 
   // Output port storage, written by CalcOutput (and m_outThrust by CalcThrust).
   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, auv_hull::RudderPlane 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
};