Commit 3b31f0b3 authored by Steve T's avatar Steve T
Browse files

added + - * / operations for polynomial curves

parent 90b35dbe
...@@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
public: public:
/// \brief Get dimension of curve. /// \brief Get dimension of curve.
/// \return 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 /// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range. /// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const { return time_control_points_.front(); } Time virtual min() const { return time_control_points_.front(); }
......
...@@ -42,6 +42,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -42,6 +42,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
typedef Eigen::Ref<coeff_t> coeff_t_ref; typedef Eigen::Ref<coeff_t> coeff_t_ref;
typedef polynomial<Time, Numeric, Safe, Point, T_Point> polynomial_t; typedef polynomial<Time, Numeric, Safe, Point, T_Point> polynomial_t;
typedef typename curve_abc_t::curve_ptr_t curve_ptr_t; typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
static const double MARGIN;
/* Constructors - destructors */ /* Constructors - destructors */
public: public:
...@@ -439,6 +440,59 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -439,6 +440,59 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
}; // class polynomial }; // class polynomial
template <typename T, typename N, bool S, typename P, typename TP >
const double polynomial<T,N,S,P,TP>::MARGIN(0.001);
template <typename P>
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 <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 polynomial<T,N,S,P,TP>& p2) {
assert_operator_compatible<polynomial<T,N,S,P,TP> >(p1,p2);
typename polynomial<T,N,S,P,TP>::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<T,N,S,P,TP>(res,p1.min(),p1.max());
}
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();
return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
}
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 polynomial<T,N,S,P,TP>& p2) {
assert_operator_compatible<polynomial<T,N,S,P,TP> >(p1,p2);
typename polynomial<T,N,S,P,TP>::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<T,N,S,P,TP>(res,p1.min(),p1.max());
}
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 double k) {
typename polynomial<T,N,S,P,TP>::coeff_t res = p1.coeff() / k;
return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
}
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 double k) {
typename polynomial<T,N,S,P,TP>::coeff_t res = p1.coeff() * k;
return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator*(const double k, const polynomial<T,N,S,P,TP>& p1) {
return p1*k;
}
} // namespace curves } // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, typename T_Point), 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 ...@@ -7,6 +7,7 @@ SET(${PROJECT_NAME}_TESTS
test-sinusoidal test-sinusoidal
test-sinusoidal-serialization test-sinusoidal-serialization
test-minjerk test-minjerk
test-operations
) )
FOREACH(TEST ${${PROJECT_NAME}_TESTS}) 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 <boost/test/included/unit_test.hpp>
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!
Please register or to comment