Commit 518f2535 authored by Steve T's avatar Steve T
Browse files

refactored += -= operations on bezier

parent 8b6561a2
......@@ -229,20 +229,34 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return integ.compute_primitive(order - 1);
}
/// \brief Computes a Bezier curve of 1 degree higher than the current curve, but strictly equivalent.
/// \brief Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent.
/// Order elevation is required for addition / substraction and other comparison operations.
/// \param order : number of order the curve must be updated
/// \return An equivalent Bezier, with one more degree.
bezier_curve_t elevate() const {
num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
t_point_t n_wp;
n_wp.push_back(*control_points_.begin());
num_t idx_deg_inv = 0.;
for (typename t_point_t::const_iterator pit = control_points_.begin()+1; pit != control_points_.end(); ++pit) {
idx_deg_inv += new_degree_inv;
n_wp.push_back(idx_deg_inv * (*(pit-1)) + (1 - idx_deg_inv) * (*pit));
}
n_wp.push_back(*(control_points_.end()-1));
return bezier_curve_t (n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_);
bezier_curve_t elevate(const std::size_t order) const {
t_point_t new_waypoints = control_points_, temp_waypoints;
for (std::size_t i = 1; i<= order; ++i)
{
num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
temp_waypoints.push_back(*new_waypoints.begin());
num_t idx_deg_inv = 0.;
for (typename t_point_t::const_iterator pit = new_waypoints.begin()+1; pit != new_waypoints.end(); ++pit) {
idx_deg_inv += new_degree_inv;
temp_waypoints.push_back(idx_deg_inv * (*(pit-1)) + (1 - idx_deg_inv) * (*pit));
}
temp_waypoints.push_back(*(new_waypoints.end()-1));
new_waypoints = temp_waypoints;
temp_waypoints.clear();
}
return bezier_curve_t (new_waypoints.begin(), new_waypoints.end(), T_min_, T_max_, mult_T_);
}
/// \brief Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent.
/// Order elevation is required for addition / substraction and other comparison operations.
/// \param order : number of order the curve must be updated
void elevate_self(const std::size_t order) {
if (order > 0)
(*this) = elevate(order);
}
/// \brief Evaluate the derivative order N of curve at time t.
......@@ -431,61 +445,37 @@ 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);
}
bezier_curve_t& operator+=(const bezier_curve_t& other) {
assert_operator_compatible(other);
bezier_curve_t other_elevated = other;
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
}
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 if(other_elevated.degree() < degree()){
other_elevated.elevate_self(degree() - other_elevated.degree());
}
else{
bezier_curve_t res = p1.elevate();
while(res.degree() < degree())
res = res.elevate();
return (*this)+=res;
typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)+=(*otherit);
}
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();
bezier_curve_t& operator-=(const bezier_curve_t& other) {
assert_operator_compatible(other);
bezier_curve_t other_elevated = other;
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
}
else if(other_elevated.degree() < degree()){
other_elevated.elevate_self(degree() - other_elevated.degree());
}
typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)-=(*otherit);
}
return *this;
}
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){
......
......@@ -634,7 +634,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def("waypoints",&wrapBezier3Waypoints)
.def("elevate",&bezier3_t::elevate,"Computes a Bezier curve of 1 degree higher than the current curve, but strictly equivalent.")
.def("elevate",&bezier3_t::elevate, bp::args("order"), "Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent.")
.def("elevateSelf",&bezier3_t::elevate_self, bp::args("order"), "Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent.")
.def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_)
.def("saveAsText", &bezier3_t::saveAsText<bezier3_t>, bp::args("filename"), "Saves *this inside a text file.")
......@@ -670,7 +671,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("compute_primitive", &bezier_t::compute_primitive)
.def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def("waypoints",&wrapBezierWaypoints)
.def("elevate",&bezier_t::elevate, "Computes a Bezier curve of 1 degree higher than the current curve, but strictly equivalent.")
.def("elevate",&bezier_t::elevate, bp::args("order"), "Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent.")
.def("elevateSelf",&bezier_t::elevate_self, bp::args("order"), "Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent.")
.def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", splitspe)
......@@ -818,6 +820,7 @@ BOOST_PYTHON_MODULE(curves) {
"Saves *this inside a binary file.")
.def("loadFromBinary", &polynomial_t::loadFromBinary<polynomial_t>, bp::args("filename"),
"Loads *this from a binary file.")
.def("cross", &polynomial_t::cross,"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.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += polynomial_t())
......
......@@ -63,7 +63,7 @@ class TestCurves(unittest.TestCase):
elif i == 1:
self.assertTrue((a.waypointAtIndex(1) == array([4., 5., 6.])).all())
a1 = a.elevate();
a1 = a.elevate(1);
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
......@@ -174,10 +174,14 @@ class TestCurves(unittest.TestCase):
# time_waypoints = array([[0., 1.]]).transpose()
# Create bezier6 and bezier
a = bezier3(waypoints, 0., 3.)
a1 = a.elevate().elevate();
a1 = a.elevate(1);
b = bezier3(waypoints, 0., 3.);
b.elevateSelf(2)
assert(b.degree == a.degree + 2)
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
self.assertTrue(norm(a(dt) - b(dt)) < __EPS)
#arithmetic
b = a + a1
......
......@@ -917,8 +917,8 @@ void BezierElevate(bool& error) {
params.push_back(c);
// 3d curve
bezier_t cf(params.begin(), params.end());
bezier_t cf2 = cf.elevate();
bezier_t cf3 = cf2.elevate();
bezier_t cf2 = cf.elevate(1);
bezier_t cf3 = cf2.elevate(1);
if (cf2.degree() - cf.degree() != 1 && cf3.degree() - cf.degree() != 2 ){
error = true;
std::string errmsg("Error in BezierElevate; Degree mismatched for elevated curves. Expected 1 / 2, got: ");
......
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