Commit 54c51f33 authored by Steve T's avatar Steve T
Browse files

added member operators for polynomials + non member operator bindings in python for linear variable

parent 3b31f0b3
......@@ -156,6 +156,11 @@ linear_variable<N, S> operator-(const linear_variable<N, S>& w1, const linear_va
return res -= w2;
}
template <typename N, bool S>
linear_variable<N, S> operator-(const linear_variable<N, S>& w1) {
return linear_variable<N, S> (-w1.B(), -w1.c());
}
template <typename N, bool S>
linear_variable<N, S> operator*(const double k, const linear_variable<N, S>& w) {
linear_variable<N, S> res(w.B(), w.c());
......
......@@ -401,6 +401,44 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
virtual std::size_t degree() const { return degree_; }
/*Helpers*/
polynomial_t& operator+=(const polynomial_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() > degree()) {
polynomial_t::coeff_t res = p1.coeff();
res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
coefficients_ = res;
degree_ = p1.degree();
}
else{
coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) += p1.coeff();
}
return *this;
}
polynomial_t& operator-=(const polynomial_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() > degree()) {
polynomial_t::coeff_t res = -p1.coeff();
res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
coefficients_ = res;
degree_ = p1.degree();
}
else{
coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) -= p1.coeff();
}
return *this;
}
polynomial_t& operator/=(const double d) {
coefficients_ /= d;
return *this;
}
polynomial_t& operator*=(const double d) {
coefficients_ *= d;
return *this;
}
/*Attributes*/
std::size_t dim_; // const
coeff_t coefficients_; // const
......@@ -409,6 +447,13 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
/*Attributes*/
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");
}
}
template <typename In>
coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
......@@ -452,11 +497,8 @@ void assert_operator_compatible(const P& p1, const P& p2){
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());
polynomial<T,N,S,P,TP> res(p1);
return res+=p2;
}
template <typename T, typename N, bool S, typename P, typename TP >
......@@ -467,29 +509,27 @@ polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1) {
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());
polynomial<T,N,S,P,TP> res(p1);
return res-=p2;
}
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());
polynomial<T,N,S,P,TP> res(p1);
return res/=k;
}
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());
polynomial<T,N,S,P,TP> res(p1);
return res*=k;
}
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;
polynomial<T,N,S,P,TP> res(p1);
return res*=k;
}
......
......@@ -696,6 +696,11 @@ BOOST_PYTHON_MODULE(curves) {
.def(self -= linear_variable_t())
.def(self *= double())
.def(self /= double())
.def(self + linear_variable_t())
.def(self - linear_variable_t())
.def(self * double())
.def(self / double())
.def(self * linear_variable_t())
.def("B", &linear_variable_t::B, return_value_policy<copy_const_reference>())
.def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
.def("size", &linear_variable_t::size)
......
......@@ -26,10 +26,16 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
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);
double k = 10.2;
BOOST_CHECK_THROW( p1 + p3, std::exception );
BOOST_CHECK_THROW( p1 - p3, std::exception );
BOOST_CHECK_THROW( p1 + p4, std::exception );
BOOST_CHECK_THROW( p1 - p4, std::exception );
BOOST_CHECK_THROW( p1 + p5, std::exception );
BOOST_CHECK_THROW( p1 - p5, std::exception );
polynomial_t pSum = p1 + p2;
polynomial_t pSumR = p2 + p1;
......@@ -50,6 +56,18 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
BOOST_TEST(( pdiv(dt) - p1(dt)/k).norm()==0.);
BOOST_TEST(( pNeg(dt) + p1(dt)).norm() == 0);
}
pSum = polynomial_t(p1); pSum += p2;
pSub = p1; pSub -= p2;
pdiv = p1; pdiv /= k;
pMul = p1; pMul *= k;
for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.;
BOOST_TEST(( pSum(dt) - (p1(dt)+p2(dt))).norm()==0.);
BOOST_TEST(( pSub(dt) - (p1(dt)-p2(dt))).norm()==0.);
BOOST_TEST(( pMul(dt) - p1(dt)*k).norm()==0.);
BOOST_TEST(( pdiv(dt) - p1(dt)/k).norm()==0.);
}
}
......
Markdown is supported
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