TrawlCableConstraintSets.h

#ifndef TRAWLCABLE_CONSTRAINT_SETS_H
#define TRAWLCABLE_CONSTRAINT_SETS_H
#include <fhsim_environment/EnvironmentProvider.h>
#include <fhsim_coribo/CollisionManager.h>
#include <fhsim_coribo/JointConstraint.h>
#include <fhsim/ThreadPool.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 ThreadPool::Task{
    public:
        CableSegmentTask(int ID, TrawlCableJointConstraint* master, RigidCoupling& data, std::vector<int> elements/*, int start, int end*/):Task(ID){
            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;
    //ThreadPool::ThreadManager* m_ThreadManager;
    CoreBoundThreadPool *m_ThreadManager;
    int m_ThreadManagerGroupID;
 
    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;
    };
 
    struct ClumpWeight {
        double length;
        double radius;
        double Mass;
        int Index;
    };
 
    EnvironmentForces(ConstraintSolver* solver, double timeConstant/*, CableSegment Cable*/);
    void AddNewCableSegment(CableSegment cable);
    void ComputeConstraints(const double T, const double * const X);
    void AddTrawlDoor(TrawlDoor Door);
    void AddClumpWeight(ClumpWeight Weight);
    void setEnvironmentPointer(environment::EnvironmentProvider* environment);
protected:
    std::vector<CableSegment> m_Cables;
    std::vector<TrawlDoor> m_TrawlDoors;
    std::vector<ClumpWeight> m_ClumpWeights;
 
    double m_timeConstant;
    environment::EnvironmentProvider* m_environment;
};
 
}
 
#endif