Commit 1155c0f4 authored by Steve T's avatar Steve T
Browse files

added degree elevation method in bezier curves

parent 04e9bc67
......@@ -215,7 +215,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
if (order == 0) {
return *this;
}
num_t new_degree = (num_t)(degree_ + 1);
num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
t_point_t n_wp;
point_t current_sum = point_t::Zero(dim_);
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
......@@ -223,12 +223,28 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
n_wp.push_back(current_sum);
for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) {
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
n_wp.push_back(current_sum * new_degree_inv);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
return integ.compute_primitive(order - 1);
}
/// \brief Computes a Bezier curve of 1 degree higher than the current curve, but strictly equivalent.
/// Order elevation is required for addition / substraction and other comparison operations.
/// \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_);
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
......
......@@ -634,6 +634,7 @@ 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_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.")
......@@ -660,6 +661,7 @@ 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_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", splitspe)
......
......@@ -62,6 +62,12 @@ class TestCurves(unittest.TestCase):
self.assertTrue((a.waypointAtIndex(0) == array([1., 2., 3.])).all())
elif i == 1:
self.assertTrue((a.waypointAtIndex(1) == array([4., 5., 6.])).all())
a1 = a.elevate();
for i in range(100):
dt = float(i) / 100. * 3.
self.assertTrue(norm(a(dt) - a1(dt)) < __EPS)
# self.assertTrue((a.waypoints == waypoints).all())
# Test : Degree, min, max, derivate
# self.print_str(("test 1")
......
......@@ -247,7 +247,7 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
using namespace std;
std::vector<double> values;
for (int i = 0; i < 100000; ++i) {
values.push_back(rand() / RAND_MAX);
values.push_back(rand() / (double)RAND_MAX);
}
// first compare regular evaluation (low dim pol)
point3_t a(1, 2, 3);
......@@ -900,6 +900,37 @@ void BezierEvalDeCasteljau(bool& error) {
}
}
void BezierElevate(bool& error) {
using namespace std;
std::vector<double> values;
for (int i = 0; i < 10; ++i) {
values.push_back(double(rand()) / double(RAND_MAX));
}
// first compare regular evaluation (low dim pol)
point3_t a(1, 2, 3);
point3_t b(2, 3, 4);
point3_t c(3, 4, 5);
std::vector<point3_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
// 3d curve
bezier_t cf(params.begin(), params.end());
bezier_t cf2 = cf.elevate();
bezier_t cf3 = cf2.elevate();
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: ");
std::cout << errmsg << cf2.degree() - cf.degree() << " ; " << cf3.degree() - cf.degree() << std::endl;
}
std::string errmsg("Error in BezierElevate; Elevated curves do not have the same evaluation : ");
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end()-1; ++cit) {
ComparePoints(cf2(*cit), cf(*(cit)), errmsg, error);
ComparePoints(cf3(*cit), cf(*cit), errmsg, error);
}
}
/**
* @brief BezierSplitCurve test the 'split' method of bezier curve
* @param error
......@@ -2855,7 +2886,7 @@ void testOperatorEqual(bool& error) {
int main(int /*argc*/, char** /*argv[]*/) {
std::cout << "performing tests... \n";
bool error = false;
PolynomialCubicFunctionTest(error);
/*PolynomialCubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
ExactCubicTwoPointsTest(error);
......@@ -2872,8 +2903,9 @@ int main(int /*argc*/, char** /*argv[]*/) {
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
CubicHermitePairsPositionDerivativeTest(error);
BezierSplitCurve(error);*/
BezierElevate(error);
/*CubicHermitePairsPositionDerivativeTest(error);
piecewiseCurveTest(error);
PiecewisePolynomialCurveFromDiscretePoints(error);
PiecewisePolynomialCurveFromFile(error);
......@@ -2891,7 +2923,7 @@ int main(int /*argc*/, char** /*argv[]*/) {
BezierLinearProblemsetup_control_pointsVarCombinatorialEnd(error);
BezierLinearProblemsetup_control_pointsVarCombinatorialMix(error);
BezierLinearProblemsetupLoadProblem(error);
testOperatorEqual(error);
testOperatorEqual(error);*/
if (error) {
std::cout << "There were some errors\n";
......
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