Commit 8b6561a2 by Steve T

### added cross product operator for polynomial

parent e4506a4d
 ... ... @@ -618,7 +618,7 @@ bezier_curve operator+(const bezier_curve& p1, const bezier_cu template bezier_curve operator-(const bezier_curve& p1) { std::vector::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::point_t::Zero(p1.dim())); } bezier_curve res (ts.begin(),ts.end(),p1.min(),p1.max()); ... ...
 ... ... @@ -439,6 +439,35 @@ struct polynomial : public curve_abc { 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 { 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"); } } ... ...
 ... ... @@ -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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!