diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h index 87362eaa7c48d9d2e8a4e07cb34ab05427ff3bf6..4b826f49b1e6c7477ec37290c6a00e192ae211c8 100644 --- a/include/curves/bezier_curve.h +++ b/include/curves/bezier_curve.h @@ -397,12 +397,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) { t_point_t res; + num_t T_square = T_ * T_; point_t P0, P1, P2, P_n_2, P_n_1, PN; P0 = *PointsBegin; PN = *(PointsEnd-1); - P1 = P0+ constraints.init_vel / (num_t)degree_; - P_n_1 = PN -constraints.end_vel / (num_t)degree_; - P2 = constraints.init_acc / (num_t)(degree_ * (degree_-1)) + 2* P1 - P0; - P_n_2 = constraints.end_acc / (num_t)(degree_ * (degree_-1)) + 2* P_n_1 - PN; + P1 = P0+ constraints.init_vel * T_ / (num_t)degree_; + P_n_1 = PN -constraints.end_vel * T_ / (num_t)degree_; + P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_-1)) + 2* P1 - P0; + P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_-1)) + 2* P_n_1 - PN; res.push_back(P0); res.push_back(P1); diff --git a/tests/Main.cpp b/tests/Main.cpp index 12bf8838301e77573769fa5dddf6034de2c69e30..2b4ac4f80a435b5e2c394cbc8cc8b46c175201fa 100644 --- a/tests/Main.cpp +++ b/tests/Main.cpp @@ -407,20 +407,21 @@ void BezierDerivativeCurveConstraintTest(bool& error) std::vector<point_t> params; params.push_back(a); params.push_back(b); - params.push_back(c); - bezier_curve_t cf3(params.begin(), params.end(), constraints); + + bezier_curve_t::num_t T_max = 2.0; + bezier_curve_t cf3(params.begin(), params.end(), constraints, T_max); assert(cf3.degree_ == params.size() + 3); assert(cf3.size_ == params.size() + 4); ComparePoints(a, cf3(0), errMsg, error); - ComparePoints(c, cf3(1), errMsg, error); + ComparePoints(c, cf3(T_max), errMsg, error); ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error); - ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error); + ComparePoints(constraints.end_vel , cf3.derivate(T_max,1), errMsg, error); ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error); - ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error); - ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error); + ComparePoints(constraints.end_vel, cf3.derivate(T_max,1), errMsg, error); + ComparePoints(constraints.end_acc, cf3.derivate(T_max,2), errMsg, error); } @@ -448,7 +449,7 @@ void BezierToPolynomialConversionTest(bool& error) params.push_back(h); params.push_back(i); - bezier_curve_t cf(params.begin(), params.end()); + bezier_curve_t cf(params.begin(), params.end(), 2.0); polynomial_t pol =from_bezier<bezier_curve_t, polynomial_t>(cf); for(double i =0.; i<1.; i+=0.01) {