curve and point addition cpp

include/curves/bezier_curve.h

@@ -446,6 +446,38 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return c_split.second.split(t2).first;
   }
 
+  ///  \brief Compute the cross product of the current bezier curve by another bezier curve.
+  /// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
+  /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
+  /// This method of course only makes sense for dimension 3 curves.
+  /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
+  ///  \param pOther other polynomial to compute the cross product with.
+  ///  \return a new polynomial defining the cross product between this and pother
+  bezier_curve_t cross(const bezier_curve_t& g) const {
+    //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
+    //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
+    assert_operator_compatible(g);
+    if (dim()!= 3)
+        throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
+    int m =(int)(degree());
+    int n =(int)(g.degree());
+    unsigned int mj, n_ij, mn_i;
+    t_point_t new_waypoints;
+    for(int i = 0; i<= m+n; ++i)
+    {
+        bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim());
+        for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
+            mj = bin(m,j);
+            n_ij = bin(n,i-j);
+            mn_i = bin(m+n,i);
+            num_t mul = num_t(mj*n_ij) / num_t(mn_i);
+            current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j));
+        }
+        new_waypoints.push_back(current_point);
+    }
+    return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_);
+  }
+
   bezier_curve_t& operator+=(const bezier_curve_t& other) {
       assert_operator_compatible(other);
       bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
@@ -478,6 +510,20 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
       return *this;
     }
 
+  bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) {
+    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
+      (*it)+=point;
+    }
+    return *this;
+  }
+
+  bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) {
+    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
+      (*it)-=point;
+    }
+    return *this;
+  }
+
   bezier_curve_t& operator/=(const double d) {
     for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
       (*it)/=d;
@@ -493,38 +539,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   }
 
 
-  ///  \brief Compute the cross product of the current bezier curve by another bezier curve.
-  /// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
-  /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
-  /// This method of course only makes sense for dimension 3 curves.
-  /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
-  ///  \param pOther other polynomial to compute the cross product with.
-  ///  \return a new polynomial defining the cross product between this and pother
-  bezier_curve_t cross(const bezier_curve_t& g) const {
-    //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
-    //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
-    assert_operator_compatible(g);
-    if (dim()!= 3)
-        throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
-    int m =(int)(degree());
-    int n =(int)(g.degree());
-    unsigned int mj, n_ij, mn_i;
-    t_point_t new_waypoints;
-    for(int i = 0; i<= m+n; ++i)
-    {
-        bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim());
-        for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
-            mj = bin(m,j);
-            n_ij = bin(n,i-j);
-            mn_i = bin(m+n,i);
-            num_t mul = num_t(mj*n_ij) / num_t(mn_i);
-            current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j));
-        }
-        new_waypoints.push_back(current_point);
-    }
-    return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_);
-  }
-
  private:
   /// \brief Ensure constraints of bezier curve.
   /// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
@@ -652,6 +666,31 @@ bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_cu
     return res-=p2;
 }
 
+
+template <typename T, typename N, bool S, typename P >
+bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
+  bezier_curve<T,N,S,P> res(p1);
+  return res-=point;
+}
+
+template <typename T, typename N, bool S, typename P >
+bezier_curve<T,N,S,P> operator-(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
+  bezier_curve<T,N,S,P> res(-p1);
+  return res+=point;
+}
+
+template <typename T, typename N, bool S, typename P >
+bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
+  bezier_curve<T,N,S,P> res(p1);
+  return res+=point;
+}
+
+template <typename T, typename N, bool S, typename P >
+bezier_curve<T,N,S,P> operator+(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
+  bezier_curve<T,N,S,P> res(p1);
+  return res+=point;
+}
+
 template <typename T, typename N, bool S, typename P >
 bezier_curve<T,N,S,P> operator/(const bezier_curve<T,N,S,P>& p1, const double k) {
     bezier_curve<T,N,S,P> res(p1);

include/curves/polynomial.h

@@ -428,6 +428,16 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
       return *this;
     }
 
+  polynomial_t& operator+=(const polynomial_t::point_t& point) {
+    coefficients_.col(0) += point;
+    return *this;
+  }
+
+  polynomial_t& operator-=(const polynomial_t::point_t& point) {
+    coefficients_.col(0) -= point;
+    return *this;
+    }
+
   polynomial_t& operator/=(const double d) {
     coefficients_ /= d;
     return *this;
@@ -519,6 +529,31 @@ polynomial<T,N,S,P,TP> operator+(const polynomial<T,N,S,P,TP>& p1, const polynom
   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 typename polynomial<T,N,S,P,TP>::point_t& point) {
+  polynomial<T,N,S,P,TP> res(p1);
+  return res+=point;
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator+(const typename polynomial<T,N,S,P,TP>::point_t& point, const polynomial<T,N,S,P,TP>& p1) {
+  polynomial<T,N,S,P,TP> res(p1);
+  return res+=point;
+}
+
+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 typename polynomial<T,N,S,P,TP>::point_t& point) {
+  polynomial<T,N,S,P,TP> res(p1);
+  return res-=point;
+}
+
+template <typename T, typename N, bool S, typename P, typename TP >
+polynomial<T,N,S,P,TP> operator-(const typename polynomial<T,N,S,P,TP>::point_t& point, const polynomial<T,N,S,P,TP>& p1) {
+  polynomial<T,N,S,P,TP> res(-p1);
+  return res+=point;
+}
+
+
 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();

tests/test-operations.cpp

@@ -116,6 +116,31 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
     }
 }
 
+BOOST_AUTO_TEST_CASE(bezierPointOperations, * boost::unit_test::tolerance(0.001)) {
+
+    t_pointX_t vec1;
+    for (int i =0; i<6; ++i)
+    {
+        vec1.push_back(Eigen::Vector3d::Random());
+    }
+
+    bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
+    Eigen::Vector3d point; point << 1., 1. , 1.;
+    //bezier_t::point_t point = bezier_t::point_t::Random(3);
+
+    bezier_t pSum  = p1 + point;
+    bezier_t pSumR = point + p1;
+    bezier_t pSub  = p1 - point;
+    bezier_t pSubR = point - p1;
+    for (double i = 0.; i <=100.; ++i ){
+        double dt = i / 100.;
+        BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
+        BOOST_TEST((pSumR(dt) - (p1(dt)+point)).norm()==0.);
+        BOOST_TEST(( pSub(dt) - (p1(dt)-point)).norm()==0.);
+        BOOST_TEST((pSubR(dt) - (point-p1(dt))).norm()==0.);
+    }
+}
+
 
 BOOST_AUTO_TEST_CASE(crossPoductLinearVariable, * boost::unit_test::tolerance(0.001)) {
     linear_variable_t l1(Eigen::Matrix3d::Identity() * 5., Eigen::Vector3d::Random());
@@ -168,6 +193,24 @@ BOOST_AUTO_TEST_CASE(crossProductBezierLinearVariable, * boost::unit_test::toler
 }
 
 
+BOOST_AUTO_TEST_CASE(polynomialPointOperations, * boost::unit_test::tolerance(0.001)) {
+    polynomial_t::coeff_t coeffs1 = Eigen::MatrixXd::Random(3,5);
+    polynomial_t::point_t point = polynomial_t::point_t::Random(3);
+    polynomial_t p1(coeffs1,0.,1.);
+
+    polynomial_t pSum  = p1 + point;
+    polynomial_t pSumR = point + p1;
+    polynomial_t pSub  = p1 - point;
+    polynomial_t pSubR = point - p1;
+    for (double i = 0.; i <=100.; ++i ){
+        double dt = i / 100.;
+        BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
+        BOOST_TEST((pSumR(dt) - (p1(dt)+point)).norm()==0.);
+        BOOST_TEST(( pSub(dt) - (p1(dt)-point)).norm()==0.);
+        BOOST_TEST((pSubR(dt) - (point-p1(dt))).norm()==0.);
+    }
+}
+
 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);