CablePendulum.h

#pragma once
 
#include "sfh/constants.h"
#include "sfh/math.h"
#include "sfh/text.h"
#include "sfh/util.h"
 
#include <Eigen/Dense>
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <memory>
class CablePendulum : public SimObject
{
   public:
   CablePendulum(const std::string& simObjectName, ISimObjectCreator* const creator);
   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 SetOutputPortValues(const double T, const double* const X) const;
   const double* MidpointPosition(const double T, const double* const X);
   const double* BallPosition(const double T, const double* const X);
   const double* BallVelocity(const double T, const double* const X);
 
   public:
   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;
 
   protected:
   void segmentJacobian(const double* pk, const double* pl,
       const double* vk, const double* vl,
       double dF_dpk[9], double dF_dpl[9],
       double dF_dvk[9], double dF_dvl[9]) const;
 
   void addDragJacobian(const double T, const double* pk, const double* pl, const double* vk,
       double dragFactor, double dF_dpk[9], double dF_dpl[9], double dF_dvk[9]) const;
   Eigen::Vector3d Drag(int k, int l, const double T, const double* const X) const;
   Eigen::Vector3d Spring(int k, int l, const double T, const double* const X) const;
   Eigen::Vector3d Damp(int k, int l, const double T, const double* const X) const;
   const double* SuspensionForce(const double T, const double* const X);
   const double* CableOnBallForce(const double T, const double* const X);
 
 
   ICommonComputation* m_SetOutputPortValues;
   ISignalPort* m_SuspensionPos;
   ISignalPort* m_SuspensionVel;
 
   environment::EnvironmentProvider* Environment;
   std::vector<int> m_PositionIndex, m_VelocityIndex;
   int m_BallPositionIndex, m_BallVelocityIndex;
   int m_stateBase = 0; 
 
   int LocalIndex(int globalStateIndex) const { return globalStateIndex - m_stateBase; }
   int m_NumPoints;
   mutable double m_MidpointPosition[3];
   mutable double m_BallPosition[3];
   mutable double m_BallVelocity[3];
   mutable double m_SuspensionForce[3];
   mutable double m_CableOnBallForce[3];
 
   double m_BallMass, m_BallRadius, m_BallDragCoef, m_RhoWater, m_BallRho;
   double m_ElementMass, m_ElementLength0;
   double m_SpringCoef, m_DamperCoef, m_GravYancy;
   double m_CrossDragCoef, m_AlongDragCoef;
   double m_Mass, m_Length, m_Diameter, m_RhoCable, m_Modulus;
 
#ifdef FH_VISUALIZATION
   Ogre::ManualObject* m_Line;
   Ogre::SceneNode* m_RenderNode;
   std::string m_MaterialName;
   std::string m_MeshName;
   double m_Scale;
#endif
};