Commit 950e634c authored by t steve's avatar t steve

Time reparametrization of bezier curves

parent 0d2098ef
......@@ -46,21 +46,60 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
bezier_curve(In PointsBegin, In PointsEnd)
: T_(1.)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || minBound >= maxBound))
if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
: T_(T)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
: T_(T)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
......@@ -69,14 +108,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param constraints : constraints applying on start / end velocities and acceleration
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.)
: T_(T)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
if(Safe && (size_<1 || minBound >= maxBound))
if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound");
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
......@@ -101,7 +140,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual point_t operator()(const time_t t) const
{
num_t nT = (t - minBound_) / (maxBound_ - minBound_);
num_t nT = t / T_;
if(Safe &! (0 <= nT && nT <= 1))
{
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
......@@ -112,22 +151,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
switch(size_)
{
case 1 :
return pts_[0];
return mult_T_ * pts_[0];
case 2 :
return pts_[0] * dt + nT * pts_[1];
return mult_T_ * (pts_[0] * dt + nT * pts_[1]);
break;
case 3 :
return pts_[0] * dt * dt
return mult_T_ * (pts_[0] * dt * dt
+ 2 * pts_[1] * nT * dt
+ pts_[2] * nT * nT;
+ pts_[2] * nT * nT);
break;
case 4 :
return pts_[0] * dt * dt * dt
return mult_T_ * (pts_[0] * dt * dt * dt
+ 3 * pts_[1] * nT * dt * dt
+ 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT;
+ pts_[3] * nT * nT *nT);
default :
return evalHorner(nT);
return mult_T_ * evalHorner(nT);
break;
}
}
......@@ -144,7 +183,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
return deriv.compute_derivate(order-1);
}
......@@ -165,7 +204,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_);
bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
return integ.compute_primitive(order-1);
}
......@@ -248,12 +287,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
virtual time_t min() const{return minBound_;}
virtual time_t max() const{return maxBound_;}
virtual time_t min() const{return 0.;}
virtual time_t max() const{return T_;}
/*Helpers*/
public:
/*const*/ time_t minBound_, maxBound_;
/*const*/ time_t T_;
/*const*/ time_t mult_T_;
/*const*/ std::size_t size_;
/*const*/ std::size_t degree_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
......
......@@ -67,17 +67,17 @@ T_Point vectorFromEigenArray(const PointList& array)
}
template <typename Bezier, typename PointList, typename T_Point>
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 0., const real ub =1.)
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), lb, ub);
return new Bezier(asVector.begin(), asVector.end(), ub);
}
template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real lb = 0., const real ub =1.)
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub);
return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
}
/*3D constructors */
......@@ -85,17 +85,17 @@ bezier_t* wrapBezierConstructor(const point_list_t& array)
{
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ;
}
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, const real ub)
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real ub)
{
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, lb, ub) ;
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, ub) ;
}
bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
}
bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub)
bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, lb, ub) ;
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ;
}
/*END 3D constructors */
/*6D constructors */
......@@ -103,17 +103,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
}
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real lb, const real ub)
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real ub)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, lb, ub) ;
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, ub) ;
}
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints) ;
}
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real lb, const real ub)
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ;
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, ub) ;
}
/*END 6D constructors */
......
......@@ -11,7 +11,7 @@ time_waypoints = matrix([0.,1.])
#testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, -1., 3.)
a = bezier(waypoints, 3.)
assert(a.degree == a.nbWaypoints -1)
a.min()
......@@ -23,6 +23,7 @@ a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert(a(t) == prim.derivate(t,1)).all()
......@@ -34,6 +35,20 @@ for i in range(10):
assert(a(t) == prim.derivate(t,2)).all()
assert(prim(0) == matrix([0.,0.,0.])).all()
waypoints = matrix([[1.,2.,3.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.]]).transpose()
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert norm(a0(t) - a1(3*t)) < __EPS
assert norm(a0.derivate(t,1) - a1.derivate(3*t,1) * 3.) < __EPS
assert norm(a0.derivate(t,2) - a1.derivate(3*t,2) * 9.) < __EPS
assert norm(prim0(t) - prim1(t*3) / 3.) < __EPS
assert(prim(0) == matrix([0.,0.,0.])).all()
#testing bezier with constraints
c = curve_constraints();
......@@ -42,7 +57,7 @@ c.end_vel = matrix([0.,1.,1.]);
c.init_acc = matrix([0.,1.,-1.]);
c.end_acc = matrix([0.,100.,1.]);
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
a = bezier(waypoints,c)
assert norm(a.derivate(0,1) - c.init_vel) < 1e-10
assert norm(a.derivate(1,2) - c.end_acc) < 1e-10
......
......@@ -175,9 +175,7 @@ void BezierCurveTest(bool& error)
//4d curve
params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
res1 = cf4(0.4);
ComparePoints(a, res1, errMsg + "3(0) ", error);
bezier_curve_t cf4(params.begin(), params.end(), 2);
res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error);
......@@ -345,6 +343,32 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
}
void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,4,5);
point_t e(3,4,5);
point_t f(3,4,5);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
params.push_back(d);
params.push_back(e);
params.push_back(f);
double T = 2.;
bezier_curve_t cf(params.begin(), params.end());
bezier_curve_t cfT(params.begin(), params.end(),T);
ComparePoints(cf(0.5), cfT(1), errMsg, error);
ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
......@@ -406,8 +430,6 @@ void BezierToPolynomConversionTest(bool& error)
polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
for(double i =0.; i<1.; i+=0.01)
{
std::cout << "START " << cf(i).transpose() << std::endl;
std::cout << pol(i).transpose() << "END " << std::endl;
ComparePoints(cf(i),pol(i),errMsg, error, true);
ComparePoints(cf(i),pol(i),errMsg, error, false);
}
......@@ -750,7 +772,7 @@ int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
/* CubicFunctionTest(error);
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
ExactCubicTwoPointsTest(error);
......@@ -764,7 +786,8 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierCurveTest(error);
BezierDerivativeCurveTest(error);
BezierDerivativeCurveConstraintTest(error);
//BezierCurveTestCompareHornerAndBernstein(error);*/
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
BezierToPolynomConversionTest(error);
if(error)
{
......
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