Winch.h

#pragma once
 
#include "SupergridCable.h"
#include <Eigen/StdVector>
#include "InitializerTools3DCurve.h"
#include <fstream>
#ifdef CORIBO_BOOST
#include <boost/container/map.hpp>
#else
//#include <unordered_set>
//#include <unordered_map>
#include <map>
#endif
 
#ifdef FH_VISUALIZATION
#include "sfh/ogre/C3DArrow.h"
#endif
 
namespace CoRiBoDynamics {
 
namespace Structures {
 
class SupergridWinchCable;
 
class Winch {
public:
    Winch(double drum_length, double drum_diameter, double flange_diameter, double flange_thickness, double beta/*, bool right_handed = true*/);
    virtual ~Winch();
    vec6 ReactionForces();
 
    void SetWinchRotation(double theta, double omega);
    double GetWinchTheta(){return m_theta;}
    double GetWinchOmega(){return m_omega;}
 
    Quat GetStandardizedOrientationNED(); 
 
    void SetStates(const vec3& P, const Quat& Q, const vec3& V, const vec3& W);
 
    JointConstraint::ExternalRigidCoupling* WinchCoupling(DiscreteElement* connection_element);
 
 
 
#ifdef FH_VISUALIZATION
    void RenderInit(Ogre::Root* const ogreRoot);
    void RenderUpdate(const double T, const double* const X);
#endif
 
 
    struct AttachedElement {
        DiscreteElement* element;
        Quat Q; 
        vec3 P; 
        double accumulated_length;
        double accumulated_volume;
        double accumulated_weight;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    /*
    Add this element to the winch
    */
    void PushElement(AttachedElement& attached_element);
    void PushElement(DiscreteElement* element, const vec3& P, const Quat& Q);
    void PushElement(DiscreteElement* element, const double* X);
 
    /*
    Remove the top element from the winch
    */
    void PopElement(ObjectFactoryStack<DiscreteElement::TempStates>& temp_states_buffer);
 
    vec3 P(){return m_P;}
    Quat Q(){return m_Q;}
    vec3 V(){return m_V;}
    vec3 W(){return m_W;}
    Quat baseQ(){return m_base_Q;}
 
    const std::vector<AttachedElement,Eigen::aligned_allocator<AttachedElement> > GetAttachedCableElements(){return m_attached_elements;}
 
    /*
    Returns a vector-of-vectors of collition candidates.
    The outer vector corresponds element-by-element to the argument vector, and each inner vector is a set of candidates for more accurate contact detection
    */
    std::vector<std::vector<CollisionManager::ExternalObject*> > GetAttachedGeometry(const std::vector<CollisionManager::InternalObject*>& InternalObjects);
    std::vector<CollisionManager::ExternalObject*> CollisionCandidates(GeometryTools::AABB aabb);
 
    double DrumVirtualRadius(){return m_virtual_radius;}
    double DrumRadius(){return 0.5*m_drum_diameter;}
    double DrumDiameter(){return m_drum_diameter;}
    double DrumLength(){return m_drum_length;}
    double FlangeRadius(){return 0.5*m_flange_diameter;}
    double FlangeDiameter(){return m_flange_diameter;}
 
    void SetLineStructure(SupergridWinchCable* line_structure);
 
    virtual void XmlInfo(TiXmlElement& xml_node);
 
    double GetRetractedLength();
 
    void WriteAttachedGeometryDataToFile(std::string filename);
 
    /*
        Utility class for reading a file of initial conditions.
    */
    class InitialStatesLoader{
    public:
        explicit InitialStatesLoader(std::string filename);
        std::vector<AttachedElement,Eigen::aligned_allocator<AttachedElement>>::iterator GetIterator();
        std::vector<AttachedElement,Eigen::aligned_allocator<AttachedElement>>::iterator GetEnd();
    private:
        std::vector<AttachedElement,Eigen::aligned_allocator<AttachedElement>> m_elements;
    };
    void LoadAttachedGeometryDataFromFile(InitialStatesLoader states, std::vector<DiscreteElement*>& super_elements, size_t& super_ix, size_t& sub_ix, const vec3& target_point);
    static void WinchInitFileInfo(std::string filename, std::vector<double>& angle, std::vector<double>& retracted_length);
 
    GeometryTools::AABB GetWinchAABB();
 
    class SpoolingDevice {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        SpoolingDevice(vec3 relative_position, double radius, double length, double beta);
        vec3 GetPassThroughPoint();
        void UpdateStates(vec3& winch_P, Quat& winch_base_Q, vec3& winch_V, vec3& winch_W);
        CollisionManager::ExternalObject* GetContactGeometry();
        vec3 RelativePosition(){return m_relative_position;}
 
#ifdef FH_VISUALIZATION
        virtual void RenderInit(Ogre::Root* const ogreRoot);
        virtual void RenderUpdate(const double T, const double* const X);
        Ogre::SceneNode*    m_node;
#endif
 
    protected:
        vec3 m_relative_position;
        double m_state_data[13];
        CollisionManager::ExternalObject m_railing;
    };
 
    void SetSpoolingDevice(vec3 relative_position, double rail_radius);
 
    SpoolingDevice* GetSpoolingDevice(){return m_spooling_device;}
    CollisionManager::ExternalObject* GetSpoolingDeviceContactGeometry();
 
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    void UpdateWinchDrumGeometry();
 
    class SpatialPartition{
    public:
        SpatialPartition();
 
        void Setup(Winch* winch);
 
        void InsertElement(GeometryTools::AABB aabb, int ix);
        void RemoveElement(GeometryTools::AABB aabb, int ix);
        void GetContactCandidates(GeometryTools::AABB aabb, DataStructures::RadixSparseSet& accumulated_candidates);
 
    protected:
        double m_lxy;
        double m_lz;
        int m_nxy;
        int m_nz;
 
        class Dim3Index{
        public:
            int GetNext();
 
            uint8_t ix;
            uint8_t iy;
            uint8_t iz;
            uint8_t lo_ix;
            uint8_t lo_iy;
            uint8_t lo_iz;
            uint8_t hi_ix;
            uint8_t hi_iy;
            uint8_t hi_iz;
            uint8_t nx;
            uint8_t ny;
            uint8_t nz;
        };
 
        std::vector<std::vector<unsigned short>> m_data;
 
        Dim3Index CreateIndex(GeometryTools::AABB aabb);
    };
    SpatialPartition m_spatial_partition;
 
    struct StateData{
        double P[3];
        double Q[4];
        double V[3];
        double W[3];
        StateData(){
            V3ptr(vec3::Zero(),P);
            Qptr(dblQ(0),Q);
            V3ptr(vec3::Zero(),V);
            V3ptr(vec3::Zero(),W);
        }
    };
 
    SupergridWinchCable* m_line_structure;
 
    double m_beta;
    public:
    struct ObjectAndStates {
        CollisionManager::ExternalObject element;
        StateData data;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
    protected:
    ObjectFactoryStack<ObjectAndStates> m_attached_geometry;
#ifdef CORIBO_BOOST
    boost::container::map<int,ObjectAndStates*> m_accumulated_candidates;
#else
   std::map<int,ObjectAndStates*> m_accumulated_candidates;
#endif
    std::vector<AttachedElement,Eigen::aligned_allocator<AttachedElement>> m_attached_elements; 
 
 
    std::vector<CollisionManager::ExternalObject*> m_winch_geometry;
    CollisionManager::ExternalObject m_drum_capsule;
    CollisionManager::ExternalObject m_flange1_disk;
    CollisionManager::ExternalObject m_flange2_disk;
 
    CollisionManager::ExternalObject m_block_pin_upper;
    CollisionManager::ExternalObject m_block_pin_lower;
 
    double m_drum_length;
    double m_drum_diameter;
    double m_flange_diameter;
    double m_flange_thickness;
    double m_virtual_radius;
 
    double m_nominal_cable_radius;
 
    double m_base_Q_array[4];
    double m_block_upper_V_array[3];
    double m_block_lower_V_array[3];
    double m_block_upper_P_array[3];
    double m_block_lower_P_array[3];
 
    double m_winch_Q_array[4];
    double m_flange1_V_array[3];
    double m_flange2_V_array[3];
    double m_flange1_P_array[3];
    double m_flange2_P_array[3];
 
    double m_theta; 
    double m_omega; 
    Quat m_base_Q; 
    vec3 m_base_W; 
 
    vec3 m_P; 
    Quat m_Q; 
    vec3 m_V; 
    vec3 m_W; 
 
    vec3 m_winch_base_pin_connection;
    vec3 m_winch_base_flange_connection;
    vec3 m_winch_base_connection_vector;
    JointConstraint::ExternalRigidCoupling m_winch_connection;
 
    SpoolingDevice *m_spooling_device;
 
 
#ifdef FH_VISUALIZATION
    Ogre::SceneNode*    m_winch_node;
    Ogre::SceneNode*    m_virtual_drum_node;
    std::vector<Ogre::SceneNode*> m_element_nodes;
    Ogre::SceneNode*    m_base_node;
#endif
};
 
 
 
class TowingBlock {
public:
    TowingBlock(ISimObjectCreator* creator, std::string state_prefix, double block_width, double block_diameter, double flange_diameter, double flange_thickness, double pivot_arm_length, double beta);
 
    vec6 ReactionForces();
 
    void SetStates(const vec3& P, const Quat& Q, const vec3& V, const vec3& W);
 
 
    const std::vector<CollisionManager::ExternalObject*> GetTowingBlockGeometry();
 
#ifdef FH_VISUALIZATION
    void RenderInit(Ogre::Root* const ogreRoot);
    void RenderUpdate(const double T, const double* const X);
#endif
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    vec3 P(){return m_P;}
    Quat Q(){return m_Q;}
    vec3 V(){return m_V;}
    vec3 W(){return m_W;}
 
    Quat GetStandardizedOrientationNED(); 
 
    void SetLineStructure(SupergridWinchCable* line_structure);
 
    double GetBlockRadius(){return 0.5*m_block_diameter;}
 
    vec3 PassThroughPoint();
    vec3 TopPoint();
    vec3 InletPoint();
    vec3 OutletPoint();
 
    virtual void XmlInfo(TiXmlElement& xml_node);
 
    void Flagging_OdeFcn(const double T, const double * const X, double * const XDot);
 
    vec3 PivotArm(); 
    vec3 NeutralPivotArm(); 
    vec3 BlockPosition(); 
    Quat BlockOrientation(); 
    double PivotAngle();
 
    void SetFlaggingResponseCoefficient(double c){m_flagging_response_coefficient = c;}
 
protected:
    struct StateData{
        double P[3];
        double Q[4];
        double V[3];
        double W[3];
    };
 
    SupergridWinchCable* m_line_structure;
    double m_beta;
 
    std::vector<CollisionManager::ExternalObject*> m_block_geometry;
    CollisionManager::ExternalObject m_block_capsule;
    CollisionManager::ExternalObject m_flange1_disk;
    CollisionManager::ExternalObject m_flange2_disk;
    CollisionManager::ExternalObject m_flange_cover_capsule;
 
    double m_block_width;
    double m_block_diameter;
    double m_flange_diameter;
    double m_flange_thickness;
    double m_pivot_arm_length;
 
    double m_pivot_arm_angle;
    int m_pivot_angle_ix;
 
    vec3 m_block_base_flange_connection;
    vec3 m_block_base_cover_connection;
 
    vec3 m_P; 
    Quat m_Q; 
    vec3 m_V; 
    vec3 m_W; 
 
 
 
    StateData m_block_states;
    StateData m_flange1_states;
    StateData m_flange2_states;
    StateData m_flange_cover_states;
 
    double m_flagging_response_coefficient;
 
#ifdef FH_VISUALIZATION
    Ogre::SceneNode*    m_block_node;
#endif
};
 
} 
}