Commit ee941bdc authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Format

parent 67c08e39
......@@ -45,16 +45,12 @@ struct Bern {
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
}
virtual bool operator== (const Bern& other) const{
return curves::isApprox<Numeric>(m_minus_i, other.m_minus_i)
&& curves::isApprox<Numeric>(i_, other.i_)
&& curves::isApprox<Numeric>(bin_m_i_, other.bin_m_i_);
}
virtual bool operator!=(const Bern& other) const {
return !(*this == other);
virtual bool operator==(const Bern& other) const {
return curves::isApprox<Numeric>(m_minus_i, other.m_minus_i) && curves::isApprox<Numeric>(i_, other.i_) &&
curves::isApprox<Numeric>(bin_m_i_, other.bin_m_i_);
}
virtual bool operator!=(const Bern& other) const { return !(*this == other); }
/* Attributes */
Numeric m_minus_i;
......
......@@ -142,46 +142,35 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see
* isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
bool equal = curves::isApprox<num_t> (T_min_, other.min())
&& curves::isApprox<num_t> (T_max_, other.max())
&& dim_ == other.dim()
&& degree_ == other.degree()
&& size_ == other.size_
&& curves::isApprox<Numeric>(mult_T_, other.mult_T_)
&& bernstein_ == other.bernstein_;
if(!equal)
return false;
for (size_t i = 0 ;i < size_;++i)
{
if(!control_points_.at(i).isApprox(other.control_points_.at(i),prec))
return false;
bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
dim_ == other.dim() && degree_ == other.degree() && size_ == other.size_ &&
curves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
if (!equal) return false;
for (size_t i = 0; i < size_; ++i) {
if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec)) return false;
}
return true;
}
virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
virtual bool isApprox(const curve_abc_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
if(other_cast)
return isApprox(*other_cast,prec);
if (other_cast)
return isApprox(*other_cast, prec);
else
return false;
}
virtual bool operator==(const bezier_curve_t& other) const { return isApprox(other); }
virtual bool operator==(const bezier_curve_t& other) const {
return isApprox(other);
}
virtual bool operator!=(const bezier_curve_t& other) const {
return !(*this == other);
}
virtual bool operator!=(const bezier_curve_t& other) const { return !(*this == other); }
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
......@@ -241,9 +230,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \param t : time when to evaluate the curve.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual point_t derivate(const time_t t, const std::size_t order) const {
return compute_derivate(order)(t);
}
virtual point_t derivate(const time_t t, const std::size_t order) const { return compute_derivate(order)(t); }
/// \brief Evaluate all Bernstein polynomes for a certain degree.
/// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
......@@ -390,7 +377,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
curves.push_back(current);
piecewise_curve_t res;
for(typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end() ; ++cit){
for (typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
res.add_curve_ptr(ptr);
}
......@@ -472,7 +459,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
virtual time_t max() const { return T_max_; }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return degree_;}
virtual std::size_t degree() const { return degree_; }
/*Helpers*/
/* Attributes */
......
......@@ -41,7 +41,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef cubic_hermite_spline<Time, Numeric, Safe, point_t> cubic_hermite_spline_t;
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
......@@ -105,47 +104,38 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see
* isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool isApprox(const cubic_hermite_spline_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
bool equal = curves::isApprox<num_t> (T_min_, other.min())
&& curves::isApprox<num_t> (T_max_, other.max())
&& dim_ == other.dim()
&& degree_ == other.degree()
&& size_ == other.size()
&& time_control_points_ == other.time_control_points_
&& duration_splines_ == other.duration_splines_;
if(!equal)
return false;
for (std::size_t i = 0 ; i < size_ ;++i)
{
if((!control_points_[i].first.isApprox(other.control_points_[i].first,prec)) ||
(!control_points_[i].second.isApprox(other.control_points_[i].second,prec)) )
bool isApprox(const cubic_hermite_spline_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
dim_ == other.dim() && degree_ == other.degree() && size_ == other.size() &&
time_control_points_ == other.time_control_points_ && duration_splines_ == other.duration_splines_;
if (!equal) return false;
for (std::size_t i = 0; i < size_; ++i) {
if ((!control_points_[i].first.isApprox(other.control_points_[i].first, prec)) ||
(!control_points_[i].second.isApprox(other.control_points_[i].second, prec)))
return false;
}
return true;
}
virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
virtual bool isApprox(const curve_abc_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const cubic_hermite_spline_t* other_cast = dynamic_cast<const cubic_hermite_spline_t*>(other);
if(other_cast)
return isApprox(*other_cast,prec);
if (other_cast)
return isApprox(*other_cast, prec);
else
return false;
}
virtual bool operator==(const cubic_hermite_spline_t& other) const {
return isApprox(other);
}
virtual bool operator!=(const cubic_hermite_spline_t& other) const {
return !(*this == other);
}
virtual bool operator==(const cubic_hermite_spline_t& other) const { return isApprox(other); }
virtual bool operator!=(const cubic_hermite_spline_t& other) const { return !(*this == other); }
/// \brief Evaluate the derivative of order N of spline at time t.
/// \param t : time when to evaluate the spline.
......@@ -168,8 +158,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return new cubic_hermite_spline_t(compute_derivate(order));
}
/// \brief Set time of each control point of cubic hermite spline.
/// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
/// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
......@@ -385,7 +373,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
Time virtual max() const { return time_control_points_.back(); }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return degree_;}
virtual std::size_t degree() const { return degree_; }
/*Helpers*/
/*Attributes*/
......
......@@ -22,9 +22,8 @@
namespace curves {
template <typename T>
bool isApprox(const T a, const T b, const T eps = 1e-6)
{
return fabs(a - b) < eps;
bool isApprox(const T a, const T b, const T eps = 1e-6) {
return fabs(a - b) < eps;
}
/// \struct curve_abc.
......@@ -37,7 +36,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
typedef Point_derivate point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_t; // parent class
typedef boost::shared_ptr<curve_t> curve_ptr_t;
/* Constructors - destructors */
......@@ -55,7 +54,6 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const = 0;
/// \brief Compute the derived curve at order N.
/// \param order : order of derivative.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
......@@ -75,27 +73,27 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool isEquivalent(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
bool equal = curves::isApprox<num_t> (min(), other->min())
&& curves::isApprox<num_t> (max(), other->max())
&& (dim() == other->dim());
if(!equal){
bool isEquivalent(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),
const size_t order = 5) const {
bool equal = curves::isApprox<num_t>(min(), other->min()) && curves::isApprox<num_t>(max(), other->max()) &&
(dim() == other->dim());
if (!equal) {
return false;
}
time_t inc = (max() - min()) / 10.; // FIXME : define this step somewhere ??
time_t inc = (max() - min()) / 10.; // FIXME : define this step somewhere ??
// check the value along the two curves
time_t t = min();
while(t<= max()){
if(!(*this)(t).isApprox(other->operator()(t),prec)){
while (t <= max()) {
if (!(*this)(t).isApprox(other->operator()(t), prec)) {
return false;
}
t += inc;
}
// check if the derivatives are equals
for(size_t n = 1 ; n <= order ; ++n){
for (size_t n = 1; n <= order; ++n) {
t = min();
while(t<= max()){
if(!derivate(t,n).isApprox(other->derivate(t,n),prec)){
while (t <= max()) {
if (!derivate(t, n).isApprox(other->derivate(t, n), prec)) {
return false;
}
t += inc;
......@@ -112,7 +110,8 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
virtual bool isApprox(const curve_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
/*Operations*/
......@@ -128,7 +127,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
virtual time_t max() const = 0;
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const =0;
virtual std::size_t degree() const = 0;
std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
/*Helpers*/
......
......@@ -28,7 +28,6 @@ Polynomial polynomial_from_curve(const typename Polynomial::curve_abc_t& curve)
coefficients.push_back(curve.derivate(curve.min(), i) / fact);
}
return Polynomial(coefficients, curve.min(), curve.max());
}
......
......@@ -68,11 +68,9 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: piecewise_curve_t()
{
exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t() {
t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
this->add_curve(*it);
}
}
......@@ -83,25 +81,22 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
/// \param constraints : constraints on the init and end velocity / accelerations of the spline.
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: piecewise_curve_t()
{
t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd,constraints);
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) : piecewise_curve_t() {
t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints);
for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
this->add_curve(*it);
}
}
/// \brief Constructor.
/// \param subSplines: vector of subSplines.
exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t()
{
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t() {
for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
this->add_curve(*it);
}
}
exact_cubic(const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) { }
exact_cubic(const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) {}
/// \brief Copy Constructor.
exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}
......@@ -111,23 +106,25 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see
* isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool isApprox(const exact_cubic_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
return piecewise_curve_t::isApprox(other,prec);
bool isApprox(const exact_cubic_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return piecewise_curve_t::isApprox(other, prec);
}
std::size_t getNumberSplines() { return this->getNumberCurves(); }
spline_t getSplineAt(std::size_t index) {
boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->curves_.at(index));
if(s_ptr)
if (s_ptr)
return *s_ptr;
else
throw std::runtime_error("Parent piecewise curve do not contain only curves created from exact_cubic class methods");
throw std::runtime_error(
"Parent piecewise curve do not contain only curves created from exact_cubic class methods");
}
private:
......
......@@ -15,43 +15,41 @@
namespace curves {
template <typename Time, typename Numeric, bool Safe, typename Point , typename Point_derivate >
struct curve_abc;
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
struct curve_abc;
template <typename Time, typename Numeric, bool Safe,typename Point >
struct bezier_curve;
template <typename Time, typename Numeric, bool Safe, typename Point>
struct bezier_curve;
template <typename Time, typename Numeric, bool Safe,typename Point >
struct cubic_hermite_spline;
template <typename Time, typename Numeric, bool Safe, typename Point>
struct cubic_hermite_spline;
template <typename Time, typename Numeric, bool Safe, typename Point,
typename T_Point,typename SplineBase >
struct exact_cubic;
template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point, typename SplineBase>
struct exact_cubic;
template <typename Time, typename Numeric, bool Safe, typename Point,
typename Point_derivate, typename CurveType>
struct piecewise_curve;
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
struct piecewise_curve;
template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
struct polynomial;
template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
struct polynomial;
template <typename Time, typename Numeric, bool Safe>
struct SE3Curve;
struct SE3Curve;
template <typename Time, typename Numeric, bool Safe>
struct SO3Linear;
struct SO3Linear;
template <typename Numeric>
struct Bern;
struct Bern;
template <typename Point>
struct curve_constraints;
struct curve_constraints;
template <typename Numeric, bool Safe>
struct linear_variable;
struct linear_variable;
template <typename Numeric>
struct quadratic_variable;
struct quadratic_variable;
// typedef of the commonly used templates arguments :
// eigen types :
......@@ -66,10 +64,12 @@ typedef std::vector<point3_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
// abstract curves types:
typedef curve_abc<double, double, true, pointX_t,pointX_t> curve_abc_t; // base abstract class
typedef curve_abc<double, double, true, point3_t,point3_t> curve_3_t; // generic class of curve of size 3
typedef curve_abc<double, double, true, matrix3_t, point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
typedef curve_abc<double, double, true, transform_t, point6_t> curve_SE3_t; // templated abstract class used for all the se3 curves (return dimension are fixed)
typedef curve_abc<double, double, true, pointX_t, pointX_t> curve_abc_t; // base abstract class
typedef curve_abc<double, double, true, point3_t, point3_t> curve_3_t; // generic class of curve of size 3
typedef curve_abc<double, double, true, matrix3_t, point3_t>
curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
typedef curve_abc<double, double, true, transform_t, point6_t>
curve_SE3_t; // templated abstract class used for all the se3 curves (return dimension are fixed)
// shared pointer to abstract types:
typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
......@@ -79,23 +79,23 @@ typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
// definition of all curves class with pointX as return type:
typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
typedef exact_cubic<double, double, true, pointX_t, t_pointX_t, polynomial_t> exact_cubic_t;
typedef bezier_curve<double, double, true, pointX_t> bezier_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve <double, double, true, pointX_t,pointX_t, curve_abc_t> piecewise_t;
typedef piecewise_curve<double, double, true, pointX_t, pointX_t, curve_abc_t> piecewise_t;
// definition of all curves class with point3 as return type:
typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
typedef exact_cubic<double, double, true, point3_t, t_point3_t, polynomial_t> exact_cubic3_t;
typedef bezier_curve<double, double, true, point3_t> bezier3_t;
typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
typedef piecewise_curve <double, double, true, point3_t,point3_t, curve_3_t> piecewise3_t;
typedef piecewise_curve<double, double, true, point3_t, point3_t, curve_3_t> piecewise3_t;
// special curves with return type fixed:
typedef SO3Linear<double, double, true> SO3Linear_t;
typedef SE3Curve<double, double, true> SE3Curve_t;
typedef piecewise_curve <double, double, true, transform_t, point6_t, curve_SE3_t> piecewise_SE3_t;
typedef piecewise_curve<double, double, true, transform_t, point6_t, curve_SE3_t> piecewise_SE3_t;
}
} // namespace curves
#endif // CURVES_FWD_H
#endif // CURVES_FWD_H
......@@ -74,36 +74,31 @@ class rotation_spline : public curve_abc_quat_t {
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see
* isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool isApprox(const rotation_spline& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
return curves::isApprox<Numeric> (min_, other.min_)
&& curves::isApprox<Numeric> (max_, other.max_)
&& dim_ == other.dim_
&& quat_from_.isApprox(other.quat_from_,prec)
&& quat_to_.isApprox(other.quat_to_,prec)
&& time_reparam_.isApprox(other.time_reparam_,prec);
bool isApprox(const rotation_spline& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return curves::isApprox<Numeric>(min_, other.min_) && curves::isApprox<Numeric>(max_, other.max_) &&
dim_ == other.dim_ && quat_from_.isApprox(other.quat_from_, prec) &&
quat_to_.isApprox(other.quat_to_, prec) && time_reparam_.isApprox(other.time_reparam_, prec);
}
virtual bool isApprox(const curve_abc_quat_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
virtual bool isApprox(const curve_abc_quat_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const rotation_spline* other_cast = dynamic_cast<const rotation_spline*>(other);
if(other_cast)
return isApprox(*other_cast,prec);
if (other_cast)
return isApprox(*other_cast, prec);
else
return false;
}
virtual bool operator==(const rotation_spline& other) const {
return isApprox(other);
}
virtual bool operator!=(const rotation_spline& other) const {
return !(*this == other);
}
virtual bool operator==(const rotation_spline& other) const { return isApprox(other); }
virtual bool operator!=(const rotation_spline& other) const { return !(*this == other); }
virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
throw std::runtime_error("TODO quaternion spline does not implement derivate");
......@@ -116,7 +111,6 @@ class rotation_spline : public curve_abc_quat_t {
throw std::logic_error("Compute derivate for quaternion spline is not implemented yet.");
}
/// \brief Initialize time reparametrization for spline.
exact_cubic_constraint_one_dim computeWayPoints() const {
t_waypoint_one_dim_t waypoints;
......@@ -130,7 +124,7 @@ class rotation_spline : public curve_abc_quat_t {
virtual time_t max() const { return max_; }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return 1;}
virtual std::size_t degree() const { return 1; }
/*Attributes*/
Eigen::Quaterniond quat_from_; // const
......
......@@ -81,7 +81,8 @@ struct linear_variable : public serialization::Serializable {
Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
bool isApprox(const linear_variable_t& other,const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
bool isApprox(const linear_variable_t& other,
const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return (*this - other).norm() < prec;
}
......
......@@ -34,7 +34,8 @@ quadratic_variable<Numeric> compute_integral_cost_internal(const problem_data<Po
typedef typename t_point_t::const_iterator cit_point_t;
bezier_t acc = pData.bezier->compute_derivate(num_derivate);
const t_point_t& wps = acc.waypoints();
quadratic_variable<Numeric> res(bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
quadratic_variable<Numeric> res(
bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
return res;
}
......
......@@ -24,21 +24,22 @@ namespace curves {
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename Point_derivate = Point, typename CurveType = curve_abc<Time, Numeric, Safe, Point,Point_derivate> >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename Point_derivate = Point,
typename CurveType = curve_abc<Time, Numeric, Safe, Point, Point_derivate> >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {