wak_utils.h

// Walk and kick: Behaviour utilities
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
 
 // Ensure header is only included once
#ifndef WAK_UTILS_H
#define WAK_UTILS_H

// Includes
#include <walk_and_kick/wak_common.h>
#include <config_server/parameter.h>
#include <field_model/field_model.h>
#include <rc_utils/math_spline.h>

// Walk and kick namespace
namespace walk_and_kick
{
    // Using declarations
    using field_model::FieldModel;

    class WAKUtils
    {
    public:
        // Tangent information struct
        struct TangentInfo
        {
            TangentInfo() : posAngle(0.0f), negAngle(0.0f), maxAdjust(0.0f), length(0.0f) {}
            float posAngle;
            float negAngle;
            float maxAdjust;
            float length;
        };

        // Calculate the tangents from a point to a circle of given centre and radius
        static void calcCircleTangent(TangentInfo& TI, const Vec2f& point, const Vec2f& centre, float radius);

        // Calculate a path from one position to another, subject to a halo (circle) of given centre and radius
        static float calculateHaloPath(Vec2f& fromNormal, const Vec2f& fromPos, const Vec2f& toPos, const Vec2f& centre, float radius, bool smoothFromNormal = true) { Vec2fArray path; return calculateHaloPath(fromNormal, path, fromPos, toPos, centre, radius, smoothFromNormal); }
        static float calculateHaloPath(Vec2f& fromNormal, Vec2fArray& path, const Vec2f& fromPos, const Vec2f& toPos, const Vec2f& centre, float radius, bool smoothFromNormal = true);

        // Calculate the wedge from a particular angle, to another particular angle, possibly forcing the sign of the output in the process
        static float calcWedge(float fromAngle, float toAngle, int forceSign = 0);

    private:
        // Constructor
        WAKUtils() {}
    };

    class Counter
    {
    public:
        Counter() { reset(); }
        void reset() { m_count = 0; }
        int count() const { return m_count; }
        void add(bool expr = true) { if(expr) m_count++; else m_count = (m_count > 0 ? m_count - 1 : 0); }
        void increment() { m_count++; }
        bool reached(int limit) const { return (m_count >= limit); }
    private:
        int m_count;
    };

    class TheWorm
    {
    public:
        explicit TheWorm(int limit = 100) { reset(limit); }
        void reset() { m_worm = 0; }
        void reset(bool decision) { if(decision) m_worm = m_limit; else m_worm = -m_limit; }
        void reset(int limit) { reset(); setLimit(limit); }
        void resetR(int range) { reset(); setRange(range); }
        void setLimit(int limit) { m_limit = (limit >= 1 ? limit : 1); } // The count from zero to a positive or negative decision
        void setRange(int range) { m_limit = (range >= 2 ? range >> 1 : 1); } // The count from a decision to the opposite decision
        void setWorm(int value) { m_worm = (value >= m_limit ? m_limit : (value <= -m_limit ? -m_limit : value)); }
        void vote(bool decision) { if(decision) m_worm = (m_worm < m_limit ? m_worm + 1 : m_limit); else m_worm = (m_worm > -m_limit ? m_worm - 1 : -m_limit); }
        void vote(bool decision, int numVotes) { if(numVotes <= 0) return; if(decision) m_worm = std::min(m_worm + numVotes, m_limit); else m_worm = std::max(m_worm - numVotes, -m_limit); }
        void neutralise() { if(m_worm > 0) m_worm = std::min(m_worm - 1, m_limit); else if(m_worm < 0) m_worm = std::max(m_worm + 1, -m_limit); }
        int  limit() const { return m_limit; }
        int  range() const { return m_limit << 1; }
        int  count() const { return m_worm; }
        bool decision() const { return (m_worm >= 0); } // Balanced votes is a decision of true!
        bool unanimousTrue() const { return (m_worm >= m_limit); }
        bool unanimousFalse() const { return (m_worm <= -m_limit); }
        bool unanimous() const { return (unanimousTrue() || unanimousFalse()); }
        void updateWormTime(const config_server::Parameter<float>* param) { setRange(TO_COUNT(param->get())); }
    private:
        int m_limit;
        int m_worm;
    };

    class LiveTrapVelSpline
    {
    public:
        LiveTrapVelSpline() { reset(); }
        void reset() { m_valid = false; m_x = m_v = m_t = 0.0; }
        void setState(double x, double v = 0.0) { m_x = x; m_v = v; m_t = 0.0; m_valid = false; }
        bool valid() const { return m_valid; }
        bool finished() const { return (!m_valid || m_t >= m_spline.T()); }
        double curX() const { return m_x; }
        double curV() const { return m_v; }
        void newTarget(double x, double v, double maxVel, double maxAcc, bool ctsVel = true)
        {
            m_spline.setParams(m_x, (ctsVel ? m_v : 0.0), x, v, maxVel, maxAcc);
            m_valid = true;
            m_t = 0.0;
        }
        double forward(double dT)
        {
            if(!m_valid) return m_x;
            m_t += dT;
            m_x = m_spline.x(m_t);
            m_v = m_spline.v(m_t);
            return m_x;
        }
    private:
        rc_utils::TrapVelSpline m_spline;
        bool m_valid; // Flag whether the spline is currently valid
        double m_x;   // Current position
        double m_v;   // Current velocity
        double m_t;   // Current time relative to the start of the spline (only if valid)
    };

    class TrapVelSpline2D
    {
    public:
        TrapVelSpline2D() { reset(); }
        void reset() { m_valid = false; m_x = m_xi = m_xf = 0.0; m_y = m_yi = m_yf = 0.0; m_t = 0.0; }
        void setState(double x, double y) { m_valid = false; m_x = m_xi = m_xf = x; m_y = m_yi = m_yf = y; m_t = 0.0; }
        bool valid() const { return m_valid; }
        bool finished() const { return (!m_valid || m_t >= m_spline.T()); }
        double curX() const { return m_x; }
        double curY() const { return m_y; }
        double targetX() const { return m_xf; }
        double targetY() const { return m_yf; }
        void newTarget(double x, double y, double maxVel, double maxAcc)
        {
            m_xi = m_x;
            m_yi = m_y;
            m_xf = x;
            m_yf = y;
            double dx = m_xf - m_xi;
            double dy = m_yf - m_yi;
            double D = sqrt(dx*dx + dy*dy);
            m_spline.setParams(0.0, 0.0, D, 0.0, maxVel, maxAcc);
            m_valid = true;
            m_t = 0.0;
        }
        void forward(double dT)
        {
            if(!m_valid) return;
            m_t += dT;
            double D = m_spline.xf();
            if(D == 0.0)
            {
                m_x = m_xf;
                m_y = m_yf;
            }
            else
            {
                double d = m_spline.x(m_t);
                double r = d/D;
                m_x = m_xi + r*(m_xf - m_xi);
                m_y = m_yi + r*(m_yf - m_yi);
            }
        }
    private:
        rc_utils::TrapVelSpline m_spline;
        bool m_valid; // Flag whether the spline is currently valid
        double m_xi;  // Initial x position of the current spline (only if valid)
        double m_yi;  // Initial y position of the current spline (only if valid)
        double m_xf;  // Final x position of the current spline (only if valid)
        double m_yf;  // Final y position of the current spline (only if valid)
        double m_x;   // Current x position
        double m_y;   // Current y position
        double m_t;   // Current time relative to the start of the spline (only if valid)
    };

    class FieldDimensions
    {
    public:
        // Constructor
        FieldDimensions() : m_field(FieldModel::getInstance()) {}

        // Field parameters
        FieldModel::FieldType fieldType() const { return m_field->type(); } 
        const std::string& fieldTypeName() const { return m_field->typeName(); } 
        float fieldLength() const { return m_field->length(); } 
        float fieldWidth() const { return m_field->width(); } 
        float circleDiameter() const { return m_field->centerCircleDiameter(); } 
        float boundary() const { return m_field->boundary(); } 
        float goalWidth() const { return m_field->goalWidth(); } 
        float goalAreaLength() const { return m_field->goalAreaDepth(); } 
        float goalAreaWidth() const { return m_field->goalAreaWidth(); } 
        float penaltyMarkDist() const { return m_field->penaltyMarkerDist(); } 
        float ballDiameter() const { return m_field->ballDiameter(); } 
        float fieldLengthH() const { return 0.5*m_field->length(); } 
        float fieldWidthH() const { return 0.5*m_field->width(); } 
        float circleRadius() const { return 0.5*m_field->centerCircleDiameter(); } 
        float goalWidthH() const { return 0.5*m_field->goalWidth(); } 
        float ballRadius() const { return 0.5*m_field->ballDiameter(); } 

    private:
        // Field model
        const FieldModel* const m_field;
    };
}

#endif
// EOF