TrawlCableConstraintSets.h

#ifndef TRAWLCABLE_CONSTRAINT_SETS_H
#define TRAWLCABLE_CONSTRAINT_SETS_H
#include "CableSegmentWaterForce.h"
 
#include <fhsim_environment/BodyWake.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <fhsim_environment/ElementFlowInteraction.h>
#include <fhsim_environment/SeafloorForcesInterface.h>
#include <fhsim_coribo/CollisionManager.h>
#include <fhsim_coribo/JointConstraint.h>
#include <vector>
 
namespace CoRiBoDynamics{
 
class TrawlCableJointConstraint: public JointConstraint{
public:
    struct CableProperties {
        double AlphaN;
        double BetaN;
        double GammaN;
        Quat AlphaM;
        Quat BetaM;
        Quat GammaM;
        double elementLength;
        std::vector<int> elements;
    };
 
    TrawlCableJointConstraint(ConstraintSolver* solver);
    size_t AddCableSegment(const CableProperties& cable_properties);
    virtual ~TrawlCableJointConstraint();
    virtual void ComputeConstraints(const double T, const double * const X);
 
    vec3 GetBallJointCouplingForce(int i);
 
protected:
    class CableSegmentTask{
    public:
        CableSegmentTask(TrawlCableJointConstraint* master, RigidCoupling& data, std::vector<int> elements/*, int start, int end*/){
            m_master = master;
            m_CouplingData = data;
            m_elements = elements;
        }
 
        void SetStates(double T, const double* X){
            m_T = T;
            m_X = X;
        }
 
        void ExecuteTask(){
            for(int i = 0; i < m_elements.size()-1; ++i){
                m_CouplingData.elementA = m_elements[i];
                m_CouplingData.elementB = m_elements[i+1];
                m_master->ComputeRigidCoupling(m_CouplingData);
            }
        }
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    protected:
        double m_T;
        const double* m_X;
        RigidCoupling m_CouplingData;
        std::vector<int> m_elements;
        TrawlCableJointConstraint* m_master;
    };
 
    std::vector<CableSegmentTask*> m_CableSegmentTask;
 
    std::vector<vec3> m_BallJointForces;
    //vec3 TrawlCableComputeBallJointCoupling( BallJointCoupling& Constraint, const double * const X );
};
 
class TrawlCableCollisionManager: public CollisionManager{
public:
    struct CableCollisionManagerProperties {
    double elementLength;
    double elementRadius;
    double elementContactStiffness;
    double frictionCoefficient;
    double contactDamping;
    std::vector<int> elements;
    };
 
    TrawlCableCollisionManager(ConstraintSolver* solver, double timeConstant);
    void AddCableSegment(const CableCollisionManagerProperties& properties);
    void AddMainCableSegment(const CableCollisionManagerProperties& properties);
 
    double getFrequencyCoefficient() {
        return m_timeConstant;
    }
 
    void setStartTimeCollision(double time) {
        m_startTimeCollision = time;
    }
 
    struct ElementContact{
        int ia;     
        int ib;     
        double la;  
        double lb;  
        double MainSegmentPosition;
        vec3 Fn;    
    };
    std::vector<ElementContact>* GetContactPoints();
 
protected:
    double m_startTimeCollision;
    void ComputeConstraints(const double T, const double * const X);
    struct CableSegment {int start; int end; double elementLength; int firstElementInSegment; int lastElementInSegment;};
    std::vector<CableSegment> m_CableSegments;
 
    std::vector<ElementContact> m_ContactPoints;
    void CollisionTest( InternalObject* objectA, InternalObject* objectB , const double * const X, ElementContact& contact);
    void TestAgainstMainSegments(int i, const double * const X);
 
public:
 
};
 
class EnvironmentForces: public ConstraintSet{
public:
    struct CableSegment {
        std::vector<int> elements;
        double cableRadius;
        double elementLength;
        double elementMass;
        double elementDisplacementVolume;
    };
    struct TrawlDoor {
        double length;
        double width;
        double DoorMass;
        double WeightMass;
        double displacementVolume;
        int DoorIndex;
        int WeightIndex;
        //vec3 HydroCenter;
 
        trawl_cable_water::Door Law() const { return {length, width, DoorMass, displacementVolume}; }
    };
 
    struct ClumpWeight {
        double length;
        double radius;
        double Mass;
        int Index;
 
        trawl_cable_water::ClumpWeight Law() const { return {length, radius, Mass}; }
    };
 
    EnvironmentForces(ConstraintSolver* solver, double timeConstant, const environment::wake::BodyWake& bodyWake);
    void AddNewCableSegment(CableSegment cable);
    void ComputeConstraints(const double T, const double * const X);
    void AddTrawlDoor(TrawlDoor Door);
    void AddClumpWeight(ClumpWeight Weight);
    void FinalSetup(const std::string& objectName, ISimObjectCreator* creator);
    void TakeWakes(const double T, const double * const X);
    environment::EnvironmentProvider* const* EnvironmentPointerAddress() const { return &m_environment; }
    int NumBodies() const;
    const TrawlDoor* DoorOfBody(int body) const;
    const ClumpWeight* ClumpOfBody(int body) const;
protected:
    friend class DoorAndClumpWakeBodies; // reads the elements of the doors and clump weights
 
    std::vector<CableSegment> m_Cables;
    std::vector<TrawlDoor> m_TrawlDoors;
    std::vector<ClumpWeight> m_ClumpWeights;
 
    double m_timeConstant;
    environment::EnvironmentProvider* m_environment;
    environment::SeafloorForcesInterface* m_seafloor = nullptr; 
    const environment::wake::BodyWake& m_bodyWake; 
    environment::wake::ElementFlowInteraction m_flow; 
};
}
 
#endif