added + - * / operations for polynomial curves

include/curves/cubic_hermite_spline.h

@@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
  public:
   /// \brief Get dimension of curve.
   /// \return dimension of curve.
-  std::size_t virtual dim() const { return dim_; };
+  std::size_t virtual dim() const { return dim_; }
   /// \brief Get the minimum time for which the curve is defined
   /// \return \f$t_{min}\f$, lower bound of time range.
   Time virtual min() const { return time_control_points_.front(); }

include/curves/polynomial.h

@@ -42,6 +42,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   typedef Eigen::Ref<coeff_t> coeff_t_ref;
   typedef polynomial<Time, Numeric, Safe, Point, T_Point> polynomial_t;
   typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
+  static const double MARGIN;
 
   /* Constructors - destructors */
  public:
@@ -439,6 +440,59 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
 
 };  // class polynomial
 
+template <typename T, typename N, bool S, typename P, typename TP >
+const double polynomial<T,N,S,P,TP>::MARGIN(0.001);
+
+template <typename P>
+void assert_operator_compatible(const P& p1, const P& p2){
+    if ((fabs(p1.min() - p2.min()) > P::MARGIN) || (fabs(p1.max() - p2.max()) > P::MARGIN) ){
+        throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges");
+    }
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator+(const polynomial<T,N,S,P,TP>& p1, const polynomial<T,N,S,P,TP>& p2) {
+  assert_operator_compatible<polynomial<T,N,S,P,TP> >(p1,p2);
+  typename polynomial<T,N,S,P,TP>::coeff_t res = Eigen::MatrixXd::Zero(p1.dim(),std::max(p1.degree(),p2.degree())+1);
+  res.block(0,0,p1.coefficients_.rows(),p1.coefficients_.cols())  = p1.coeff();
+  res.block(0,0,p2.coefficients_.rows(),p2.coefficients_.cols()) += p2.coeff();
+  return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1) {
+    typename polynomial<T,N,S,P,TP>::coeff_t res = -p1.coeff();
+    return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1, const polynomial<T,N,S,P,TP>& p2) {
+    assert_operator_compatible<polynomial<T,N,S,P,TP> >(p1,p2);
+    typename polynomial<T,N,S,P,TP>::coeff_t res = Eigen::MatrixXd::Zero(p1.dim(),std::max(p1.degree(),p2.degree())+1);
+    res.block(0,0,p1.coefficients_.rows(),p1.coefficients_.cols())  = p1.coeff();
+    res.block(0,0,p2.coefficients_.rows(),p2.coefficients_.cols()) -= p2.coeff();
+    return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
+}
+
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator/(const polynomial<T,N,S,P,TP>& p1, const double k) {
+    typename polynomial<T,N,S,P,TP>::coeff_t res = p1.coeff() / k;
+    return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator*(const polynomial<T,N,S,P,TP>& p1,const double k)  {
+    typename polynomial<T,N,S,P,TP>::coeff_t res = p1.coeff() * k;
+    return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator*(const double k, const polynomial<T,N,S,P,TP>& p1)  {
+    return p1*k;
+}
+
+
 }  // namespace curves
 
 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, typename T_Point),

tests/CMakeLists.txt

@@ -7,6 +7,7 @@ SET(${PROJECT_NAME}_TESTS
   test-sinusoidal
   test-sinusoidal-serialization
   test-minjerk
+  test-operations
   )
 
 FOREACH(TEST ${${PROJECT_NAME}_TESTS})

tests/test-operations.cpp

+#define BOOST_TEST_MODULE test_operations
+
+#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 "curves/serialization/curves.hpp"
+#include <boost/test/included/unit_test.hpp>
+
+using namespace curves;
+
+BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
+
+BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001)) {
+    polynomial_t::coeff_t coeffs1 = Eigen::MatrixXd::Random(3,5);
+    polynomial_t::coeff_t coeffs2 = Eigen::MatrixXd::Random(3,2);
+    polynomial_t p1(coeffs1,0.,1.);
+    polynomial_t p2(coeffs2,0.,1.);
+    polynomial_t p3(coeffs2,0.,0.5);
+    double k = 10.2;
+
+    BOOST_CHECK_THROW( p1 + p3, std::exception );
+    BOOST_CHECK_THROW( p1 - p3, std::exception );
+
+    polynomial_t pSum  = p1 + p2;
+    polynomial_t pSumR = p2 + p1;
+    polynomial_t pSub  = p1 - p2;
+    polynomial_t pSubR = p2 - p1;
+    polynomial_t pdiv  = p1 / k;
+    polynomial_t pMul  = p1 * k;
+    polynomial_t pMulR = k  * p1;
+    polynomial_t pNeg  = -p1;
+    for (double i = 0.; i <=100.; ++i ){
+        double dt = i / 100.;
+        BOOST_TEST(( pSum(dt) - (p1(dt)+p2(dt))).norm()==0.);
+        BOOST_TEST((pSumR(dt) - (p1(dt)+p2(dt))).norm()==0.);
+        BOOST_TEST(( pSub(dt) - (p1(dt)-p2(dt))).norm()==0.);
+        BOOST_TEST((pSubR(dt) - (p2(dt)-p1(dt))).norm()==0.);
+        BOOST_TEST(( pMul(dt) -  p1(dt)*k).norm()==0.);
+        BOOST_TEST((pMulR(dt) -  p1(dt)*k).norm()==0.);
+        BOOST_TEST(( pdiv(dt) -  p1(dt)/k).norm()==0.);
+        BOOST_TEST(( pNeg(dt) +  p1(dt)).norm() == 0);
+    }
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()