isApprox can now return true only with two curves of the same type

include/curves/bezier_curve.h

@@ -140,9 +140,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     }
   }
 
-  virtual bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    //std::cout<<"is approx in bezier called."<<std::endl;
-    (void)order;
+  /**
+   * @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
+   * @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{
+    std::cout<<"is approx in bezier called."<<std::endl;
     bool equal = T_min_ == other.min()
         && T_max_ == other.max()
         && dim_ == other.dim()
@@ -160,6 +166,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return true;
   }
 
+  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);
+    else
+      return false;
+  }
+
 
   virtual bool operator==(const bezier_curve_t& other) const {
     return isApprox(other);
@@ -169,15 +183,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return !(*this == other);
   }
 
-  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(&other);
-    if(other_cast)
-      return isApprox(*other_cast);
-    else
-      return curve_abc_t::isApprox(other,prec,order);
-  }
-
-
 
   ///  \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).

include/curves/cubic_hermite_spline.h

@@ -104,17 +104,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
   }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const cubic_hermite_spline_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    //std::cout<<"is approx in hermite called."<<std::endl;
-    (void)order; // silent warning, order is not used in this class.
+  bool isApprox(const cubic_hermite_spline_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    std::cout<<"is approx in hermite called."<<std::endl;
     bool equal =  T_min_ == other.min()
         && T_max_ == other.max()
         && dim_ == other.dim()
@@ -133,6 +130,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     return true;
   }
 
+  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);
+    else
+      return false;
+  }
+
   virtual bool operator==(const cubic_hermite_spline_t& other) const {
     return isApprox(other);
   }
@@ -141,14 +146,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     return !(*this == other);
   }
 
-  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    const cubic_hermite_spline_t* other_cast = dynamic_cast<const cubic_hermite_spline_t*>(&other);
-    if(other_cast)
-      return isApprox(*other_cast);
-    else
-      return curve_abc_t::isApprox(other,prec,order);
-  }
-
 
 
   ///  \brief Evaluate the derivative of order N of spline at time t.

include/curves/curve_abc.h

@@ -59,42 +59,17 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
   /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
   virtual point_derivate_t derivate(const time_t t, const std::size_t order) const = 0;
 
+
+
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @brief isApprox check if other and *this are approximately equals given a precision treshold
+   * 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 order the order up to which the derivatives of the curves are checked for equality
    * @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 size_t order = 5) const{
-    bool equal = (min() == other.min())
-               && (max() == other.max())
-               && (dim() == other.dim());
-    if(!equal){
-      return false;
-    }
-    // check the value along the two curves
-    Numeric t = min();
-    while(t<= max()){
-      if(!(*this)(t).isApprox(other(t),prec)){
-        return false;
-      }
-      t += 0.01; // FIXME : define this step somewhere ??
-    }
-    //  check if the derivatives are equals
-    for(size_t n = 1 ; n <= order ; ++n){
-      t = min();
-      while(t<= max()){
-        if(!derivate(t,n).isApprox(other.derivate(t,n),prec)){
-          return false;
-        }
-        t += 0.01; // FIXME : define this step somewhere ??
-      }
-    }
-    return true;
-  }
+  virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
 
   /*Operations*/
 

include/curves/exact_cubic.h

@@ -109,6 +109,17 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
   /// \brief Destructor.
   virtual ~exact_cubic() {}
 
+  /**
+   * @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
+   * @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);
+  }
+
   std::size_t getNumberSplines() { return this->getNumberCurves(); }
 
   spline_t getSplineAt(std::size_t index) {

include/curves/helpers/effector_spline_rotation.h

@@ -73,21 +73,27 @@ class rotation_spline : public curve_abc_quat_t {
   }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const rotation_spline& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
+  bool isApprox(const rotation_spline& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
     return min_ == other.min_
         && 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,order);
+        && 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{
+    const rotation_spline* other_cast = dynamic_cast<const rotation_spline*>(other);
+    if(other_cast)
+      return isApprox(*other_cast,prec);
+    else
+      return false;
   }
 
   virtual bool operator==(const rotation_spline& other) const {

include/curves/piecewise_curve.h

@@ -81,25 +81,30 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
   }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const piecewise_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
+  bool isApprox(const piecewise_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
     std::cout<<"is approx in piecewise called."<<std::endl;
     if(num_curves() != other.num_curves())
       return false;
     for (size_t i = 0 ; i < num_curves() ; ++i) {
-      if(! curve_at_index(i)->isApprox(*other.curve_at_index(i),prec,order))
+      if(! curve_at_index(i)->isApprox(other.curve_at_index(i).get(),prec))
         return false;
     }
     return true;
   }
 
+  virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    const piecewise_curve_t* other_cast = dynamic_cast<const piecewise_curve_t*>(other);
+    if(other_cast)
+      return isApprox(*other_cast,prec);
+    else
+      return false;
+  }
 
   virtual bool operator==(const piecewise_curve_t& other) const {
     return isApprox(other);

include/curves/polynomial.h

@@ -255,17 +255,14 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const polynomial_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    //std::cout<<"is approx in polynomial called."<<std::endl;
-    (void)order; // silent warning, order is not used in this class.
+  bool isApprox(const polynomial_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    std::cout<<"is approx in polynomial called."<<std::endl;
     return T_min_ == other.min()
         && T_max_ == other.max()
         && dim_ == other.dim()
@@ -273,6 +270,15 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
         && coefficients_.isApprox(other.coefficients_,prec);
   }
 
+  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    const polynomial_t* other_cast = dynamic_cast<const polynomial_t*>(other);
+    if(other_cast)
+      return isApprox(*other_cast,prec);
+    else
+      return false;
+  }
+
+
   virtual bool operator==(const polynomial_t& other) const {
     return isApprox(other);
   }
@@ -281,14 +287,6 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
     return !(*this == other);
   }
 
-  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    const polynomial_t* other_cast = dynamic_cast<const polynomial_t*>(&other);
-    if(other_cast)
-      return isApprox(*other_cast);
-    else
-      return curve_abc_t::isApprox(other,prec,order);
-  }
-
 
 
   ///  \brief Evaluation of the derivative of order N of spline at time t.

include/curves/se3_curve.h

@@ -151,23 +151,29 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
   }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    //std::cout<<"is approx in SE3 called."<<std::endl;
-    (void)order; // silent warning, order is not used in this class.
+  bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    std::cout<<"is approx in SE3 called."<<std::endl;
     return T_min_ == other.min()
         && T_max_ == other.max()
-        && (translation_curve_ == other.translation_curve_ || translation_curve_->isApprox(*other.translation_curve_,prec))
-        && (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(*other.rotation_curve_,prec));
+        && (translation_curve_ == other.translation_curve_ || translation_curve_->isApprox(other.translation_curve_.get(),prec))
+        && (rotation_curve_ == other.rotation_curve_ || rotation_curve_->isApprox(other.rotation_curve_.get(),prec));
   }
 
+  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
+    if(other_cast)
+      return isApprox(*other_cast,prec);
+    else
+      return false;
+  }
+
+
   virtual bool operator==(const SE3Curve_t& other) const {
     return isApprox(other);
   }
@@ -176,13 +182,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
     return !(*this == other);
   }
 
-  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(&other);
-    if(other_cast)
-      return isApprox(*other_cast);
-    else
-      return curve_abc_t::isApprox(other,prec,order);
-  }
 
   ///  \brief Evaluation of the derivative of order N of spline at time t.
   ///  \param t : the time when to evaluate the spline.

include/curves/so3_linear.h

@@ -108,17 +108,14 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
   virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
 
   /**
-   * @brief isApprox check if other and *this are equals, given a precision treshold.
-   * This test is done by discretizing, it should be re-implemented in the child class to check exactly
-   * all the members.
+   * @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
    * @param other the other curve to check
-   * @param order the order up to which the derivatives of the curves are checked for equality
    * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
-  virtual bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    //std::cout<<"is approx in SO3 called."<<std::endl;
-    (void)order; // silent warning, order is not used in this class.
+  bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    std::cout<<"is approx in SO3 called."<<std::endl;
     return  T_min_ == other.min()
         && T_max_ == other.max()
         && dim_ == other.dim()
@@ -126,6 +123,15 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         && end_rot_.toRotationMatrix().isApprox(other.end_rot_.toRotationMatrix(),prec);
   }
 
+  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+    const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other);
+    if(other_cast)
+      return isApprox(*other_cast,prec);
+    else
+      return false;
+  }
+
+
   virtual bool operator==(const SO3Linear_t& other) const {
     return isApprox(other);
   }
@@ -134,14 +140,6 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
     return !(*this == other);
   }
 
-  virtual bool isApprox(const curve_abc_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),const size_t order = 5) const{
-    const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(&other);
-    if(other_cast)
-      return isApprox(*other_cast);
-    else
-      return curve_abc_t::isApprox(other,prec,order);
-  }
-
 
   ///  \brief Evaluation of the derivative of order N of spline at time t.
   ///  \param t : the time when to evaluate the spline.