RundDorg.h

#pragma once
// #include "ITrawlCable.h"
// #include "ICollidableObject.h"
// #include "ConstraintCableInterface.h"
#include "TrawlCableConstraintSets.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_coribo/eigen_matrix_defs.h>
 
#ifdef FH_VISUALIZATION
#   include "sfh/ogre/C3DLine.h"
#endif
 
class RundDorg : public SimObject //, public ITrawlCable
{
public:
   RundDorg(const std::string& simObjectName, ISimObjectCreator* const creator);
 
   ~RundDorg();
 
   void OdeFcn(const double T, const double* const X, double* const XDot) const;
 
   void InitialConditionSetup(const double T, const double* const currentIC, double* const updatedIC, ISimObjectCreator* const creator);
 
   void FinalSetup(const double T, const double* const X, ISimObjectCreator* const creator);
 
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator);
   void RenderUpdate(const double T, const double* const X);
#endif
 
protected:
   void MainCalculations(const double T, const double* const X);
   void MainCalculationsCommon(const double T, const double* const X) const;
 
   struct element
   {
      int p;
      int q;
      int v;
      int w;
      int n;
   };
   void DistributeCatenary(CoRiBoDynamics::vec3 P1, CoRiBoDynamics::vec3 P2, std::vector<element> elements, double L, double* states, int i1, int i2, ISimObjectCreator* creator);
 
   environment::EnvironmentProvider* m_environment;
 
   enum Chirality
   {
      PORT,
      STARBOARD
   };
   Chirality m_Chirality;
 
   enum WeightType
   {
      FOIL,
      CLUMP
   };
   WeightType m_WeightType;
 
   double               m_FoilMass;
   double               m_FoilArea;
   double               m_FoilLx;
   double               m_FoilLy;
   double               m_FoilLz;
   CoRiBoDynamics::vec3 m_WeightMainConnection;
   CoRiBoDynamics::vec3 m_WeightFishLine1Connection;
   CoRiBoDynamics::vec3 m_WeightFishLine2Connection;
 
   double m_ClumpWeightMass;
   double m_ClumpWeightLength;
   double m_ClumpWeightRadius;
 
   int m_NumMain;  
   int m_NumLine1; 
   int m_NumLine2; 
 
   double m_MainDiameter;  
   double m_Line1Diameter; 
   double m_Line2Diameter; 
 
   double m_MainLength;  
   double m_Line1Length; 
   double m_Line2Length; 
 
   double m_frequencyCoefficient; 
 
   double m_MainElementLength;  
   double m_Line1ElementLength; 
   double m_Line2ElementLength; 
 
   CoRiBoDynamics::ConstraintSolver*          m_ConstraintSolver;  
   CoRiBoDynamics::TrawlCableJointConstraint* m_JointConstraint;   
   CoRiBoDynamics::EnvironmentForces*         m_environmentForces; 
 
   ICommonComputation* m_MainCalculations; 
 
   std::vector<element> m_MainElements;
   std::vector<element> m_Line1Elements;
   std::vector<element> m_Line2Elements;
   element              m_FoilElement;
   element              m_FoilWeightElement;
   element              m_ClumpWeightElement;
 
   const double* ExternalJointForce(const double T, const double* const X, int connectionIndex, double* force);
 
   const double* MainForce(const double T, const double* const X);
   const double* MainForceVector(const double T, const double* const X);
   double        m_MainForce[3];
   double        m_MainForceMagnitude;
   int           m_MainForceConnectionIndex;
 
   const double* Line1Force(const double T, const double* const X);
   const double* Line1ForceVector(const double T, const double* const X);
   double        m_Line1Force[3];
   double        m_Line1ForceMagnitude;
   int           m_Line1ConnectionIndex;
 
   const double* Line2Force(const double T, const double* const X);
   const double* Line2ForceVector(const double T, const double* const X);
   double        m_Line2Force[3];
   double        m_Line2ForceMagnitude;
   int           m_Line2ConnectionIndex;
 
   const double* WeightPosition(const double T, const double* const X);
 
   /*const double*  WeightMainForce( const double T, const double* const X );
   double m_WeightMainForce[3];
   const double*  WeightLine1Force( const double T, const double* const X );
   double m_WeightLine1Force[3];
   const double*  WeightLine2Force( const double T, const double* const X );
   double m_WeightLine2Force[3];*/
 
   ISignalPort* MainPosition;
   ISignalPort* m_MainVelocity;
 
   ISignalPort* m_Line1Position;
   ISignalPort* m_Line1Velocity;
 
   ISignalPort* m_Line2Position;
   ISignalPort* m_Line2Velocity;
 
#ifdef FH_VISUALIZATION
   // Every pointer below is assigned in RenderInit only, which a headless run never calls.
   bool              m_HQ_Render;
   Ogre::SceneNode** m_MainNodes  = nullptr;
   Ogre::SceneNode** m_Line1Nodes = nullptr;
   Ogre::SceneNode** m_Line2Nodes = nullptr;
   Ogre::SceneNode*  m_WeightNode = nullptr;
   C3DLine*          m_MainLine   = nullptr;
   C3DLine*          m_Line1Line  = nullptr;
   C3DLine*          m_Line2Line  = nullptr;
#endif
};