TrawlCable.h

#pragma once
 
#include "ITrawlCable.h"
// #include "ICollidableObject.h"
// #include "ConstraintCableInterface.h"
 
 
#ifdef FH_VISUALIZATION
#   include "sfh/ogre/C3DLine.h"
#endif
 
class TrawlCable : public SimObject, public ITrawlCable
{
public:
   TrawlCable(const std::string& simObjectName, ISimObjectCreator* const creator);
 
   ~TrawlCable();
 
   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
 
   void                                        Compute(const double T, const double* const X);
   void                                        AddDependency(ICommonComputation* dependecy);
   CoRiBoDynamics::ConstraintSolver*           GetConstraintSolver();
   CoRiBoDynamics::TrawlCableCollisionManager* GetCollisionManager();
   CoRiBoDynamics::TrawlCableJointConstraint*  GetJointConstraint();
   CoRiBoDynamics::EnvironmentForces*          GetEnvironmentForces();
   double                                      getRegulatorTimeConstant();
 
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;
 
   double               m_TrawlDoorMass;
   double               m_TrawlDoorArea;
   double               m_TrawlDoorLx;
   double               m_TrawlDoorLy;
   double               m_TrawlDoorLz;
   CoRiBoDynamics::vec3 m_TrawlDoorWarpConnection;
   CoRiBoDynamics::vec3 m_TrawlDoorTopBridleConnection;
   CoRiBoDynamics::vec3 m_TrawlDoorBottomBridleConnection;
 
   double m_ClumpWeightMass;
   double m_ClumpWeightLength;
   double m_ClumpWeightRadius;
 
   int    m_NumTopWarp;
   double m_TopWarpLength;
   double m_TopWarpElementLength;
 
   int m_NumWarp;         
   int m_NumTopBridle;    
   int m_NumBottomBridle; 
 
   double m_WarpDiameter;         
   double m_TopBridleDiameter;    
   double m_BottomBridleDiameter; 
 
   double m_WarpLength;         
   double m_TopBridleLength;    
   double m_BottomBridleLength; 
 
   double m_frequencyCoefficient;          
   double m_CollisionFrequencyCoefficient; 
 
   double m_WarpElementLength;         
   double m_TopBridleElementLength;    
   double m_BottomBridleElementLength; 
 
   double m_collisionDampingFactor; 
   // double m_collisionStiffness; /// Gamma value of contact constraint.
 
   CoRiBoDynamics::ConstraintSolver*           m_ConstraintSolver;  
   CoRiBoDynamics::TrawlCableCollisionManager* m_CollisionManager;  
   CoRiBoDynamics::TrawlCableJointConstraint*  m_JointConstraint;   
   CoRiBoDynamics::EnvironmentForces*          m_environmentForces; 
 
   std::set<ICommonComputation*> m_ExternalDependencies; 
 
   ICommonComputation* m_MainCalculations; 
   ITrawlCable*        m_ExtTrawlCable;
 
   std::vector<element> m_TopWarpElements;
   std::vector<element> m_WarpElements;
   std::vector<element> m_TopBridleElements;
   std::vector<element> m_BottomBridleElements;
   element              m_TrawlDoorElement;
   element              m_TrawlDoorWeightElement;
 
   element m_ClumpWeightElement;
 
   const double* winchForce(const double T, const double* const X);
   double        m_winchForce[3];
   int           m_WinchPointConnectionIndex;
 
   const double* TopBridleEndForce(const double T, const double* const X);
   double        m_TopBridleEndForce[3];
   int           m_TopBridleEndPointConnectionIndex;
 
   const double* BottomBridleEndForce(const double T, const double* const X);
   double        m_BottomBridleEndForce[3];
   int           m_BottomBridleEndPointConnectionIndex;
 
   const double* trawlDoorPosition(const double T, const double* const X);
 
 
   ICommonComputation* m_ContactForceCalculations;
   void                ContactForceCalculations(const double T, const double* const X);
   void                ContactForceCalculationsCommon(const double T, const double* const X) const;
   const double*       normalForceTrawlDoor(const double T, const double* const X);
   const double*       normalForceClumpWeight(const double T, const double* const X);
   const double*       normalForceTopBridle(const double T, const double* const X);
   const double*       normalForceBottomBridle(const double T, const double* const X);
   const double*       normalForceWarp(const double T, const double* const X);
   const double*       contactPositionTopBridle(const double T, const double* const X);
   const double*       contactPositionBottomBridle(const double T, const double* const X);
   const double*       contactPositionWarp(const double T, const double* const X);
   double              m_normalForceTrawlDoor[1];
   double              m_normalForceClumpWeight[1];
   double              m_normalForceTopBridle[3];
   double              m_normalForceBottomBridle[3];
   double              m_normalForceWarp[3];
 
   ISignalPort* m_WinchPosition;
   ISignalPort* m_WinchVelocity;
 
   ISignalPort* m_TopBridleEndPosition;
   ISignalPort* m_TopBridleEndVelocity;
 
   ISignalPort* m_BottomBridleEndPosition;
   ISignalPort* m_BottomBridleEndVelocity;
 
#ifdef FH_VISUALIZATION
   // Every pointer below is assigned in RenderInit only, which a headless run never calls.
   bool              m_HQ_Render;
   Ogre::SceneNode** m_TopWarpNodes      = nullptr;
   Ogre::SceneNode** m_WarpNodes         = nullptr;
   Ogre::SceneNode** m_TopBridleNodes    = nullptr;
   Ogre::SceneNode** m_BottomBridleNodes = nullptr;
   Ogre::SceneNode*  m_TrawlDoorNode     = nullptr;
   Ogre::SceneNode*  m_ClumpWeightNode   = nullptr;
   C3DLine*          m_TopWarpLine       = nullptr;
   C3DLine*          m_WarpLine          = nullptr;
   C3DLine*          m_TopBridleLine     = nullptr;
   C3DLine*          m_BottomBridleLine  = nullptr;
#endif
};