Bsplines.hh

Go to the documentation of this file.

 
#ifndef _BSPLINES_H_
#define _BSPLINES_H_
 
#include <math.h>
 
#include <deque>
#include <iostream>
#include <vector>
 
struct Point {
  double x;
  double y;
};
 
namespace PatternGeneratorJRL {
 
class Bsplines {
 public:
  Bsplines(long int degree);
 
  ~Bsplines();
 
  void GenerateDegree();
 
  // void GenerateKnotVector(std::string method);
 
  Bsplines DerivativeBsplines();
 
  int ComputeBasisFunctions(double t);
 
  // computes the basis function without the derivatives
  int ComputeBasisFunctionsRecursively(double t, std::deque<double> &knot,
                                       unsigned int degree);
  double Nij_t(int i, int j, double t, std::deque<double> &knot);
 
  double ComputeBsplines(double t);
 
  void SetDegree(long int degree);
 
  void SetControlPoints(std::vector<double> &control_points);
 
  void SetKnotVector(std::deque<double> &knot_vector);
 
  long int GetDegree() const;
 
  std::vector<double> GetControlPoints() const;
 
  std::deque<double> GetKnotVector() const;
 
  void PrintKnotVector() const;
 
  void PrintControlPoints() const;
 
  void PrintDegree() const;
 
 protected:
  long int m_degree;
 
  std::vector<double> m_control_points;
  std::vector<double> m_derivative_control_points;
  std::vector<double> m_sec_derivative_control_points;
 
  std::vector<std::vector<double> > m_basis_functions;
  std::vector<double> m_basis_functions_derivative;
  std::vector<double> m_basis_functions_sec_derivative;
 
  std::deque<double> m_knot;
};
 
class BSplinesFoot : public Bsplines {
 public:
  BSplinesFoot(double FT = 1.0, double IP = 0.0, double FP = 0.0,
               std::vector<double> ToMP = std::vector<double>(),
               std::vector<double> MP = std::vector<double>(), double IS = 0.0,
               double IA = 0.0, double FS = 0.0, double FA = 0.0);
 
  ~BSplinesFoot();
 
  void SetParameters(double FT, double IP, double FP, std::vector<double> ToMP,
                     std::vector<double> MP, double IS = 0.0, double IA = 0.0,
                     double FS = 0.0, double FA = 0.0);
  void SetParametersWithoutMPAndToMP(double FT, double IP, double FP, double IS,
                                     double IA, double FS, double FA);
 
  int Compute(double t, double &x, double &dx, double &ddx);
 
  void ComputeControlPointFrom2DataPoint();
  void ComputeControlPointFrom3DataPoint();
  void ComputeControlPointFrom4DataPoint();
 
  void GetParameters(double &FT, double &IP, double &FP,
                     std::vector<double> &ToMP, std::vector<double> &MP);
 
  std::vector<double> MP() { return m_MP; }
 
  std::vector<double> ToMP() { return m_ToMP; }
 
  double FT() { return m_FT; }
 
  void FT(double ft) { m_FT = ft; }
 
  double IP() { return m_IP; }
 
  double FP() { return m_FP; }
 
  void SetParametersWithInitFinalPose(double FT, double IP, double FP,
                                      std::vector<double> &ToMP,
                                      std::vector<double> &MP);
 
 private:
  double m_FT;  // final time
  double m_IP;  // Initial Position
  double m_IS;  // Initial Speed
  double m_IA;  // Initial Acceleration
  double m_FP;  // Final Position
  double m_FS;  // Final Speed
  double m_FA;  // Final Acceleration
  std::vector<double>
      m_ToMP;  // times to reach the middle (intermediate) positions
  std::vector<double> m_MP;  // middle (intermediate) positions
};
 
}  // namespace PatternGeneratorJRL
#endif /* _BSPLINES_H_*/