WrappingCylinder.h

#ifndef BIORBD_MUSCLES_WRAPPING_CYLINDER_H
#define BIORBD_MUSCLES_WRAPPING_CYLINDER_H
 
#include "biorbdConfig.h"
#include "Muscles/WrappingObject.h"
 
namespace biorbd {
namespace muscles {
class BIORBD_API WrappingCylinder : public biorbd::muscles::WrappingObject
{
public:
    WrappingCylinder();
 
    WrappingCylinder(
            const biorbd::utils::RotoTrans& rt,
            const biorbd::utils::Scalar& diameter,
            const biorbd::utils::Scalar& length,
            bool isCylinderPositiveSign);
 
    WrappingCylinder(
            const biorbd::utils::RotoTrans& rt,
            const biorbd::utils::Scalar& diameter,
            const biorbd::utils::Scalar& length,
            bool isCylinderPositiveSign,
            const biorbd::utils::String& name,
            const biorbd::utils::String& parentName);
 
    biorbd::muscles::WrappingCylinder DeepCopy() const;
 
    void DeepCopy(
            const biorbd::muscles::WrappingCylinder& other);
 
    void wrapPoints(
            const biorbd::utils::RotoTrans& rt,
            const biorbd::utils::Vector3d& p1_bone,
            const biorbd::utils::Vector3d& p2_bone,
            biorbd::utils::Vector3d& p1,
            biorbd::utils::Vector3d& p2,
            biorbd::utils::Scalar* length = nullptr);
 
    void wrapPoints(
            biorbd::rigidbody::Joints& model,
            const biorbd::rigidbody::GeneralizedCoordinates& Q,
            const biorbd::utils::Vector3d& p1_bone,
            const biorbd::utils::Vector3d& p2_bone,
            biorbd::utils::Vector3d& p1,
            biorbd::utils::Vector3d& p2,
            biorbd::utils::Scalar* length = nullptr) ;
 
    void wrapPoints(
            biorbd::utils::Vector3d& p1,
            biorbd::utils::Vector3d& p2,
            biorbd::utils::Scalar* length = nullptr);
 
    virtual const biorbd::utils::RotoTrans& RT(
            biorbd::rigidbody::Joints &model,
            const biorbd::rigidbody::GeneralizedCoordinates& Q,
            bool updateKin = true);
 
    void setDiameter(
            const biorbd::utils::Scalar& val);
 
    const biorbd::utils::Scalar& diameter() const;
 
    biorbd::utils::Scalar radius() const;
 
    void setLength(
            const biorbd::utils::Scalar& val);
 
    const biorbd::utils::Scalar& length() const;
 
protected:
    class NodeMusclePair{
    public:
        NodeMusclePair(
                const biorbd::utils::Vector3d &p1,
                const biorbd::utils::Vector3d &p2) :
            m_p1(std::make_shared<biorbd::utils::Vector3d>(p1)),
            m_p2(std::make_shared<biorbd::utils::Vector3d>(p2))
        {}
        std::shared_ptr<biorbd::utils::Vector3d> m_p1; 
        std::shared_ptr<biorbd::utils::Vector3d> m_p2; 
    };
 
 
    void findTangentToCircle(
            const biorbd::utils::Vector3d& p,
            biorbd::utils::Vector3d& p_tan) const;
    
    void selectTangents(
            const NodeMusclePair&p,
            biorbd::utils::Vector3d& p_tan) const;
 
    bool findVerticalNode(
            const NodeMusclePair& pointsInGlobal,
            NodeMusclePair& pointsToWrap) const;
 
#ifdef BIORBD_USE_CASADI_MATH
    biorbd::utils::Scalar checkIfWraps(
#else
    bool checkIfWraps(
#endif
            const NodeMusclePair &pointsInGlobal,
            NodeMusclePair &pointsToWrap) const;
 
    biorbd::utils::Scalar computeLength(
            const NodeMusclePair &p) const;
 
    std::shared_ptr<biorbd::utils::Scalar> m_dia; 
    std::shared_ptr<biorbd::utils::Scalar> m_length; 
    std::shared_ptr<bool> m_isCylinderPositiveSign; 
    std::shared_ptr<biorbd::utils::RotoTrans> m_RTtoParent; 
 
    std::shared_ptr<biorbd::utils::Vector3d> m_p1Wrap; 
    std::shared_ptr<biorbd::utils::Vector3d> m_p2Wrap; 
    std::shared_ptr<biorbd::utils::Scalar> m_lengthAroundWrap ; 
 
};
 
}}
 
#endif // BIORBD_MUSCLES_WRAPPING_CYLINDER_H