Commit 3b31f0b3 by Steve T

### added + - * / operations for polynomial curves

parent 90b35dbe
 ... ... @@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc { public: /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const { return dim_; }; std::size_t virtual dim() const { return dim_; } /// \brief Get the minimum time for which the curve is defined /// \return \f\$t_{min}\f\$, lower bound of time range. Time virtual min() const { return time_control_points_.front(); } ... ...
 ... ... @@ -42,6 +42,7 @@ struct polynomial : public curve_abc { typedef Eigen::Ref coeff_t_ref; typedef polynomial polynomial_t; typedef typename curve_abc_t::curve_ptr_t curve_ptr_t; static const double MARGIN; /* Constructors - destructors */ public: ... ... @@ -439,6 +440,59 @@ struct polynomial : public curve_abc { }; // class polynomial template const double polynomial::MARGIN(0.001); template void assert_operator_compatible(const P& p1, const P& p2){ if ((fabs(p1.min() - p2.min()) > P::MARGIN) || (fabs(p1.max() - p2.max()) > P::MARGIN) ){ throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges"); } } template polynomial operator+(const polynomial& p1, const polynomial& p2) { assert_operator_compatible >(p1,p2); typename polynomial::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(res,p1.min(),p1.max()); } template polynomial operator-(const polynomial& p1) { typename polynomial::coeff_t res = -p1.coeff(); return polynomial(res,p1.min(),p1.max()); } template polynomial operator-(const polynomial& p1, const polynomial& p2) { assert_operator_compatible >(p1,p2); typename polynomial::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(res,p1.min(),p1.max()); } template polynomial operator/(const polynomial& p1, const double k) { typename polynomial::coeff_t res = p1.coeff() / k; return polynomial(res,p1.min(),p1.max()); } template polynomial operator*(const polynomial& p1,const double k) { typename polynomial::coeff_t res = p1.coeff() * k; return polynomial(res,p1.min(),p1.max()); } template polynomial operator*(const double k, const polynomial& p1) { return p1*k; } } // namespace curves DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, typename T_Point), ... ...
 ... ... @@ -7,6 +7,7 @@ SET(\${PROJECT_NAME}_TESTS test-sinusoidal test-sinusoidal-serialization test-minjerk test-operations ) FOREACH(TEST \${\${PROJECT_NAME}_TESTS}) ... ...
 #define BOOST_TEST_MODULE test_operations #include "curves/fwd.h" #include "curves/exact_cubic.h" #include "curves/bezier_curve.h" #include "curves/polynomial.h" #include "curves/helpers/effector_spline.h" #include "curves/helpers/effector_spline_rotation.h" #include "curves/curve_conversion.h" #include "curves/cubic_hermite_spline.h" #include "curves/piecewise_curve.h" #include "curves/optimization/definitions.h" #include "load_problem.h" #include "curves/so3_linear.h" #include "curves/se3_curve.h" #include "curves/serialization/curves.hpp" #include using namespace curves; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 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); polynomial_t p1(coeffs1,0.,1.); polynomial_t p2(coeffs2,0.,1.); polynomial_t p3(coeffs2,0.,0.5); double k = 10.2; BOOST_CHECK_THROW( p1 + p3, std::exception ); BOOST_CHECK_THROW( p1 - p3, std::exception ); polynomial_t pSum = p1 + p2; polynomial_t pSumR = p2 + p1; polynomial_t pSub = p1 - p2; polynomial_t pSubR = p2 - p1; polynomial_t pdiv = p1 / k; polynomial_t pMul = p1 * k; polynomial_t pMulR = k * p1; polynomial_t pNeg = -p1; for (double i = 0.; i <=100.; ++i ){ double dt = i / 100.; BOOST_TEST(( pSum(dt) - (p1(dt)+p2(dt))).norm()==0.); BOOST_TEST((pSumR(dt) - (p1(dt)+p2(dt))).norm()==0.); BOOST_TEST(( pSub(dt) - (p1(dt)-p2(dt))).norm()==0.); BOOST_TEST((pSubR(dt) - (p2(dt)-p1(dt))).norm()==0.); BOOST_TEST(( pMul(dt) - p1(dt)*k).norm()==0.); BOOST_TEST((pMulR(dt) - p1(dt)*k).norm()==0.); BOOST_TEST(( pdiv(dt) - p1(dt)/k).norm()==0.); BOOST_TEST(( pNeg(dt) + p1(dt)).norm() == 0); } } BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!