Main.cpp

#include "curves/fwd.h"
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/curve_conversion.h"
#include "curves/cubic_hermite_spline.h"
#include "curves/piecewise_curve.h"
#include "curves/optimization/definitions.h"
#include "load_problem.h"
#include "curves/so3_linear.h"
#include "curves/se3_curve.h"
#include <string>
#include <iostream>
#include <cmath>
#include <ctime>
#include <boost/smart_ptr/shared_ptr.hpp>

using namespace std;

namespace curves {
typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
typedef exact_cubic_t::spline_constraints spline_constraints_t;

typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double, 1, 1> point_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef std::pair<pointX_t, pointX_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;



const double margin = 1e-3;
bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
bool QuasiEqual(const point3_t a, const point3_t b) {
  bool equal = true;
  for (size_t i = 0; i < 3; ++i) {
    equal = equal && QuasiEqual(a[i], b[i]);
  }
  return equal;
}
}  // End namespace curves

using namespace curves;

ostream& operator<<(ostream& os, const point3_t& pt) {
  os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
  return os;
}

void ComparePoints(const transform_t& pt1, const transform_t& pt2, const std::string& errmsg, bool& error,
                   double prec = Eigen::NumTraits<double>::dummy_precision() ,bool notequal = false) {
  if (!pt1.isApprox(pt2,prec)  && !notequal) {
    error = true;
    std::cout << errmsg <<" translation :"<<pt1.translation() << " ; " << pt2.translation() << std::endl<<"rotation : "<<pt1.rotation() << " ; " << pt2.rotation() << std::endl;
  }
}

void ComparePoints(const Eigen::MatrixXd& pt1, const Eigen::MatrixXd& pt2, const std::string& errmsg, bool& error,
                   double prec = Eigen::NumTraits<double>::dummy_precision() ,bool notequal = false) {
  if (!pt1.isApprox(pt2,prec) && !(pt1.isZero(prec) && pt2.isZero(prec)) && !notequal) {
    error = true;
    std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
  }
}

template <typename curve1, typename curve2>
void CompareCurves(const curve1& c1,const curve2& c2, const std::string& errMsg, bool& error ,double prec = Eigen::NumTraits<double>::dummy_precision()) {
  double T_min = c1.min();
  double T_max = c1.max();
  if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max())) {
    std::cout << errMsg<<"CompareCurves, ERROR, time min and max of curves do not match [" << T_min << "," << T_max << "] "
              << " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
    error = true;
  } else {
    // derivative in T_min and T_max
    ComparePoints(c1.derivate(T_min, 1), c2.derivate(T_min, 1), errMsg+" Derivates at tMin do not match.", error,prec, false);
    ComparePoints(c1.derivate(T_max, 1), c2.derivate(T_max, 1), errMsg+" Derivates at tMax do not match.", error,prec, false);
    // Test values on curves
    for (double i = T_min; i <= T_max; i += 0.01) {
      ComparePoints(c1(i), c2(i), errMsg+" Curves evaluation do not match at t = "+boost::lexical_cast<std::string>(i), error,prec, false);
    }
  }
}

/*Cubic Function tests*/
void PolynomialCubicFunctionTest(bool& error) {
  std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  t_pointX_t vec;
  vec.push_back(a);
  vec.push_back(b);
  vec.push_back(c);
  vec.push_back(d);
  polynomial_t cf(vec.begin(), vec.end(), 0, 1);
  point3_t res1;
  res1 = cf(0);
  point3_t x0(1, 2, 3);
  ComparePoints(x0, res1, errMsg + "(0) ", error);
  point3_t x1(9, 15, 19);
  res1 = cf(1);
  ComparePoints(x1, res1, errMsg + "(1) ", error);
  point3_t x2(3.125, 5.25, 7.125);
  res1 = cf(0.5);
  ComparePoints(x2, res1, errMsg + "(0.5) ", error);
  vec.clear();
  vec.push_back(a);
  vec.push_back(b);
  vec.push_back(c);
  vec.push_back(d);
  polynomial_t cf2(vec, 0.5, 1);
  res1 = cf2(0.5);
  ComparePoints(x0, res1, errMsg + "x3 ", error);
  error = true;
  try {
    cf2(0.4);
  } catch (...) {
    error = false;
  }
  if (error) {
    std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
  }
  error = true;
  try {
    cf2(1.1);
  } catch (...) {
    error = false;
  }
  if (error) {
    std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
  }
  if (!QuasiEqual(cf.max(), 1.0)) {
    error = true;
    std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
  }
  if (!QuasiEqual(cf.min(), 0.0)) {
    error = true;
    std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
  }
  // Test derivate and compute_derivative
  // Order 1
  curve_abc_t* cf_derivated = cf.compute_derivate_ptr(1);
  ComparePoints(cf.derivate(0, 1), (*cf_derivated)(0), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.3, 1), (*cf_derivated)(0.3), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.5, 1), (*cf_derivated)(0.5), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(1, 1), (*cf_derivated)(1), errMsg + " - derivate order 1 : ", error);
  // Order 2
  polynomial_t cf_derivated_2 = cf.compute_derivate(2);
  ComparePoints(cf.derivate(0, 2), (cf_derivated_2)(0), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.3, 2), (cf_derivated_2)(0.3), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.5, 2), (cf_derivated_2)(0.5), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(1, 2), (cf_derivated_2)(1), errMsg + " - derivate order 1 : ", error);
}

/*bezier_curve Function tests*/
void BezierCurveTest(bool& error) {
  std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  std::vector<point3_t> params;
  params.push_back(a);
  // 1d curve in [0,1]
  bezier_t cf1(params.begin(), params.end());
  point3_t res1;
  res1 = cf1(0);
  point3_t x10 = a;
  ComparePoints(x10, res1, errMsg + "1(0) ", error);
  res1 = cf1(1);
  ComparePoints(x10, res1, errMsg + "1(1) ", error);
  // 2d curve in [0,1]
  params.push_back(b);
  bezier_t cf(params.begin(), params.end());
  res1 = cf(0);
  point3_t x20 = a;
  ComparePoints(x20, res1, errMsg + "2(0) ", error);
  point3_t x21 = b;
  res1 = cf(1);
  ComparePoints(x21, res1, errMsg + "2(1) ", error);
  // 3d curve in [0,1]
  params.push_back(c);
  bezier_t cf3(params.begin(), params.end());
  res1 = cf3(0);
  ComparePoints(a, res1, errMsg + "3(0) ", error);
  res1 = cf3(1);
  ComparePoints(c, res1, errMsg + "3(1) ", error);
  // 4d curve in [1,2]
  params.push_back(d);
  bezier_t cf4(params.begin(), params.end(), 1., 2.);
  // testing bernstein polynomials
  bezier_t cf5(params.begin(), params.end(), 1., 2.);
  std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
  bezier_t cf5_derivated = cf5.compute_derivate(1);

  for (double d = 1.; d < 2.; d += 0.1) {
    ComparePoints(cf5.evalBernstein(d), cf5(d), errMsg2, error);
    ComparePoints(cf5.evalHorner(d), cf5(d), errMsg2, error);
    ComparePoints(cf5_derivated.evalBernstein(d), cf5_derivated(d), errMsg2, error);
    ComparePoints(cf5_derivated.evalHorner(d), cf5_derivated(d), errMsg2, error);
    ComparePoints(cf5.derivate(d,1), cf5_derivated(d), errMsg2, error);

  }
  bool error_in(true);
  try {
    cf(-0.4);
  } catch (...) {
    error_in = false;
  }
  if (error_in) {
    std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
    error = true;
  }
  error_in = true;
  try {
    cf(1.1);
  } catch (...) {
    error_in = false;
  }
  if (error_in) {
    std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
    error = true;
  }
  if (!QuasiEqual(cf.max(), 1.0)) {
    error = true;
    std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
  }
  if (!QuasiEqual(cf.min(), 0.0)) {
    error = true;
    std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
  }
}

void BezierCurveTestCompareHornerAndBernstein(bool&)  // error
{
  using namespace std;
  std::vector<double> values;
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  point3_t e(3, 61, 7);
  point3_t f(3, 56, 7);
  point3_t g(3, 36, 7);
  point3_t h(43, 6, 7);
  point3_t i(3, 6, 77);
  std::vector<point3_t> params;
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  // 3d curve
  bezier_t cf(params.begin(), params.end());  // defined in [0,1]
  // Check all evaluation of bezier curve
  clock_t s0, e0, s1, e1, s2, e2, s3, e3;
  s0 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf(*cit);
  }
  e0 = clock();
  s1 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf.evalBernstein(*cit);
  }
  e1 = clock();

  s2 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf.evalHorner(*cit);
  }
  e2 = clock();
  s3 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf.evalDeCasteljau(*cit);
  }
  e3 = clock();
  std::cout << "time for analytical eval  " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for bernstein eval   " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for horner eval      " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
  std::cout << "now with high order polynomial " << std::endl;
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  params.push_back(g);
  params.push_back(h);
  params.push_back(i);
  bezier_t cf2(params.begin(), params.end());
  s1 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf2.evalBernstein(*cit);
  }
  e1 = clock();
  s2 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf2.evalHorner(*cit);
  }
  e2 = clock();
  s0 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf2(*cit);
  }
  e0 = clock();
  s3 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    cf2.evalDeCasteljau(*cit);
  }
  e3 = clock();
  std::cout << "time for analytical eval  " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for bernstein eval   " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for horner eval      " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
}

void BezierDerivativeCurveTest(bool& error) {
  std::string errMsg("In test BezierDerivativeCurveTest ;, Error While checking value of point on curve : ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  std::vector<point3_t> params;
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  bezier_t cf3(params.begin(), params.end());
  ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true);
  ComparePoints(point3_t::Zero(), cf3.derivate(0., 100), errMsg, error);
}

void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
  std::string errMsg(
      "In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 4, 5);
  point3_t e(3, 4, 5);
  point3_t f(3, 4, 5);
  std::vector<point3_t> params;
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  double Tmin = 0.;
  double Tmax = 2.;
  double diffT = Tmax - Tmin;
  bezier_t cf(params.begin(), params.end());
  bezier_t cfT(params.begin(), params.end(), Tmin, Tmax);
  ComparePoints(cf(0.5), cfT(1), errMsg, error);
  ComparePoints(cf.derivate(0.5, 1), cfT.derivate(1, 1) * (diffT), errMsg, error);
  ComparePoints(cf.derivate(0.5, 2), cfT.derivate(1, 2) * diffT * diffT, errMsg, error);
}

void BezierDerivativeCurveConstraintTest(bool& error) {
  std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  bezier_t::curve_constraints_t constraints(3);
  constraints.init_vel = point3_t(-1, -1, -1);
  constraints.init_acc = point3_t(-2, -2, -2);
  constraints.end_vel = point3_t(-10, -10, -10);
  constraints.end_acc = point3_t(-20, -20, -20);
  std::vector<point3_t> params;
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  bezier_t::num_t T_min = 1.0;
  bezier_t::num_t T_max = 3.0;
  bezier_t cf(params.begin(), params.end(), constraints, T_min, T_max);
  ComparePoints(a, cf(T_min), errMsg0, error);
  ComparePoints(c, cf(T_max), errMsg0, error);
  ComparePoints(constraints.init_vel, cf.derivate(T_min, 1), errMsg0, error);
  ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
  ComparePoints(constraints.init_acc, cf.derivate(T_min, 2), errMsg0, error);
  ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
  ComparePoints(constraints.end_acc, cf.derivate(T_max, 2), errMsg0, error);
  std::string errMsg1(
      "In test BezierDerivativeCurveConstraintTest, Error While checking checking degree of bezier curve :");
  std::string errMsg2(
      "In test BezierDerivativeCurveConstraintTest, Error While checking checking size of bezier curve :");
  if (cf.degree_ != params.size() + 3) {
    error = true;
    std::cout << errMsg1 << cf.degree_ << " ; " << params.size() + 3 << std::endl;
  }
  if (cf.size_ != params.size() + 4) {
    error = true;
    std::cout << errMsg2 << cf.size_ << " ; " << params.size() + 4 << std::endl;
  }
}

void toPolynomialConversionTest(bool& error) {
  // bezier to polynomial
  std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  point3_t e(3, 61, 7);
  point3_t f(3, 56, 7);
  point3_t g(3, 36, 7);
  point3_t h(43, 6, 7);
  point3_t i(3, 6, 77);
  std::vector<point3_t> control_points;
  control_points.push_back(a);
  control_points.push_back(b);
  control_points.push_back(c);
  control_points.push_back(d);
  control_points.push_back(e);
  control_points.push_back(f);
  control_points.push_back(g);
  control_points.push_back(h);
  control_points.push_back(i);
  bezier_t::num_t T_min = 1.0;
  bezier_t::num_t T_max = 3.0;
  bezier_t bc(control_points.begin(), control_points.end(), T_min, T_max);
  polynomial_t pol = polynomial_from_curve<polynomial_t>(bc);
  CompareCurves<polynomial_t, bezier_t>(pol, bc, errMsg, error);
}

void cubicConversionTest(bool& error) {
  std::string errMsg0(
      "In test CubicConversionTest - convert hermite to, Error While checking value of point on curve : ");
  std::string errMsg1(
      "In test CubicConversionTest - convert bezier to, Error While checking value of point on curve : ");
  std::string errMsg2(
      "In test CubicConversionTest - convert polynomial to, Error While checking value of point on curve : ");
  // Create cubic hermite spline : Test hermite to bezier/polynomial
  point3_t p0(1, 2, 3);
  point3_t m0(2, 3, 4);
  point3_t p1(3, 4, 5);
  point3_t m1(3, 6, 7);
  pair_point_tangent_t pair0(p0, m0);
  pair_point_tangent_t pair1(p1, m1);
  t_pair_point_tangent_t control_points;
  control_points.push_back(pair0);
  control_points.push_back(pair1);
  std::vector<double> time_control_points;
  polynomial_t::num_t T_min = 1.0;
  polynomial_t::num_t T_max = 3.0;
  time_control_points.push_back(T_min);
  time_control_points.push_back(T_max);
  cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(), time_control_points);
  // hermite to bezier
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to bezier \n";
  bezier_t bc0 = bezier_from_curve<bezier_t>(chs0);
  CompareCurves<cubic_hermite_spline_t, bezier_t>(chs0, bc0, errMsg0, error);
  // hermite to pol
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to polynomial \n";
  polynomial_t pol0 = polynomial_from_curve<polynomial_t>(chs0);
  CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
  // pol to hermite
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to hermite \n";
  cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t>(pol0);
  CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0, chs1, errMsg2, error);
  // pol to bezier
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to bezier \n";
  bezier_t bc1 = bezier_from_curve<bezier_t>(pol0);
  CompareCurves<bezier_t, polynomial_t>(bc1, pol0, errMsg2, error);
  // Bezier to pol
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to polynomial \n";
  polynomial_t pol1 = polynomial_from_curve<polynomial_t>(bc0);
  CompareCurves<bezier_t, polynomial_t>(bc0, pol1, errMsg1, error);
  // bezier => hermite
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to hermite \n";
  cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t>(bc0);
  CompareCurves<bezier_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);

  // Test : compute derivative of bezier => Convert it to polynomial
  curve_abc_t* bc_der = bc0.compute_derivate_ptr(1);
  polynomial_t pol_test = polynomial_from_curve<polynomial_t>(*bc_der);
  CompareCurves<curve_abc_t, polynomial_t>(*bc_der, pol_test, errMsg1, error);
}

/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error) {
  // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
  curves::T_Waypoint waypoints;
  for (double i = 0.0; i <= 3.0; i = i + 0.5) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
  // Test number of polynomials in exact cubic
  std::size_t numberSegments = exactCubic.getNumberSplines();
  if (numberSegments != 6) {
    error = true;
    std::cout << "In ExactCubicNoErrorTest, Error While checking number of splines" << numberSegments << " ; " << 6
              << std::endl;
  }
  // Test getSplineAt function
  for (std::size_t i = 0; i < numberSegments; i++) {
    exactCubic.getSplineAt(i);
  }
  // Other tests
  try {
    exactCubic(0.0);
    exactCubic(3.0);
  } catch (...) {
    error = true;
    std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
  }
  error = true;
  try {
    exactCubic(3.2);
  } catch (...) {
    error = false;
  }
  if (error) {
    std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
  }
  if (!QuasiEqual(exactCubic.max(), 3.0)) {
    error = true;
    std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3 but is : " << exactCubic.max() << "\n";
  }
  if (!QuasiEqual(exactCubic.min(), 0.0)) {
    error = true;
    std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0 but is : " << exactCubic.min() << "\n";
  }
}

/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error) {
  // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
  curves::T_Waypoint waypoints;
  for (double i = 0.0; i < 2.0; ++i) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
  point3_t res1 = exactCubic(0);
  std::string errmsg0(
      "in ExactCubicTwoPointsTest, Error While checking that given wayPoints  are crossed (expected / obtained)");
  ComparePoints(point3_t(0, 0, 0), res1, errmsg0, error);
  res1 = exactCubic(1);
  ComparePoints(point3_t(1, 1, 1), res1, errmsg0, error);
  // Test number of polynomials in exact cubic
  std::size_t numberSegments = exactCubic.getNumberSplines();
  if (numberSegments != 1) {
    error = true;
    std::cout << "In ExactCubicTwoPointsTest, Error While checking number of splines" << numberSegments << " ; " << 1
              << std::endl;
  }
  // Test getSplineAt
  std::string errmsg1("in ExactCubicTwoPointsTest, Error While checking value on curve");
  ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1, error);
}

void ExactCubicOneDimTest(bool& error) {
  curves::T_WaypointOne waypoints;
  point_one zero;
  zero(0, 0) = 9;
  point_one one;
  one(0, 0) = 14;
  point_one two;
  two(0, 0) = 25;
  waypoints.push_back(std::make_pair(0., zero));
  waypoints.push_back(std::make_pair(1., one));
  waypoints.push_back(std::make_pair(2., two));
  exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
  point_one res1 = exactCubic(0);
  std::string errmsg(
      "in ExactCubicOneDim Error While checking that given wayPoints  are crossed (expected / obtained)");
  ComparePoints(zero, res1, errmsg, error);
  res1 = exactCubic(1);
  ComparePoints(one, res1, errmsg, error);
}

void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&,
                             const exact_cubic_t* curve, bool& error, double prec = Eigen::NumTraits<double>::dummy_precision()) {
  point3_t res1;
  for (double i = 0; i <= 1; i = i + step) {
    res1 = (*curve)(i);
    ComparePoints(point3_t(i, i, i), res1, errmsg, error,prec);
  }
}

void ExactCubicPointsCrossedTest(bool& error) {
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 1; i = i + 0.2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
  std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
  CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
}

void ExactCubicVelocityConstraintsTest(bool& error) {
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 1; i = i + 0.2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  std::string errmsg(
      "Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / "
      "obtained)");
  spline_constraints_t constraints(3);
  constraints.end_vel = point3_t(0, 0, 0);
  constraints.init_vel = point3_t(0, 0, 0);
  constraints.end_acc = point3_t(0, 0, 0);
  constraints.init_acc = point3_t(0, 0, 0);
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
  // now check that init and end velocity are 0
  CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
  std::string errmsg3(
      "Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
  // now check derivatives
  ComparePoints(constraints.init_vel, exactCubic.derivate(0, 1), errmsg3, error,1e-10);
  ComparePoints(constraints.end_vel, exactCubic.derivate(1, 1), errmsg3, error,1e-10);
  ComparePoints(constraints.init_acc, exactCubic.derivate(0, 2), errmsg3, error,1e-10);
  ComparePoints(constraints.end_acc, exactCubic.derivate(1, 2), errmsg3, error,1e-10);
  constraints.end_vel = point3_t(1, 2, 3);
  constraints.init_vel = point3_t(-1, -2, -3);
  constraints.end_acc = point3_t(4, 5, 6);
  constraints.init_acc = point3_t(-4, -4, -6);
  std::string errmsg2(
      "Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / "
      "obtained)");
  exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(), constraints);
  CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error,1e-10);
  std::string errmsg4(
      "Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
  // now check derivatives
  ComparePoints(constraints.init_vel, exactCubic2.derivate(0, 1), errmsg4, error,1e-10);
  ComparePoints(constraints.end_vel, exactCubic2.derivate(1, 1), errmsg4, error,1e-10);
  ComparePoints(constraints.init_acc, exactCubic2.derivate(0, 2), errmsg4, error,1e-10);
  ComparePoints(constraints.end_acc, exactCubic2.derivate(1, 2), errmsg4, error,1e-10);
}

template <typename CurveType>
void CheckPointOnline(const std::string& errmsg, const point3_t& A, const point3_t& B, const double target,
                      const CurveType* curve, bool& error) {
  point3_t res1 = curve->operator()(target);
  point3_t ar = (res1 - A);
  ar.normalize();
  point3_t rb = (B - res1);
  rb.normalize();
  if (ar.dot(rb) < 0.99999) {
    error = true;
    std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " << target << " ; "
              << res1.transpose() << std::endl;
  }
}

void EffectorTrajectoryTest(bool& error) {
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 10; i = i + 2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  helpers::exact_cubic_t* eff_traj = helpers::effector_spline(
      waypoints.begin(), waypoints.end(), Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0, 0, 2), 1, 0.02, 1, 0.5);
  point3_t zero(0, 0, 0);
  point3_t off1(0, 0, 1);
  point3_t off2(10, 10, 10.02);
  point3_t end(10, 10, 10);
  std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
  std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
  // first check start / goal positions
  ComparePoints(zero, (*eff_traj)(0), errmsg, error);
  ComparePoints(off1, (*eff_traj)(1), errmsg, error);
  ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
  ComparePoints(end, (*eff_traj)(10), errmsg, error);
  // now check derivatives
  ComparePoints(zero, (*eff_traj).derivate(0, 1), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(10, 1), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(0, 2), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(10, 2), errmsg2, error);
  // check that end and init splines are line
  std::string errmsg3(
      "Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / "
      "point obtained) \n");
  for (double i = 0.1; i < 1; i += 0.1) {
    CheckPointOnline<helpers::exact_cubic_t>(errmsg3, (*eff_traj)(0), (*eff_traj)(1), i, eff_traj, error);
  }
  for (double i = 9.981; i < 10; i += 0.002) {
    CheckPointOnline<helpers::exact_cubic_t>(errmsg3, (*eff_traj)(9.5), (*eff_traj)(10), i, eff_traj, error);
  }
  delete eff_traj;
}

helpers::quat_t GetXRotQuat(const double theta) {
  Eigen::AngleAxisd m(theta, Eigen::Vector3d::UnitX());
  return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
}

double GetXRotFromQuat(helpers::quat_ref_const_t q) {
  Eigen::Quaterniond quat(q.data());
  Eigen::AngleAxisd m(quat);
  return m.angle() / M_PI * 180.;
}

void EffectorSplineRotationNoRotationTest(bool& error) {
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 10; i = i + 2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end());
  helpers::config_t q_init;
  q_init << 0., 0., 0., 0., 0., 0., 1.;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  helpers::config_t q_to;
  q_to << 0., 0, 0.02, 0., 0., 0., 1.;
  helpers::config_t q_land;
  q_land << 10, 10, 10.02, 0, 0., 0., 1.;
  helpers::config_t q_mod;
  q_mod << 6., 6., 6., 0., 0., 0., 1.;
  std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(6), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
}

void EffectorSplineRotationRotationTest(bool& error) {
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 10; i = i + 2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  helpers::quat_t init_quat = GetXRotQuat(M_PI);
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(), init_quat);
  helpers::config_t q_init = helpers::config_t::Zero();
  q_init.tail<4>() = init_quat;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  helpers::config_t q_to = q_init;
  q_to(2) += 0.02;
  helpers::config_t q_land = q_end;
  q_land(2) += 0.02;
  helpers::quat_t q_mod = GetXRotQuat(M_PI_2);
  ;
  std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(5).tail<4>(), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
}

void EffectorSplineRotationWayPointRotationTest(bool& error) {
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
  for (double i = 0; i <= 10; i = i + 2) {
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
  }
  helpers::quat_t init_quat = GetXRotQuat(0);
  helpers::t_waypoint_quat_t quat_waypoints_;
  helpers::quat_t q_pi_0 = GetXRotQuat(0);
  helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
  helpers::quat_t q_pi = GetXRotQuat(M_PI);
  quat_waypoints_.push_back(std::make_pair(0.4, q_pi_0));
  quat_waypoints_.push_back(std::make_pair(6, q_pi_2));
  quat_waypoints_.push_back(std::make_pair(8, q_pi));
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(), quat_waypoints_.begin(),
                                             quat_waypoints_.end());
  helpers::config_t q_init = helpers::config_t::Zero();
  q_init.tail<4>() = init_quat;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  q_end.tail<4>() = q_pi;
  helpers::config_t q_mod;
  q_mod.head<3>() = point3_t(6, 6, 6);
  q_mod.tail<4>() = q_pi_2;
  helpers::config_t q_to = q_init;
  q_to(2) += 0.02;
  helpers::config_t q_land = q_end;
  q_land(2) += 0.02;
  std::string errmsg(
      "Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(6), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
}

void TestReparametrization(bool& error) {
  helpers::rotation_spline s;
  const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
  if (!QuasiEqual(sp.min(), 0.0)) {
    std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
    error = true;
  }
  if (!QuasiEqual(sp.max(), 1.0)) {
    std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
    error = true;
  }
  if (!QuasiEqual(sp(1)[0], 1.0)) {
    std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
    error = true;
  }
  if (!QuasiEqual(sp(0)[0], 0.0)) {
    std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
    error = true;
  }
  for (double i = 0; i < 1; i += 0.002) {
    if (sp(i)[0] > sp(i + 0.002)[0]) {
      std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
      error = true;
    }
  }
}

point3_t randomPoint(const double min, const double max) {
  point3_t p;
  for (size_t i = 0; i < 3; ++i) {
    p[i] = (rand() / (double)RAND_MAX) * (max - min) + min;
  }
  return p;
}

void BezierEvalDeCasteljau(bool& error) {
  using namespace std;
  std::vector<double> values;
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
  point3_t a(1, 2, 3);
  point3_t b