added member operators for polynomials + non member operator bindings in python for linear variable

include/curves/linear_variable.h

@@ -156,6 +156,11 @@ linear_variable<N, S> operator-(const linear_variable<N, S>& w1, const linear_va
   return res -= w2;
 }
 
+template <typename N, bool S>
+linear_variable<N, S> operator-(const linear_variable<N, S>& w1) {
+  return linear_variable<N, S> (-w1.B(), -w1.c());
+}
+
 template <typename N, bool S>
 linear_variable<N, S> operator*(const double k, const linear_variable<N, S>& w) {
   linear_variable<N, S> res(w.B(), w.c());

include/curves/polynomial.h

@@ -401,6 +401,44 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   virtual std::size_t degree() const { return degree_; }
   /*Helpers*/
 
+  polynomial_t& operator+=(const polynomial_t& p1) {
+    assert_operator_compatible(p1);
+    if (p1.degree() > degree())  {
+      polynomial_t::coeff_t res = p1.coeff();
+      res.block(0,0,coefficients_.rows(),coefficients_.cols())  += coefficients_;
+      coefficients_ = res;
+      degree_ = p1.degree();
+    }
+    else{
+       coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) += p1.coeff();
+    }
+    return *this;
+  }
+
+  polynomial_t& operator-=(const polynomial_t& p1) {
+      assert_operator_compatible(p1);
+      if (p1.degree() > degree())  {
+        polynomial_t::coeff_t res = -p1.coeff();
+        res.block(0,0,coefficients_.rows(),coefficients_.cols())  += coefficients_;
+        coefficients_ = res;
+        degree_ = p1.degree();
+      }
+      else{
+         coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) -= p1.coeff();
+      }
+      return *this;
+    }
+
+  polynomial_t& operator/=(const double d) {
+    coefficients_ /= d;
+    return *this;
+  }
+
+  polynomial_t& operator*=(const double d) {
+    coefficients_ *= d;
+    return *this;
+  }
+
   /*Attributes*/
   std::size_t dim_;       // const
   coeff_t coefficients_;  // const
@@ -409,6 +447,13 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
                           /*Attributes*/
 
  private:
+
+  void assert_operator_compatible(const polynomial_t& other){
+      if ((fabs(min() - other.min()) > polynomial_t::MARGIN) || (fabs(max() - other.max()) > polynomial_t::MARGIN) ){
+          throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges");
+      }
+  }
+
   template <typename In>
   coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
     std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
@@ -452,11 +497,8 @@ void assert_operator_compatible(const P& p1, const P& p2){
 
 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());
+  polynomial<T,N,S,P,TP> res(p1);
+  return res+=p2;
 }
 
 template <typename T, typename N, bool S, typename P, typename TP >
@@ -467,29 +509,27 @@ polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1) {
 
 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());
+    polynomial<T,N,S,P,TP> res(p1);
+    return res-=p2;
 }
 
 
 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());
+    polynomial<T,N,S,P,TP> res(p1);
+    return res/=k;
 }
 
 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());
+    polynomial<T,N,S,P,TP> res(p1);
+    return res*=k;
 }
 
 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;
+    polynomial<T,N,S,P,TP> res(p1);
+    return res*=k;
 }
 
 

python/curves/curves_python.cpp

@@ -696,6 +696,11 @@ BOOST_PYTHON_MODULE(curves) {
       .def(self -= linear_variable_t())
       .def(self *= double())
       .def(self /= double())
+      .def(self +  linear_variable_t())
+      .def(self -  linear_variable_t())
+      .def(self * double())
+      .def(self / double())
+      .def(self *  linear_variable_t())
       .def("B", &linear_variable_t::B, return_value_policy<copy_const_reference>())
       .def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
       .def("size", &linear_variable_t::size)

tests/test-operations.cpp

@@ -26,10 +26,16 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
     polynomial_t p1(coeffs1,0.,1.);
     polynomial_t p2(coeffs2,0.,1.);
     polynomial_t p3(coeffs2,0.,0.5);
+    polynomial_t p4(coeffs2,0.1,1.);
+    polynomial_t p5(coeffs2,0.1,.5);
     double k = 10.2;
 
     BOOST_CHECK_THROW( p1 + p3, std::exception );
     BOOST_CHECK_THROW( p1 - p3, std::exception );
+    BOOST_CHECK_THROW( p1 + p4, std::exception );
+    BOOST_CHECK_THROW( p1 - p4, std::exception );
+    BOOST_CHECK_THROW( p1 + p5, std::exception );
+    BOOST_CHECK_THROW( p1 - p5, std::exception );
 
     polynomial_t pSum  = p1 + p2;
     polynomial_t pSumR = p2 + p1;
@@ -50,6 +56,18 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
         BOOST_TEST(( pdiv(dt) -  p1(dt)/k).norm()==0.);
         BOOST_TEST(( pNeg(dt) +  p1(dt)).norm() == 0);
     }
+
+    pSum = polynomial_t(p1); pSum += p2;
+    pSub = p1; pSub -= p2;
+    pdiv = p1; pdiv /= k;
+    pMul = p1; pMul *= k;
+    for (double i = 0.; i <=100.; ++i ){
+        double dt = i / 100.;
+        BOOST_TEST(( pSum(dt) - (p1(dt)+p2(dt))).norm()==0.);
+        BOOST_TEST(( pSub(dt) - (p1(dt)-p2(dt))).norm()==0.);
+        BOOST_TEST(( pMul(dt) -  p1(dt)*k).norm()==0.);
+        BOOST_TEST(( pdiv(dt) -  p1(dt)/k).norm()==0.);
+    }
 }