Commit e4506a4d authored by Steve T's avatar Steve T
Browse files

added operations for bezier curves + python tests

parent fab62c18
......@@ -431,6 +431,76 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return c_split.second.split(t2).first;
}
bezier_curve_t& operator+=(const bezier_curve_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() == degree()) {
typename t_point_t::const_iterator otherit = p1.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)+=(*otherit);
}
}
else if (p1.degree() > degree()){
bezier_curve_t res = elevate();
while(res.degree() < p1.degree()){
res = res.elevate();
}
res+=p1;
size_ = res.size_;
degree_ = res.degree();
bernstein_ = res.bernstein_;
control_points_ = res.control_points_;
}
else{
bezier_curve_t res = p1.elevate();
while(res.degree() < degree())
res = res.elevate();
return (*this)+=res;
}
return *this;
}
bezier_curve_t& operator-=(const bezier_curve_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() == degree()) {
typename t_point_t::const_iterator otherit = p1.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)-=(*otherit);
}
}
else if (p1.degree() > degree()){
bezier_curve_t res = elevate();
while(res.degree() < p1.degree()){
res = res.elevate();
}
res-=p1;
size_ = res.size_;
degree_ = res.degree();
bernstein_ = res.bernstein_;
control_points_ = res.control_points_;
}
else{
bezier_curve_t res = p1.elevate();
while(res.degree() < degree())
res = res.elevate();
return (*this)-=res;
}
return *this;
}
bezier_curve_t& operator/=(const double d) {
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
(*it)/=d;
}
return *this;
}
bezier_curve_t& operator*=(const double d) {
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
(*it)*=d;
}
return *this;
}
private:
/// \brief Ensure constraints of bezier curve.
/// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
......@@ -469,6 +539,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
"Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
}
}
void assert_operator_compatible(const bezier_curve_t& other){
if ((fabs(min() - other.min()) > bezier_curve_t::MARGIN) || (fabs(max() - other.max()) > bezier_curve_t::MARGIN) || (fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)){
throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges or mult");
}
}
/*Operations*/
public:
......@@ -531,6 +609,47 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
template <typename Time, typename Numeric, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Safe, Point>::MARGIN(0.001);
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
bezier_curve<T,N,S,P> res(p1);
return res+=p2;
}
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){
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());
res-=p1;
return res;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
bezier_curve<T,N,S,P> res(p1);
return res-=p2;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator/(const bezier_curve<T,N,S,P>& p1, const double k) {
bezier_curve<T,N,S,P> res(p1);
return res/=k;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const bezier_curve<T,N,S,P>& p1,const double k) {
bezier_curve<T,N,S,P> res(p1);
return res*=k;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const double k, const bezier_curve<T,N,S,P>& p1) {
bezier_curve<T,N,S,P> res(p1);
return res*=k;
}
} // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
......
......@@ -488,13 +488,6 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
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) {
polynomial<T,N,S,P,TP> res(p1);
......@@ -513,7 +506,6 @@ polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1, const polynom
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) {
polynomial<T,N,S,P,TP> res(p1);
......@@ -532,7 +524,6 @@ polynomial<T,N,S,P,TP> operator*(const double k, const polynomial<T,N,S,P,TP>& p
return res*=k;
}
} // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, typename T_Point),
......
......@@ -650,6 +650,15 @@ BOOST_PYTHON_MODULE(curves) {
//.def(SerializableVisitor<bezier_t>())
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += bezier3_t())
.def(self -= bezier3_t())
.def(self *= double())
.def(self /= double())
.def(self + bezier3_t())
.def(self - bezier3_t())
.def(-self)
.def(self * double())
.def(self / double())
.def_pickle(curve_pickle_suite<bezier3_t>());
/** END bezier3 curve**/
/** BEGIN bezier curve**/
......@@ -677,6 +686,15 @@ BOOST_PYTHON_MODULE(curves) {
"Loads *this from a binary file.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += bezier_t())
.def(self -= bezier_t())
.def(self *= double())
.def(self /= double())
.def(self + bezier_t())
.def(self - bezier_t())
.def(-self)
.def(self * double())
.def(self / double())
//.def(SerializableVisitor<bezier_t>())
.def_pickle(curve_pickle_suite<bezier_t>())
;
......@@ -700,6 +718,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(self /= double())
.def(self + linear_variable_t())
.def(self - linear_variable_t())
.def(-self)
.def(self * double())
.def(self / double())
.def(self * linear_variable_t())
......@@ -728,6 +747,15 @@ BOOST_PYTHON_MODULE(curves) {
.def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += bezier_linear_variable_t())
.def(self -= bezier_linear_variable_t())
.def(self *= double())
.def(self /= double())
.def(self + bezier_linear_variable_t())
.def(self - bezier_linear_variable_t())
.def(-self)
.def(self * double())
.def(self / double())
.def_pickle(curve_pickle_suite<bezier_linear_variable_t>());
class_<quadratic_variable_t>("cost", no_init)
......@@ -798,6 +826,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(self /= double())
.def(self + polynomial_t())
.def(self - polynomial_t())
.def(-self)
.def(self * double())
.def(self / double())
.def_pickle(curve_pickle_suite<polynomial_t>());
......
......@@ -67,12 +67,16 @@ class TestCurves(unittest.TestCase):
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
a = bezier3(waypoints, 0., 3.)
a1 = a.elevate();
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
#arithmetic
b = a + a1
b += a1
b = a - a1
b -= a
a1*=0.1
a1/=0.1
b = -a1
# self.assertTrue((a.waypoints == waypoints).all())
# Test : Degree, min, max, derivate
......@@ -170,6 +174,20 @@ class TestCurves(unittest.TestCase):
# time_waypoints = array([[0., 1.]]).transpose()
# Create bezier6 and bezier
a = bezier3(waypoints, 0., 3.)
a1 = a.elevate().elevate();
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
#arithmetic
b = a + a1
b += a1
b = a - a1
b -= a
a1*=0.1
a1/=0.1
b = -a1
# Test waypoints
self.assertTrue(a.nbWaypoints == 2)
for i in range(0, a.nbWaypoints):
......@@ -266,6 +284,17 @@ class TestCurves(unittest.TestCase):
a.min()
a.max()
a(0.4)
waypoints2 = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a1 = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
b = a + a1
b += a1
b = a - a1
b -= a
a1*=0.1
a1/=0.1
b = -a1
# Test get coefficient at degree
self.assertTrue((a.coeff() == waypoints).all())
self.assertTrue((a.coeffAtDegree(0) == array([1., 2., 3.])).all())
......
......@@ -20,6 +20,65 @@ using namespace curves;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
t_pointX_t vec1;
t_pointX_t vec2;
for (int i =0; i<3; ++i)
{
vec1.push_back(Eigen::Vector3d::Random());
vec2.push_back(Eigen::Vector3d::Random());
}
for (int i =0; i<2; ++i)
{
vec1.push_back(Eigen::Vector3d::Random());
}
bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
bezier_t p2(vec2.begin(),vec2.end(),0.,1.);
bezier_t p3(vec2.begin(),vec2.end(),0.,0.5);
bezier_t p4(vec2.begin(),vec2.end(),0.1,1.);
bezier_t p5(vec2.begin(),vec2.end(),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 );
bezier_t pSum = p1 + p2;
bezier_t pSumR = p2 + p1;
bezier_t pSub = p1 - p2;
bezier_t pSubR = p2 - p1;
bezier_t pdiv = p1 / k;
bezier_t pMul = p1 * k;
bezier_t pMulR = k * p1;
bezier_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);
}
pSum = bezier_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.);
}
}
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);
......@@ -70,5 +129,4 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
}
}
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!
Please register or to comment