From bf5275b852a57a1f0c117532bd0c28f3f9af2e12 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Tue, 30 Apr 2019 17:55:29 +0200
Subject: [PATCH] Edit std::exception thrown, indentation and style
---
include/curves/MathDefs.h | 2 +
include/curves/bezier_curve.h | 63 +++++++++--
include/curves/cubic_hermite_spline.h | 2 +-
include/curves/exact_cubic.h | 18 +++-
.../curves/helpers/effector_spline_rotation.h | 2 +
include/curves/linear_variable.h | 34 +++++-
include/curves/optimization/OptimizeSpline.h | 51 +++++----
include/curves/polynom.h | 26 ++++-
include/curves/spline_deriv_constraint.h | 10 +-
python/python_definitions.h | 2 +
python/python_variables.cpp | 8 ++
tests/Main.cpp | 100 ++++++++++--------
12 files changed, 230 insertions(+), 88 deletions(-)
diff --git a/include/curves/MathDefs.h b/include/curves/MathDefs.h
index 628898e..18cced6 100644
--- a/include/curves/MathDefs.h
+++ b/include/curves/MathDefs.h
@@ -35,7 +35,9 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
+ {
m_sigma_inv(i,i)=1.0/m_sigma(i);
+ }
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 2ddc4b0..54e5244 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -58,10 +58,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
+ 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.
@@ -81,9 +85,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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);
+ }
}
@@ -106,9 +114,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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
@@ -127,10 +139,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
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)
+ {
pts_.push_back(*cit);
+ }
}
///\brief Destructor
@@ -152,10 +168,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
virtual point_t operator()(const time_t t) const
{
if(Safe &! (0 <= t && t <= T_))
+ {
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
- if (size_ == 1){
+ }
+ if (size_ == 1)
+ {
return mult_T_*pts_[0];
- }else{
+ }else
+ {
return evalHorner(t);
}
}
@@ -166,7 +186,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
- if(order == 0) return *this;
+ if(order == 0)
+ {
+ return *this;
+ }
t_point_t derived_wp;
for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end()-1; ++pit)
derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
@@ -183,7 +206,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
- if(order == 0) return *this;
+ if(order == 0)
+ {
+ return *this;
+ }
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
point_t current_sum = point_t::Zero(Dim);
@@ -227,7 +253,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typename t_point_t::const_iterator pts_it = pts_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++pts_it)
+ {
res += cit->operator()(u) * (*pts_it);
+ }
return res*mult_T_;
}
@@ -274,7 +302,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
// normalize time :
const Numeric u = t/T_;
t_point_t pts = deCasteljauReduction(waypoints(),u);
- while(pts.size() > 1){
+ while(pts.size() > 1)
+ {
pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
@@ -296,12 +325,17 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
if(u < 0 || u > 1)
+ {
throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
+ }
if(pts.size() == 1)
+ {
return pts;
+ }
t_point_t new_pts;
- for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
+ for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
+ {
new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
@@ -314,14 +348,17 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
if (t == T_)
+ {
throw std::runtime_error("can't split curve, interval range is equal to original curve");
+ }
t_point_t wps_first(size_),wps_second(size_);
const double u = t/T_;
wps_first[0] = pts_.front();
wps_second[degree_] = pts_.back();
t_point_t casteljau_pts = waypoints();
size_t id = 1;
- while(casteljau_pts.size() > 1){
+ while(casteljau_pts.size() > 1)
+ {
casteljau_pts = deCasteljauReduction(casteljau_pts,u);
wps_first[id] = casteljau_pts.front();
wps_second[degree_-id] = casteljau_pts.back();
@@ -335,13 +372,21 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
bezier_curve_t extract(const Numeric t1, const Numeric t2){
if(t1 < 0. || t1 > T_ || t2 < 0. || t2 > T_)
+ {
throw std::out_of_range("In Extract curve : times out of bounds");
+ }
if(t1 == 0. && t2 == T_)
+ {
return bezier_curve_t(waypoints().begin(), waypoints().end(), T_,mult_T_);
+ }
if(t1 == 0.)
+ {
return split(t2).first;
+ }
if(t2 == T_)
+ {
return split(t1).second;
+ }
std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
return c_split.second.split(t2-t1).first;
@@ -364,7 +409,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
res.push_back(P2);
for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
+ {
res.push_back(*it);
+ }
res.push_back(P_n_2);
res.push_back(P_n_1);
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index fcb7aad..4403f7b 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -69,7 +69,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
std::size_t const size(std::distance(PairsBegin, PairsEnd));
if(Safe && size < 1)
{
- throw std::length_error("can't create cubic_hermite_spline, number of pairs is inferior to 2.");
+ throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
}
// Push all pairs in controlPoints
In it(PairsBegin);
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 7ec2fae..8617c1b 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -138,11 +138,11 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
- }
+ }
// adding last x
x.row(size-1)= (*it).second.transpose();
// Compute coefficients of polynom.
- a= x;
+ a = x;
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
@@ -171,11 +171,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
virtual point_t operator()(const time_t t) const
{
- if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
+ if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max()))
+ {
+ throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the spline");
+ }
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
+ {
return it->operator()(t);
+ }
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
@@ -188,11 +193,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
virtual point_t derivate(const time_t t, const std::size_t order) const
{
- if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
+ if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max()))
+ {
+ throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the spline");
+ }
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
+ {
return it->derivate(t, order);
+ }
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index 44fccfe..4e7430f 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -223,7 +223,9 @@ class effector_spline_rotation
exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
{
if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
+ {
return simple_quat_spline();
+ }
exact_cubic_quat_t::t_spline_t splines;
InQuat it(quatWayPointsBegin);
Time init = time_lift_offset_;
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index 5a1dc49..14ea20e 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -75,13 +75,16 @@ struct variables{
variables& operator+=(const variables& w1)
{
if(variables_.size() == 0)
+ {
variables_ = w1.variables_;
- else if (w1.variables_.size() !=0)
+ } else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
+ {
(*it)+=(*cit);
+ }
}
return *this;
}
@@ -89,13 +92,16 @@ struct variables{
variables& operator-=(const variables& w1)
{
if(variables_.size() == 0)
+ {
variables_ = w1.variables_;
- else if (w1.variables_.size() !=0)
+ } else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
+ {
(*it)-=(*cit);
+ }
}
return *this;
}
@@ -137,14 +143,20 @@ template<typename V>
variables<V> operator+(const variables<V>& w1, const variables<V>& w2)
{
if(w2.variables_.size() == 0)
+ {
return w1;
+ }
if(w1.variables_.size() == 0)
+ {
return w2;
+ }
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
+ {
res.variables_.push_back((*cit)+(*cit2));
+ }
return res;
}
@@ -152,14 +164,20 @@ template<typename V>
variables<V> operator-(const variables<V>& w1, const variables<V>& w2)
{
if(w2.variables_.size() == 0)
+ {
return w1;
+ }
if(w1.variables_.size() == 0)
+ {
return w2;
+ }
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
+ {
res.variables_.push_back((*cit)-(*cit2));
+ }
return res;
}
@@ -167,10 +185,14 @@ template<typename V>
variables<V> operator*(const double k, const variables<V>& w)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back(k*(*cit));
+ }
return res;
}
@@ -178,10 +200,14 @@ template<typename V>
variables<V> operator*(const variables<V>& w,const double k)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back((*cit)*k);
+ }
return res;
}
@@ -189,10 +215,14 @@ template<typename V>
variables<V> operator/(const variables<V>& w,const double k)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back((*cit)/k);
+ }
return res;
}
diff --git a/include/curves/optimization/OptimizeSpline.h b/include/curves/optimization/OptimizeSpline.h
index 1107a58..85fbb64 100644
--- a/include/curves/optimization/OptimizeSpline.h
+++ b/include/curves/optimization/OptimizeSpline.h
@@ -125,7 +125,7 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
- throw; // TODO
+ throw std::length_error("can not generate optimizedCurve, number of waypoints should be superior to one");; // TODO
}
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
@@ -258,7 +258,7 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
for(int j = 0; j<Dim; ++j)
{
int id = i + j;
- h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
+ h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
h3x_h4x.block(id, j*size + ptOff , 1, size) = h4.row(i%3);
h3x_h4x.block(id, j*size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
}
@@ -430,56 +430,67 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
/* Append 'numcon' empty constraints.
The constraints will initially have no bounds. */
if ( r == MSK_RES_OK )
+ {
r = MSK_appendcons(task,numcon);
+ }
/* Append 'numvar' variables.
The variables will initially be fixed at zero (x=0). */
if ( r == MSK_RES_OK )
+ {
r = MSK_appendvars(task,numvar);
+ }
for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
{
- /* Set the linear term c_j in the objective.*/
- if(r == MSK_RES_OK)
- r = MSK_putcj(task,j,0);
-
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putcj(task,j,0);
+ }
/* Set the bounds on variable j.
blx[j] <= x_j <= bux[j] */
if(r == MSK_RES_OK)
- r = MSK_putvarbound(task,
- j, /* Index of variable.*/
- bkx[j], /* Bound key.*/
- blx[j], /* Numerical value of lower bound.*/
- bux[j]); /* Numerical value of upper bound.*/
-
+ {
+ r = MSK_putvarbound(task,
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+ }
/* Input column j of A */
if(r == MSK_RES_OK)
- r = MSK_putacol(task,
- j, /* Variable (column) index.*/
- aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
- asub+aptrb[j], /* Pointer to row indexes of column j.*/
- aval+aptrb[j]); /* Pointer to Values of column j.*/
+ {
+ r = MSK_putacol(task,
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ asub+aptrb[j], /* Pointer to row indexes of column j.*/
+ aval+aptrb[j]); /* Pointer to Values of column j.*/
+ }
}
/* Set the bounds on constraints.
for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
+ {
r = MSK_putconbound(task,
i, /* Index of constraint.*/
bkc[i], /* Bound key.*/
blc[i], /* Numerical value of lower bound.*/
buc[i]); /* Numerical value of upper bound.*/
+ }
/* Maximize objective function. */
if (r == MSK_RES_OK)
+ {
r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
+ }
if ( r==MSK_RES_OK )
{
- /* Input the Q for the objective. */
-
- r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
+ /* Input the Q for the objective. */
+ r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
}
if ( r==MSK_RES_OK )
diff --git a/include/curves/polynom.h b/include/curves/polynom.h
index 9159f26..ff6471a 100644
--- a/include/curves/polynom.h
+++ b/include/curves/polynom.h
@@ -106,9 +106,13 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
if(Safe)
{
if(t_min_ > t_max_)
- std::out_of_range("TODO");
+ {
+ std::out_of_range("Tmin should be inferior to Tmax");
+ }
if(coefficients_.size() != int(order_+1))
+ {
std::runtime_error("Spline order and coefficients do not match");
+ }
}
}
@@ -136,11 +140,16 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual point_t operator()(const time_t t) const
{
- if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ if((t < t_min_ || t > t_max_) && Safe)
+ {
+ throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the curve");
+ }
time_t const dt (t-t_min_);
point_t h = coefficients_.col(order_);
for(int i=(int)(order_-1); i>=0; i--)
+ {
h = dt*h + coefficients_.col(i);
+ }
return h;
}
@@ -151,12 +160,17 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
virtual point_t derivate(const time_t t, const std::size_t order) const
{
- if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ if((t < t_min_ || t > t_max_) && Safe)
+ {
+ throw std::out_of_range("TODO");
+ }
time_t const dt (t-t_min_);
time_t cdt(1);
point_t currentPoint_ = point_t::Zero();
- for(int i = (int)(order); i < (int)(order_+1); ++i, cdt*=dt)
+ for(int i = (int)(order); i < (int)(order_+1); ++i, cdt*=dt)
+ {
currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
+ }
return currentPoint_;
}
@@ -165,7 +179,9 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
num_t res(1);
for(std::size_t i = 0; i < order; ++i)
+ {
res *= (num_t)(n-i);
+ }
return res;
}
@@ -198,7 +214,9 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
coeff_t res = coeff_t(Dim, size); int i = 0;
for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)
+ {
res.col(i) = *cit;
+ }
return res;
}
}; //class polynom
diff --git a/include/curves/spline_deriv_constraint.h b/include/curves/spline_deriv_constraint.h
index 1c4aec7..ec12814 100644
--- a/include/curves/spline_deriv_constraint.h
+++ b/include/curves/spline_deriv_constraint.h
@@ -123,13 +123,19 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
- if(Safe && size < 1) throw; // TODO
- t_spline_t subSplines; subSplines.reserve(size-1);
+ if(Safe && size < 1)
+ {
+ throw std::length_error("number of waypoints should be superior to one"); // TODO
+ }
+ t_spline_t subSplines;
+ subSplines.reserve(size-1);
spline_constraints cons = constraints;
In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd-1);
++next;
for(std::size_t i(0); next != end; ++next, ++it, ++i)
+ {
compute_one_spline<In>(it, next, cons, subSplines);
+ }
compute_end_spline<In>(it, next,cons, subSplines);
return subSplines;
}
diff --git a/python/python_definitions.h b/python/python_definitions.h
index de4fb57..8c4b3d6 100644
--- a/python/python_definitions.h
+++ b/python/python_definitions.h
@@ -35,7 +35,9 @@ T_Point vectorFromEigenArray(const PointList& array)
{
T_Point res;
for(int i =0;i<array.cols();++i)
+ {
res.push_back(array.col(i));
+ }
return res;
}
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 715c0d9..1638985 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -12,7 +12,9 @@ std::vector<linear_variable_3_t> matrix3DFromEigenArray(const point_list_t& matr
assert(vectors.cols() * 3 == matrices.cols() ) ;
std::vector<linear_variable_3_t> res;
for(int i =0;i<vectors.cols();++i)
+ {
res.push_back(linear_variable_3_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
+ }
return res;
}
@@ -21,10 +23,14 @@ variables_3_t fillWithZeros(const linear_variable_3_t& var, const std::size_t to
variables_3_t res;
std::vector<linear_variable_3_t>& vars = res.variables_;
for (std::size_t idx = 0; idx < i; ++idx)
+ {
vars.push_back(linear_variable_3_t::Zero());
+ }
vars.push_back(var);
for (std::size_t idx = i+1; idx < totalvar; ++idx)
+ {
vars.push_back(linear_variable_3_t::Zero());
+ }
return res;
}
@@ -35,7 +41,9 @@ std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matric
// now need to fill all this with zeros...
std::size_t totalvar = variables.size();
for (std::size_t i = 0; i < totalvar; ++i)
+ {
res.push_back( fillWithZeros(variables[i],totalvar,i));
+ }
return res;
}
diff --git a/tests/Main.cpp b/tests/Main.cpp
index d22c63e..84be398 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -146,49 +146,49 @@ void CubicFunctionTest(bool& error)
/*bezier_curve Function tests*/
void BezierCurveTest(bool& error)
{
- std::string errMsg("In test BezierCurveTest ; 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,6,7);
+ std::string errMsg("In test BezierCurveTest ; 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,6,7);
- std::vector<point_t> params;
- params.push_back(a);
+ std::vector<point_t> params;
+ params.push_back(a);
- // 1d curve
- bezier_curve_t cf1(params.begin(), params.end());
- point_t res1;
- res1 = cf1(0);
- point_t x10 = a ;
+ // 1d curve
+ bezier_curve_t cf1(params.begin(), params.end());
+ point_t res1;
+ res1 = cf1(0);
+ point_t x10 = a ;
ComparePoints(x10, res1, errMsg + "1(0) ", error);
- res1 = cf1(1);
+ res1 = cf1(1);
ComparePoints(x10, res1, errMsg + "1(1) ", error);
- // 2d curve
- params.push_back(b);
- bezier_curve_t cf(params.begin(), params.end());
- res1 = cf(0);
- point_t x20 = a ;
+ // 2d curve
+ params.push_back(b);
+ bezier_curve_t cf(params.begin(), params.end());
+ res1 = cf(0);
+ point_t x20 = a ;
ComparePoints(x20, res1, errMsg + "2(0) ", error);
- point_t x21 = b;
- res1 = cf(1);
+ point_t x21 = b;
+ res1 = cf(1);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
- //3d curve
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
- res1 = cf3(0);
+ //3d curve
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end());
+ res1 = cf3(0);
ComparePoints(a, res1, errMsg + "3(0) ", error);
- res1 = cf3(1);
+ res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
- //4d curve
- params.push_back(d);
+ //4d curve
+ params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 2);
- res1 = cf4(2);
+ res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error);
//testing bernstein polynomes
@@ -203,11 +203,11 @@ void BezierCurveTest(bool& error)
}
bool error_in(true);
+
try
{
cf(-0.4);
- }
- catch(...)
+ } catch(...)
{
error_in = false;
}
@@ -217,11 +217,11 @@ void BezierCurveTest(bool& error)
error = true;
}
error_in = true;
+
try
{
cf(1.1);
- }
- catch(...)
+ } catch(...)
{
error_in = false;
}
@@ -298,7 +298,6 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
}
e3 = clock();
-
std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
@@ -306,7 +305,6 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
std::cout << "now with high order polynom " << std::endl;
-
params.push_back(d);
params.push_back(e);
params.push_back(f);
@@ -344,8 +342,6 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
}
e3 = clock();
-
-
std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
@@ -800,7 +796,9 @@ point_t randomPoint(const double min, const double max )
{
point_t p;
for(size_t i = 0 ; i < 3 ; ++i)
+ {
p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
+ }
return p;
}
@@ -890,47 +888,56 @@ void BezierSplitCurve(bool& error)
// test on splitted curves :
- if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) )){
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
+ {
error = true;
std::cout<<" Degree of the splitted curve are not the same as the original curve"<<std::endl;
}
- if(c.max() != (cs.first.max() + cs.second.max())){
+ if(c.max() != (cs.first.max() + cs.second.max()))
+ {
error = true;
std::cout<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(c(0) != cs.first(0)){
+ if(c(0) != cs.first(0))
+ {
error = true;
std::cout<<"initial point of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(c(t) != cs.second(cs.second.max())){
+ if(c(t) != cs.second(cs.second.max()))
+ {
error = true;
std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(cs.first.max() != ts){
+ if(cs.first.max() != ts)
+ {
error = true;
std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(cs.first(ts) != cs.second(0.)){
+ if(cs.first(ts) != cs.second(0.))
+ {
error = true;
std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
}
// check along curve :
double ti = 0.;
- while(ti <= ts){
- if((cs.first(ti) - c(ti)).norm() > 1e-14){
+ while(ti <= ts)
+ {
+ if((cs.first(ti) - c(ti)).norm() > 1e-14)
+ {
error = true;
std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
}
ti += 0.01;
}
while(ti <= t){
- if((cs.second(ti-ts) - c(ti)).norm() > 1e-14){
+ if((cs.second(ti-ts) - c(ti)).norm() > 1e-14)
+ {
error = true;
std::cout<<"second splitted curve and original curve doesn't correspond, error = "<<cs.second(ti-ts) - c(ti)<<std::endl;
}
@@ -1058,8 +1065,7 @@ int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "There were some errors\n";
return -1;
- }
- else
+ } else
{
std::cout << "no errors found \n";
return 0;
--
GitLab