added cross product operator for polynomial

include/curves/bezier_curve.h

@@ -618,7 +618,7 @@ bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const bezier_cu
 template <typename T, typename N, bool S, typename P >
 bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1) {
     std::vector<typename bezier_curve<T,N,S,P>::point_t> ts;
-    for (int i = 0; i <= p1.degree(); ++i){
+    for (std::size_t i = 0; i <= p1.degree(); ++i){
       ts.push_back(bezier_curve<T,N,S,P>::point_t::Zero(p1.dim()));
     }
     bezier_curve<T,N,S,P> res (ts.begin(),ts.end(),p1.min(),p1.max());

include/curves/polynomial.h

@@ -439,6 +439,35 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
     return *this;
   }
 
+  ///  \brief Compute the cross product of the current polynomial by another polynomial.
+  /// 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 polynomials.
+  ///  \param pOther other polynomial to compute the cross product with.
+  ///  \return a new polynomial defining the cross product between this and pother
+  polynomial_t cross(const polynomial_t& pOther) const {
+    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);
+    Eigen::Vector3d currentVec;
+    Eigen::Vector3d currentVecCrossed;
+    for(long i = 0; i< coefficients_.cols(); ++i){
+        currentVec = coefficients_.col(i);
+        for(long j = 0; j< pOther.coeff().cols(); ++j){
+            currentVecCrossed = pOther.coeff().col(j);
+            nCoeffs.col(i+j) += currentVec.cross(currentVecCrossed);
+        }
+    }
+    // remove last degrees is they are equal to 0
+    long finalDegree = nDegree;
+    while(nCoeffs.col(finalDegree).norm() <= polynomial_t::MARGIN){
+        --finalDegree;
+    }
+    return polynomial_t(nCoeffs.leftCols(finalDegree+1), min(), max());
+  }
+
   /*Attributes*/
   std::size_t dim_;       // const
   coeff_t coefficients_;  // const
@@ -448,9 +477,9 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
 
  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");
+  void assert_operator_compatible(const polynomial_t& other) const{
+      if ((fabs(min() - other.min()) > polynomial_t::MARGIN) || (fabs(max() - other.max()) > polynomial_t::MARGIN) || dim() != other.dim()){
+          throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges or different dimensions");
       }
   }
 

tests/test-operations.cpp

@@ -129,4 +129,50 @@ 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);
+    polynomial_t::coeff_t coeffsDim4 = Eigen::MatrixXd::Random(4,2);
+    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);
+    polynomial_t pDim4(coeffsDim4,0.,1.);
+
+    BOOST_CHECK_THROW( p1.cross(p3), std::exception );
+    BOOST_CHECK_THROW( p1.cross(p4), std::exception );
+    BOOST_CHECK_THROW( p1.cross(p5), std::exception );
+    BOOST_CHECK_THROW( p1.cross(pDim4), std::exception );
+    BOOST_CHECK_THROW( pDim4.cross(p1), std::exception );
+
+    polynomial_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(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);
+    coeffs2.col(2) =coeffs1.col(4);
+    polynomial_t p1(coeffs1,0.,1.);
+    polynomial_t p2(coeffs2,0.,1.);
+
+    polynomial_t pCross  = p1.cross(p2);
+    BOOST_CHECK_EQUAL (pCross.degree(), 5);
+    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_SUITE_END()