Commit bf5275b8 authored by JasonChmn's avatar JasonChmn
Browse files

Edit std::exception thrown, indentation and style

parent dd97e5e6
......@@ -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());
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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");
......
......@@ -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_;
......
......@@ -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;
}
......
......@@ -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 )
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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;
}
......
......@@ -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)));
}