problem.h

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
//
// Author: Ugo Pattacini - <ugo.pattacini@iit.it>
 
#ifndef PROBLEM_H
#define PROBLEM_H
 
#include <memory>
#include <string>
#include <vector>
#include <tuple>
#include <utility>
#include <cmath>
#include <algorithm>
#include <yarp/sig/Vector.h>
 
namespace problem_ns {
 
struct Force {
    double t{0.};
    double fn{0.};
    double ft{0.};
};
 
class Problem
{
    bool configured{false};
    std::vector<double> ti{M_PI/4.0, 3.0*M_PI/4.0, 5.0*M_PI/4.0, 7.0*M_PI/4.0};
    std::vector<double> si{.2, .2, .2, .2};
    std::vector<double> ci{0., 0., 0., 0.};
    yarp::sig::Vector COM=yarp::sig::Vector(2,0.);
    double friction{0.};
    Force F;
 
    size_t get_quadrant(const double t) const;
    std::tuple<double,size_t,double> calc_quantities(const double t) const;
    yarp::sig::Vector calc_COM();
    yarp::sig::Vector get_d2P(const double t) const;
 
public:
    bool configure(const std::vector<double> &shape, const double friction,
                   const Force &F);
 
    static std::shared_ptr<Problem> generate();
 
    static double wrap_angle(const double t);
 
    const std::vector<double>& get_shape() const;
 
    double get_friction() const;
 
    const Force& get_F() const;
 
    const yarp::sig::Vector& get_COM() const;
        
    yarp::sig::Vector get_P(const double t) const;
    
    yarp::sig::Vector get_dP(const double t) const;
    
    yarp::sig::Vector get_T(const double t) const;
 
    yarp::sig::Vector get_dT(const double t) const;
 
    yarp::sig::Vector get_N(const double t) const;
 
    yarp::sig::Vector get_dN(const double t) const;
 
    std::pair<yarp::sig::Vector,double> compute_newton_law(const std::vector<Force>& forces) const;
 
    bool check_no_slippage(const Force& force) const;
 
    bool check_no_slippage(const std::vector<Force>& forces) const;
};
 
}
 
#endif