FishCage.h

#pragma once
 
#include "eigen_matrix_defs.h"
#include "sfh/constants.h"
#include "SimObject.h"
#include <memory>
#include "Eigen/StdVector"
#include "CEnvironment.h"
#include <CPrintDuringExec.h>
 
#ifdef FH_VISUALIZATION
#include "sfh/ogre/C3DLine.h"
#endif
 
namespace Fish{
 
class FishCage {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   ~FishCage();
 
    void Init(ISimObjectCreator* creator);
    void OdeFcn(double T, const double* X, double* XDot);
    void PreOdeFcn(const double T, const double* X, IStateUpdater* updater);
 
    double Radius(){return m_cage_radius;}
    double Depth(){return m_Depth;}
 
    void SetEnvironment(CEnvironment* environment);
 
    struct CageEdge{
        vec3 P; 
        vec3 N; 
        double depth; 
    };
 
    CageEdge GetClosestEdge(const vec3& P);
 
    struct NetPanel{
        vec3 P;                 // Net panel position
        vec3 N;                 // Net panel normal vector. Points "out" of cage
        double Distance;        // Distance to the obstacle
 
        vec3 G;
        vec3 VN;
        vec3 VV;
    };
 
    struct ix_pair { uint16_t i;    uint16_t j; ix_pair(uint16_t a, uint16_t b) { i = a; j = b; } };
 
    NetPanel GetClosestNetPanel(const vec3& P, double& velR, const vec3& V);
 
    //============================================================ Herman_dev
    struct Triangle {
        vec3 vertex1;   // First vertex (N,E,D) of triangle
        vec3 vertex2;   // 2nd --''--
        vec3 vertex3;   // 3rd --''--
    };
    bool GetRayIntersection(const vec3& rayOrigin, const vec3& rayDirection, double& outDistance);
    bool RayIntersectsTriangle(const Triangle* inTriangle, const vec3& rayOrigin, const vec3& rayVector, double& outDistance);
    //============================================================ Herman_dev
 
    double              m_dragForceNet[3];      // Total hydrodynamic drag force on the net
    double              m_dragForceSkirt[3];    // Total hydrodynamic drag force on the skirt
    double              m_dragForceDepth[3];    // Total hydrodynamic drag force on the net above a given depth
    int                 m_DepthI;
 
    bool                   m_ExtTopConn;        // Boolean, true if external connection is considered
    double*             m_TopOutForce;      // Top connection force on the net
    int                 m_TopConnectNum;    // Top connection
    int                 m_BotConnectNum;    // Bottom connection
    double              m_RingOutPos[3];    // Ring out position
    double              m_BotOutPos[3];     // Bottom out position
    double              m_NetOutPos[3];     // Net out position (single)
    double              m_BotOutVel[3];     // Bottom out velocity
 
    int                 m_NumNetOut;        // Number of net out position
    double*             m_MatNetOut;        // Net out position (multiple)
    
    int                 m_ExtTopConnNum;        // External top connection number   
    bool                m_ExtTopBody;           // External top body
    vec3                m_RelTopPos;            // Top position relative to body
    double              m_ExtTopBodyForce[6];   // External top body force
 
    bool                   m_PortPosToForce;        // Port to input position and output force
   int               m_PortPosNum;        // Port number
    int*                    m_PortPosIndex;     // Port index
    double              m_PortConLength;        // Port connection length
    double*             m_PortOutForce;         // Port out force
 
    bool                   m_PortForceToPos;        // Port to input force and output position
   int               m_PortForceNum;      // Port number
    int*                    m_PortForceIndex;       // Port index
    double*             m_PortOutPos;           // Port out position
    double*             m_PortOutVel;           // Port out velocity
 
    double              m_SkirtPos[3];          // Position of the sea-lice skirt
 
    bool                m_CalVolume;            // Port to output cage volume
    double              m_Volume[3];            // Cage volume
    double              m_CageTopCentre[3];     // Cage top centre
 
    double              m_FlowReduction;        // Flow reduction factor
 
    double              m_SkirtCD[2];           // Skirt drag coefficient, [0]: drag coefficient; [1]: flow reduction factor
    double              m_InputCD[3];           // Input drag coefficient, [0]: drag coefficient; [1]: flow reduction factor (global); [2]: flow reduction factor
 
    bool                m_PortCD;               // Port to output drag coefficient
    double*             m_PortOutCD;            // Port out drag coefficient
    int                 m_PortCDIndex[2];       // Port index
    double              m_PortCDDepth;          // Port depth
    double*             m_PortNodeCD;           // Node drag coefficient
 
    bool                m_RigidCage;
    
    bool                m_InBottomForce;
    ISignalPort*        m_AddBottomForce;
 
    bool                m_InRingForce;
    ISignalPort*        m_AddRingForce;
 
    int                 m_InCdNum;
    double*             m_InCd;
    int*                m_InCdIndex;
    ISignalPort**       m_AddCd;
 
    bool                m_ExtBotRing;
    ISignalPort**       m_InRingPos;
    ISignalPort**       m_InRingVel;
    double*             m_OutRingForce;
 
    bool                m_ExtChain;
    ISignalPort**       m_InChainPos;
    ISignalPort**       m_InChainVel;
    double*             m_OutChainForce;
    std::vector<double> m_ChainPos;
    vec3                m_ChainCen;
    double              m_ChainLen;
    double              m_ChainLim;
    int                 m_ExtChainNum;
   int               m_ExtChainDepIdx;
    int                 m_RingStatePos;
    int                 m_RingStateVel;
    double              m_RingStateM;
    double              m_RingStateA;
    double              m_RingStateD;
 
    bool                m_BotRingInState;
    ISignalPort*        m_BotRingInPos;
    ISignalPort*        m_BotRingInVel;
 
    // Net track ===============================
    bool                m_NetTrack;             // Boolean, true if net panels are tracked
    double              m_NetTrackDT;
    int                 m_NetTrackNum;
    double              m_NetTrackDis;
    ISignalPort*        m_NetTrackPos;
    int                 m_NetPanelNum;
    int                 m_NetPanelNumH;
    int                 m_NetPanelNumV;
    int*                m_NetPanelMat;
    double*             m_NetPanelArea;
    double              m_NetPanelAreaTrack;
    double              m_NetPanelOutDT;
    int                 m_NetPanelOutNum;
    std::string         m_NetPanelOutFile;
 
    double              m_CageNodeOutDT;
    int                 m_CageNodeOutNum;
    std::string         m_CageNodeOutFile;
    double              m_CageNodeOutTSta;
    double              m_CageNodeOutTStp;
 
    double              m_NetMinDis;
    double              m_NetTrackAng;
 
    bool                m_NetIdentify;
    ISignalPort*        m_NetIdentifyNum;
 
    bool                m_NetTrackHeadingToPosition;
    ISignalPort*        m_NetTrackHeading;  
 
    int                 m_NetTrack_States;
    double              m_NetTrackPosOut[12];
    double              m_NetTrackPosIn[6];
    double              m_NetTrackMaxDiff;
    double              m_NetTrackCenDiff[4];
    double              m_NetTrackInitialTime[3];
 
    double              m_CageHeading;
    bool                m_Quat;
    Quat                m_CageQL;
    mat3                m_CageRL;
 
    int NodeNumber() {return m_num_nodes;}
    vec3    NodePosition(int index) {return m_P[index];};
    // Net track ===============================
 
   //============================================================================ PC_dev start
   bool        m_isSmallTank;    // Set to true if a small tank is simulated
   bool        m_useDummyTankVel;// Set to true if a dummy tank velocity should be used.
   double      m_URadialMax;     // Max value for radially linearly varying water velocity in tank.
   double      m_UzConst;        // Constant vertical water velocity in tank
   void        GetDummyTankVel(const double* pos, double* outVal);// Function that returns dummy tank velocity
   //void        UsingDummyTankVel(bool outVal); // Function that returns true if dummy tank velocity is used
   //============================================================================ PC_dev end
 
#ifdef FH_VISUALIZATION
    virtual void RenderInit(Ogre::Root *const ogreRoot, ISimObjectCreator *const creator);
    virtual void RenderUpdate(const double T, const double *const X);
    C3DLine* m_net_line;
    C3DLine* m_top_line;
    C3DLine* m_ring_line;
    //C3DLine* m_node_normal_line;
    C3DLine* m_node_unit_hydro_force;
    Ogre::SceneNode* m_ring_node;
    std::vector<ix_pair> m_net_pair;
    C3DLine* m_top_line_ext;
 
    C3DLine* m_chain_line;
 
    bool m_net_Render;
    C3DLine* m_net_triangle;
 
    bool m_skirt_Render;
    C3DLine* m_skirt_triangle;
    int m_skirt_DI;
    int m_skirt_TN;
 
    // Net track ===============================
    C3DLine*    m_track_triangle;
    double      m_track_color[4];
    // Net track ===============================
#endif
 
 
private:
    void distance_constraint(vec3& PA, vec3& PB, vec3& VA, vec3& VB, vec3& NA, vec3& NB, vec3& F, mat3& M, double L, double L_dot, double igamma);
    void distance_constraint_t(vec3& PA, vec3& PB, vec3& VA, vec3& VB, vec3& NA, vec3& NB, vec3& F, mat3& M, double L, double L_dot, double igamma);
    void ring_constraint(vec3& P, vec3 V, double angle, vec3& F, vec3& T, mat3& NN, mat3& NK, mat3& KK, double igamma, double ST, const double* SX, int ic);
   void ring_constraint_ext(vec3& P, vec3 V, double angle, vec3& F, vec3& T, mat3& NN, mat3& NK, mat3& KK, double igamma, double ST, const double* SX, int ic);
   void chain_constraint(vec3& P, vec3 V, double angle, vec3& F, vec3& T, mat3& NN, mat3& NK, mat3& KK, double igamma, double ST, const double* SX, int ic, vec3& FR, mat3& NNR, int dIdx, mat3& NNC, vec3& FC);
 
    uint16_t ConvertToZindex(const vec3& P);
    vec3 ConvertFromZindex(uint16_t Z_index);
 
    std::vector<int> m_EdgeLookupArray;
 
    Eigen::Matrix<double,15,1> m_net_bottom_function_approximation_coefficients;
    
    double m_cage_radius;
    int m_num_nodes_circumference;
    int m_num_nodes_vertical;
    int m_num_nodes;
 
    double m_net_diameter;  // Net twine diameter
    double m_bar_length;
    double m_net_solidity;
    double m_ring_connection_bar_length;
    double m_net_size;
 
    double m_node_mass;
    double m_node_added_mass;
    double m_node_projected_area;
    double m_node_submerged_weight;
 
    vec3 TopRingConnectionPoint(int ix, double T, const double* X);
 
    ISignalPort* m_WinchLength;
    ISignalPort* m_WinchSpeed;  
 
    std::vector<mat3,Eigen::aligned_allocator<mat3>> m_connection_matrices;
    std::vector<mat3,Eigen::aligned_allocator<mat3>> m_node_matrices;
    std::vector<mat36,Eigen::aligned_allocator<mat36>> m_ring_matrices;
    
    std::vector<ix_pair> m_connections; // structural connections   
    
    std::vector<vec3,Eigen::aligned_allocator<vec3>> m_N; // node normal vector
    std::vector<vec3,Eigen::aligned_allocator<vec3>> m_P; // node position
    std::vector<vec3,Eigen::aligned_allocator<vec3>> m_V; // node velocity
 
    std::vector<vec3, Eigen::aligned_allocator<vec3>> m_unitHydF; // node unit hydrodynamic force
 
   // ============================================================================ herman_dev
    double m_delta_t;
   //bool m_tau_given;
   // ============================================================================ herman_dev
    double m_tau;
    double m_gamma;
    SparseMat m_system_matrix;
    Eigen::SimplicialLDLT<SparseMat,Eigen::Upper> m_solver;
 
    int m_position_states;
    int m_velocity_states;
 
    CEnvironment* m_environment;
 
    struct BottomRing {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        int     states_ix;
        double  major_radius;
        double  minor_radius;
        double  mass;
 
        double tau;
        double delta_t;
 
        void Init();
 
        void UpdateStates(const double* X);
        void HydroDynamics(vec6& Forces, mat6& Inertia, double T, const double* X);
        void Kinematics(const vec6& Accelerations, double* XDot);
 
        double buoyancy;
        vec3 translational_inertia;
        vec3 angular_inertia;
 
        vec3 P;
        Quat Q;
        vec3 V;
        vec3 W;
 
        CEnvironment* m_environment;
 
        void UpdateStatesInput(const double* X, const double* InPos, const double* InVel);
        void KinematicsInput(const vec6& Accelerations, double* XDot);
    } m_ring;   
 
    int                                                         m_Triangles;        // Number of triangular elements
    std::vector<int>                                            m_TI;               // Index of the triangle nodes in Node matrix
    //std::vector<vec3, Eigen::aligned_allocator<vec3>>         m_TP;               // Triangle position
    //std::vector<vec3, Eigen::aligned_allocator<vec3>>         m_TN;               // Triangle normal vector
    //std::vector<double>                                       m_TA;               // Triangle area
 
    std::vector<int>                                            m_TopI;
    
    double                                                      m_Depth;            // Cage depth
    std::vector<int>                                            m_BotI;
    int                                                         m_BotNode;          // Bottom node
    double                                                      m_BotForce;         // Bottom force
        
    double                                                      m_Density;          // Net density
    std::vector<double>                                         m_A;                // Node area
    int                                                         m_BNum;             // Number of connections
    std::vector<double>                                         m_BL;               // Bar length
 
    int                                                         m_ENum;             // Number of edges
 
    bool                                                        m_BotEdge;          // Boolean, true if bottom edges are checked
    std::vector<int>                                            m_BotELookupArray;
    int                                                         m_BotENum;          // Number of bottom edges
 
    vec3                                                        m_Current;          // Current velocities
    double                                                      m_NetE;             // Net elastic modulus
 
    double                                                      m_TopConnL;         // Top connection length
    ISignalPort*                                                m_WinchTime;        // Input port, when to winch
 
    ISignalPort**                                               m_TopInPos;         // Input port, top positions
    ISignalPort**                                               m_TopInVel;         // Input port, top velocities
 
    ISignalPort*                                                m_TopInBodyPos;     // Input port
    ISignalPort*                                                m_TopInBodyQuat;    // Input port
 
    int                                                         m_BotRI;
    int                                                         m_EBNum;
    int                                                         m_BBNum;
    
    int                                                         m_NetOutI[4];
    double                                                      m_NetOutR[2];
 
    int*                                                        m_MatNetOutI;
    double*                                                     m_MatNetOutR;
 
    std::vector<double>                                         m_RingPos;
    int                                                         m_RingOutI[2];
    double                                                      m_RingOutR[2];
 
    double                                                      m_SkirtDepth;
    int                                                         m_SkirtI;
 
    double                                                      m_BotRingForce;
 
    bool                                                        m_ModelBottom;
 
    int                                                         m_TopINum;
    std::vector<vec3, Eigen::aligned_allocator<vec3>>           m_TopPPos;
    std::vector<vec3, Eigen::aligned_allocator<vec3>>           m_TopTPos;
 
    double                                                      m_VelRedFactor;
 
    ISignalPort**                                      m_PortInPos;
    ISignalPort**                                      m_PortInVel;
    ISignalPort**                                      m_PortInForce;
 
    bool PointInsideTriangle(vec3& P, vec3& A, vec3& B, vec3& C);
    int Factorial(int n);
 
    ISignalPort*                                                m_BotConForce;          // Input port, bottom connection force
 
    double                                            m_OutputDT;
    int                                               m_OutputNum;
   //============================================================================ herman_dev
   //double                                          m_prev_time = 0;
   CPrintDuringExec*                               m_logger;
   //============================================================================ herman_dev
 
 
};
};