InitializerTools3DCurve.h

#pragma once
#include "eigen_matrix_defs.h"
#include <vector>
#include "sfh/cable/catenary.h"
 
namespace CoRiBoDynamics {
    namespace Utilities {
 
 
        /*
            Interface class for a general curve initalizer for 6-dof elements
        */
        class CurveSegment{
        public:
            virtual ~CurveSegment(){};
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) = 0;
            virtual double Length() = 0;
            vec3 GetStartPoint(){vec3 P; Quat Q; Compute(0,0,P,Q); return P;}
            vec3 GetEndPoint(){vec3 P; Quat Q; Compute(Length(),0,P,Q); return P;}
        };
 
        /*
            Combines several curve segments in sequence
        */
        class CompositeCurve : public CurveSegment{
        public:
            CompositeCurve();
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) override;
            void AddCurveSegment(CurveSegment* curve_segment);
            virtual double Length() override;
        protected:
            struct segment{ double start_coordinate; double end_coordinate; CurveSegment* curve_segment;};
            std::vector<segment> m_segments;
            double m_accumulated_length;
        };
 
        /*
            Straight line curve, starting from point P0, in the direction of the unit vector N
        */
        class StraightSegment : public CurveSegment{
        public:
            StraightSegment(const vec3& P0, const vec3& N, double length);
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) override;
            virtual double Length() override;
 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        protected:
            vec3 m_P0;
            vec3 m_N;
            Quat m_Q;
            double m_length;
        };
 
        /*
            Catenary curve from point A to B, with the given length
        */
        class CatenarySegment : public CurveSegment{
        public:
            CatenarySegment(const vec3& PA, const vec3& PB, double length);
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) override;
            virtual double Length() override;
 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        protected:
            sfh::cable::Catenary3D m_curve;
            double m_length;
        };
 
        class RotatedCatenarySegment: public CatenarySegment{
        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            RotatedCatenarySegment(const Quat& Q, const vec3& PA, const vec3& PB, double length);
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) override;
        protected:
            Quat m_Q;
        };
 
        /*
            CurveSegment implementation that defines a helix curve
        */
        class HelixSegment : public CurveSegment{
        public:
            HelixSegment(double inner_radius, double curve_radius, double z_offset, double length = std::numeric_limits<double>::max());
            virtual void Compute(double parameter_coordinate, double element_length, vec3& Position, Quat& Orientation) override;
            virtual double Length() override;
 
            void SetOrientation(const vec3& P, const Quat& Q);
            void SetOrientation(const vec3& P, const Quat& Q, bool right_winding);
 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        protected:
            double m_major_radius;
            double m_delta;
            double m_theta_offset;
            double m_helix_length_factor;
            vec3 m_P;
            Quat m_Q;
            bool m_right_winding;
            double m_length;
        };
 
        /*
            Defines a circle sector curve with the specified length that intersects point A and B. 
            A and B must have a horizontal distance greater than 0.
            The circle sector points downwards with gravity.
        */
        class CircleInitCurve: public CurveSegment {
        public:
            CircleInitCurve(const vec3& PA, const vec3& PB, double Length);
 
            void Compute(double cable_coordinate, double element_length, vec3& P, Quat& Q);
            double Length(){return m_Length;}
 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        private:
            static double Length_Function(double D, double H);
            static double Length_Function_Derivative(double D, double H);
            double m_Length;
            Quat m_Q;
            vec3 m_Center;
            vec3 m_unit_rotation_axis;
            vec3 m_rotation_arm;
            double m_radius;
            //double m_theta0;
            Quat m_Q0;
        };
    } 
}