NetStructureWithConstraints.h

#pragma once
 
#include <limits>
#include "CommonTypes.h"
 
// #define NetStructureWithConstraints_DO_PROFILE
 
// Includes
#include "NetCableElementSpec.h"
#include "NetCableElement.h"
#include "NetCore.h"
#include "NetElement3N.h"
#include "NetElementDef.h"
#include "NetNodeStates.h"
#include "NetStructureSpec.h"
 
#include "sfh/timers/ProfilerWallclock.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_coribo/JointConstraint.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <memory>
#include <string>
#include <vector>
 
 
#ifdef FH_VISUALIZATION
#   include "sfh/ogre/C3DLine.h"
#   include "sfh/ogre/NMeshGenerators.h"
#endif
 
// Class definition
class NetStructureWithConstraints : public SimObject, private NetNodeStates
{
public:
   NetStructureWithConstraints(std::string simObjectName, ISimObjectCreator* creator);
   virtual ~NetStructureWithConstraints();
   virtual void        OdeFcn(const double dT, const double* const adX, double* const adXDot) const;
   virtual void        InitialConditionSetup(const double dT, const double* const adCurrentIC, double* const adUpdatedIC, ISimObjectCreator* const creator);
   virtual void        FinalSetup(const double dT, const double* const adX, ISimObjectCreator* const creator);
   void                PreOdeFcn(double T, const double* X) override;
 
#ifdef FH_VISUALIZATION
   virtual void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator);
   virtual void RenderUpdate(const double dT, const double* const adX);
 
   void     CreateCapsule(Ogre::ManualObject* capsule_manual);
   C3DLine* myline;
#endif
   void                        SetAddedDragPerMPS(double AddedDragPerMPS);
protected:
 
   NetNodeLayout Layout() const override;
   void AddExternalForces(double T, const double* X, double* nodeForce) const override;
   void TakeNodeForces(double T, const double* X, const double* nodeForce, double* XDot) const override;
   virtual void   AddInputPorts(ISimObjectCreator* creator);
   virtual void   CreateDataStructures();
   VSPairStrInt   CreateExternalNodeMap(std::string tag, int* piUsed, const NetStructureSpec& NetSpec, ISimObjectCreator* creator);
   void           ValidateConstraintInput(ISimObjectCreator* creator) const;
 
 
   // ISignalPort **m_inExternalForces;
   /*ISignalPort **m_InConstraintsPos;                                                                  // NEW: added to test new approach where pos and vel for selected nodes are set by external objects
   ISignalPort **m_InConstraintsVel;                                                                    // NEW: added to test new approach where pos and vel for selected nodes are set by external objects*/
   ISignalPort** m_InTopConstraintsPos = nullptr;
   ISignalPort** m_InTopConstraintsVel = nullptr;
   int*          m_iStatePos;
   int*          m_iStateVel;
 
 
   int m_numNodesInputForce;
   int m_numNodesOutputPosAndVel;
   int m_numNodesCalc;
 
   int (*m_hasPosAndVelStates)[2] = nullptr; 
 
   int m_NumNodesInputPosAndVel; // NEW: added to test new approach where pos and vel for selected nodes are set by external objects
   int m_NumNodesOutputForce;    // NEW: added to test new approach where pos and vel for selected nodes are set by external objects
 
   VSPairStrInt m_VsNodesInputPosAndVel; // NEW: added to test new approach where pos and vel for selected nodes are set by external objects
   VSPairStrInt m_VsNodesOutputForce;    // NEW: added to test new approach where pos and vel for selected nodes are set by external objects
   // VSPairStrInt          m_VsNodesTopConstraintsInputPos;
 
 
   std::unique_ptr<NetCore> m_core;
 
 
 
   VSPairStrInt m_VsNodesOutputPosAndVel;
   VSPairStrInt m_VsNodesInputForce;
   bool         m_useTagForExternalNodes;
 
 
   bool m_CageWakeEffectsActive;
 
   // CONSTRAINTS (NB LOTS OF HARD CODING HERE NOW)
   std::unique_ptr<CoRiBoDynamics::ConstraintSolver> m_constraintSolver;
   struct states
   {
      int p;
      int q;
      int w;
      int v;
   };
   std::vector<states>              m_stateVector;
 
   const double* NodePositionState(const double* X, int node) const { return X + (NodeHasOwnStates(node) ? m_iStatePos[node] : DrivingElement(node).p); }
   const double* NodeVelocityState(const double* X, int node) const { return X + (NodeHasOwnStates(node) ? m_iStateVel[node] : DrivingElement(node).v); }
   bool NodeHasOwnStates(int node) const { return m_hasPosAndVelStates[node][1] == 1; }
   const states& DrivingElement(int node) const { return m_stateVector[m_hasPosAndVelStates[node][0]]; }
 
   CoRiBoDynamics::JointConstraint* m_jointConstr; 
   // Coordinates for attachment of the constraints chains
   mutable double m_topPosition1[3];
   mutable double m_topVelocity1[3];
   double         m_topOrientation1[4];
   double         m_topangularVel1[3];
   mutable double m_topPosition2[3];
   mutable double m_topVelocity2[3];
   double         m_topOrientation2[4];
 
   double m_topangularVel2[3];
 
   double m_YoungsModulus;
   double m_solverFrequencyCoefficient; 
   int    m_numConstraintsElements;
   double m_depthOfConstraintsCable;
#ifdef FH_VISUALIZATION
   Ogre::SceneNode** m_renderNodes; 
                                    // Ogre::SceneNode*     m_renderNode2;  ///< The node to which the rendered entity is attached.
#endif
 
#ifdef NetStructureWithConstraints_DO_PROFILE
   sfh::timers::ProfilerWallclock m_Profiler;
#endif
};