LimpModel.h

#ifndef LIMPMODEL_H_
#define LIMPMODEL_H_

#include <cap_gait/contrib/Limp.h>
#include <cap_gait/contrib/LimpState.h>
#include <cap_gait/contrib/Vec2f.h>
#include <cap_gait/contrib/Vec3f.h>
#include <cap_gait/cap_gait_config.h>

namespace margait_contrib
{

struct LimpModel
{
    const cap_gait::CapConfig* config;
    double systemIterationTime;
    double fusedAngleX;
    double fusedAngleY;

//  char name;
    Vec3f gcv; // input

    // current state
    double x;
    double vx;
    double ax;
    double y;
    double vy;
    double ay;
    double energyX;
    double energyY;
    int supportLegSign;
    bool crossing;
    bool stalling;

    LimpState nominalState;
    Vec2f nominalCapturePoint;
    Vec3f nominalFootStep;
    Vec3f nominalStepSize;
    double nominalFootStepTHalf;

    double timeSinceStep;
    double timeToStep;
    double nominalTimeToStep;

    Vec2f zmp;
    LimpState endOfStepState;
    Vec3f footStep;
    Vec3f stepSize;

    Vec2f rxZmp;
    Vec2f rxCapturePoint;

private:
    Limp limp;

public:
    explicit LimpModel(const cap_gait::CapConfig* capConfig);
    ~LimpModel(){};

    void reset()
    {
        LimpState ms;
        ms.reset(); // Just to be safe
        setState(ms, Vec3f(0.0, 0.0, 0.0)); // TODO: This is probably not such a good idea as a zero LimpState is actually highly atypical and a situation that is in unstable equilibrium, leading to possibly weird timeToStep's and so on.
        updateInputData(systemIterationTime, 0.0, 0.0);
    }

    inline void updateInputData(double systemIterationTime, double fusedAngleX, double fusedAngleY)
    {
        this->systemIterationTime = systemIterationTime;
        this->fusedAngleX = fusedAngleX;
        this->fusedAngleY = fusedAngleY;
    }

    void setState(LimpState ms, Vec3f ggcv);
    bool forwardThroughStep(double dt);
    void forward(double dt);
    LimpModel forwarded(double t);
    void step(); // Make a footstep in the model

    double timeToLoc(double loc);
    double timeToSel();
    double timeToApex();

    LimpState mirroredToRight();
    LimpState getMotionState();
    LimpState getNominalState();
    LimpState getEndOfStepState();
    Vec2f getCapturePoint();

    inline bool operator==(const LimpModel& v) const
    {
        return (x==v.x) && (vx==v.vx) && (y==v.y) && (vy==v.vy);
    }
    inline bool operator!=(const LimpModel& v) const
    {
        return (x!=v.x) || (vx!=v.vx) || (y!=v.y) || (vy!=v.vy);
    }
};

}

#endif