added cross product for bezier

include/curves/bezier_curve.h

@@ -10,6 +10,7 @@
 #define _CLASS_BEZIERCURVE
 
 #include "curve_abc.h"
+#include "cross_implementation.h"
 #include "bernstein.h"
 #include "curve_constraint.h"
 #include "piecewise_curve.h"
@@ -447,6 +448,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
 
   bezier_curve_t& operator+=(const bezier_curve_t& other) {
       assert_operator_compatible(other);
+      if(fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)
+          throw std::runtime_error("addition not implemented yet for curves of different mult");
       bezier_curve_t other_elevated = other;
       if(other.degree() > degree()){
           elevate_self(other.degree() - degree());
@@ -463,6 +466,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
 
   bezier_curve_t& operator-=(const bezier_curve_t& other)  {
       assert_operator_compatible(other);
+      if(fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)
+          throw std::runtime_error("addition not implemented yet for curves of different mult");
       bezier_curve_t other_elevated = other;
       if(other.degree() > degree()){
           elevate_self(other.degree() - degree());
@@ -491,6 +496,38 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return *this;
   }
 
+
+  ///  \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 polynomials with dimensions != 3 ");
+    int m =(int)(degree());
+    int n =(int)(g.degree());
+    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(3);
+        for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
+            unsigned int mj = bin(m,j);
+            unsigned int n_ij = bin(n,i-j);
+            unsigned int mn_i = bin(m+n,i);
+            num_t mul = num_t(mj*n_ij) / num_t(mn_i);
+            current_point += mul*curves::cross<bezier_curve_t::point_t>(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
@@ -531,9 +568,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   }
 
 
-  void assert_operator_compatible(const bezier_curve_t& other){
-    if ((fabs(min() - other.min()) > bezier_curve_t::MARGIN) || (fabs(max() - other.max()) > bezier_curve_t::MARGIN) || (fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)){
-        throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges or mult");
+  void assert_operator_compatible(const bezier_curve_t& other) const{
+    if ((fabs(min() - other.min()) > bezier_curve_t::MARGIN) || (fabs(max() - other.max()) > bezier_curve_t::MARGIN)){
+        throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges");
     }
   }
 

include/curves/cross_implementation.h

+/**
+ * \file cubic_hermite_spline.h
+ * \brief class allowing to create a cubic hermite spline of any dimension.
+ * \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
+ * \date 05/2019
+ */
+
+#ifndef _CLASS_CROSSIMP
+#define _CLASS_CROSSIMP
+
+#include "curves/fwd.h"
+
+namespace curves {
+    template<typename Point>
+Eigen::Vector3d cross(const Point& a, const Point& b){
+    Eigen::Vector3d c;
+    c << a[1]*b[2] - a[2]*b[1], a[2]*b[0] - a[0]*b[2], a[0]*b[1] - a[1]*b[0] ;
+    return c;
+}
+}  // namespace curves
+#endif  //_CLASS_CROSSIMP

include/curves/polynomial.h

@@ -449,8 +449,8 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
     assert_operator_compatible(pOther);
     if (dim()!= 3)
         throw std::invalid_argument("Can't perform cross product polynomials with dimensions != 3 ");
-    std::size_t nDegree =degree() + pOther.degree();
-    coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,nDegree+1);
+    std::size_t new_degree =degree() + pOther.degree();
+    coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+1);
     Eigen::Vector3d currentVec;
     Eigen::Vector3d currentVecCrossed;
     for(long i = 0; i< coefficients_.cols(); ++i){
@@ -461,11 +461,11 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
         }
     }
     // remove last degrees is they are equal to 0
-    long finalDegree = nDegree;
-    while(nCoeffs.col(finalDegree).norm() <= polynomial_t::MARGIN){
-        --finalDegree;
+    long final_degree = new_degree;
+    while(nCoeffs.col(final_degree).norm() <= polynomial_t::MARGIN){
+        --final_degree;
     }
-    return polynomial_t(nCoeffs.leftCols(finalDegree+1), min(), max());
+    return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
   }
 
   /*Attributes*/

python/curves/curves_python.cpp

@@ -651,6 +651,7 @@ BOOST_PYTHON_MODULE(curves) {
       //.def(SerializableVisitor<bezier_t>())
       .def(bp::self == bp::self)
       .def(bp::self != bp::self)
+      .def("cross", &bezier3_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials 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.")
       .def(self += bezier3_t())
       .def(self -= bezier3_t())
       .def(self *= double())
@@ -688,6 +689,7 @@ BOOST_PYTHON_MODULE(curves) {
            "Loads *this from a binary file.")
       .def(bp::self == bp::self)
       .def(bp::self != bp::self)
+      .def("cross", &bezier_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials 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.")
       .def(self += bezier_t())
       .def(self -= bezier_t())
       .def(self *= double())

python/test/test.py

@@ -77,7 +77,9 @@ class TestCurves(unittest.TestCase):
         a1*=0.1
         a1/=0.1
         b = -a1
-                
+        c = a.cross(b)
+        c(0)
+        
         # self.assertTrue((a.waypoints == waypoints).all())
         # Test : Degree, min, max, derivate
         # self.print_str(("test 1")
@@ -191,6 +193,8 @@ class TestCurves(unittest.TestCase):
         a1*=0.1
         a1/=0.1
         b = -a1
+        c = a.cross(b)
+        c(0)
         
         # Test waypoints
         self.assertTrue(a.nbWaypoints == 2)

tests/test-operations.cpp

@@ -20,6 +20,34 @@ using namespace curves;
 
 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
+
+
+BOOST_AUTO_TEST_CASE(crossPoductBezier, * boost::unit_test::tolerance(0.001)) {
+
+
+    t_pointX_t vec1;
+    t_pointX_t vec2;
+    for (int i =0; i<4; ++i)
+    {
+        vec1.push_back(Eigen::Vector3d::Random());
+        vec2.push_back(Eigen::Vector3d::Random());
+    }
+    for (int i =0; i<3; ++i)
+    {
+        vec1.push_back(Eigen::Vector3d::Random());
+    }
+    bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
+    bezier_t p2(vec2.begin(),vec2.end(),0.,1.);
+
+    bezier_t pCross  (p1.cross(p2));
+    for (double i = 0.; i <=100.; ++i ){
+        double dt = i / 100.;
+        Eigen::Vector3d v1 = p1(dt);
+        Eigen::Vector3d v2 = p2(dt);
+        BOOST_TEST(( pCross(dt) - v1.cross(v2)).norm()==0.);
+    }
+}
+
 BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
     t_pointX_t vec1;
     t_pointX_t vec2;
@@ -79,6 +107,9 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
     }
 }
 
+
+
+
 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);
@@ -129,8 +160,6 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
     }
 }
 
-
-
 BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * 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);
@@ -157,7 +186,6 @@ BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * boost::unit_test::tolerance(0.001
     }
 }
 
-
 BOOST_AUTO_TEST_CASE(crossPoductPolynomialSimplification, * 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,3);