integral_cost.h

Go to the documentation of this file.

 
#ifndef _CLASS_QUADRATIC_COST
#define _CLASS_QUADRATIC_COST
 
#include <Eigen/Core>
 
#include "ndcurves/optimization/definitions.h"
#include "ndcurves/optimization/details.h"
 
namespace ndcurves {
namespace optimization {
 
enum integral_cost_flag {
  DISTANCE = 0x000,
  VELOCITY = 0x001,
  ACCELERATION = 0x002,
  JERK = 0x003,
  FOURTH = 0x004,
  FIFTH = 0x005
};
 
template <typename Point, typename Numeric>
quadratic_variable<Numeric> compute_integral_cost_internal(
    const problem_data<Point, Numeric>& pData, const std::size_t num_derivate) {
  typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> >
      bezier_t;
  typedef typename bezier_t::t_point_t t_point_t;
  typedef typename t_point_t::const_iterator cit_point_t;
  bezier_t acc = pData.bezier->compute_derivate(num_derivate);
  const t_point_t& wps = acc.waypoints();
  quadratic_variable<Numeric> res(bezier_product<Point, Numeric, cit_point_t>(
      wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
  return res;
}
 
template <typename Point, typename Numeric>
quadratic_variable<Numeric> compute_integral_cost(
    const problem_data<Point, Numeric>& pData, const integral_cost_flag flag) {
  std::size_t size = (std::size_t)(flag);
  return compute_integral_cost_internal<Point, Numeric>(pData, size);
}
 
}  // namespace optimization
}  // namespace ndcurves
#endif  //_CLASS_QUADRATIC_COST