Cable.h

#pragma once
#include <Eigen/Eigen>
 
#include <fhsim/PrintDuringExec.h>
#include <fhsim/simobject/SimObject.h>
#include <fhsim_environment/EnvironmentProvider.h>
#ifdef FH_VISUALIZATION
#   include "sfh/ogre/C3DLine.h"
#endif
 
namespace RbCable
{
class CableRM : public SimObject
{
public:
   CableRM(const std::string& simObjectName, ISimObjectCreator* const creator); 
   ~CableRM();
   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);
 
   const double* forceA(const double T, const double* const X);
   const double* forceB(const double T, const double* const X);
   void calculations(const double T, const double* const X);
   void calculationsCommon(const double T, const double* const X) const;
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator);
   void RenderUpdate(const double T, const double* const X);
#endif
 
protected:
   typedef Eigen::Matrix<double, 3, 3> mat3;
   typedef Eigen::Matrix<double, 3, 1> vec3;
 
   void DistributeCatenary(Eigen::Matrix<double, 3, 1> P1, Eigen::Matrix<double, 3, 1> P2, double L, double* states, int i1, int i2, ISimObjectCreator* creator);
 
   PrintDuringExec*                  m_print;
   environment::EnvironmentProvider* m_environment;
 
   int    m_numElements;
   double m_totalLength;
   double m_radius;
   double m_weight;
   double m_alphaN;
   double m_betaN;
   double m_epsilonN;
   double m_alphaM;
   double m_betaM;
   double m_epsilonM;
   double m_alphaT;
   double m_betaT;
   double m_epsilonT;
 
   double m_bending_epsilon[3];
 
   // double  m_rho;
   // double  m_surfacePosZ;
   // double    m_rhoWater;
   // double  m_Ct; // tangential damping coefficient
   // double  m_Cn; // normal damping coefficient
 
   double m_length;
   double m_mass;
   double m_Ixy;
   double m_Iz;
   // bool TEMP_BOOL_VAR__ISMAJORTIMESTEP;
 
   // Sugar kelp rope application - start
   bool   m_kelp;  // 1 if used as a kelp rope, 0 otherwise
   double m_wKelp; // Biomass of kelp per meter rope/cable in Newtons
   // Sugar kelp rope application - end
 
   struct element
   {
      int    p;
      int    q;
      int    v;
      int    w;
      mat3   Mi;
      vec3   k;
      mat3   K;
      vec3   eDot;
      double nuDot;
   };
   element* m_el;
 
   ISignalPort* m_posA;
   ISignalPort* m_posB;
   ISignalPort* m_velA;
   ISignalPort* m_velB;
   ISignalPort* m_retractedLengthA;
   ISignalPort* m_retractedLengthB;
   ISignalPort* m_retractedSpeedA;
   ISignalPort* m_retractedSpeedB;
 
   int m_retractedNodesA;
   int m_retractedNodesB;
   int m_numFreeNodes;
 
   vec3 m_ka;
   vec3 m_kb;
 
   ICommonComputation*                      m_calcDynamics;
   Eigen::Matrix<double, Eigen::Dynamic, 1> m_lambda;
   Eigen::Matrix<double, Eigen::Dynamic, 1> m_F_MDotV; //< external force F, minus Coriolis term dM/dt*V
 
   double m_forceA[3];
   double m_forceB[3];
 
#ifdef FH_VISUALIZATION
   double            m_scale;
   Ogre::SceneNode** m_ManualObjectNodes;
#endif
};
} // namespace RbCable