Commit d53ed18c authored by stevet's avatar stevet
Browse files

to compare double replaced == by isApprox

parent 3930e168
......@@ -148,8 +148,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
* @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 = T_min_ == other.min()
&& T_max_ == other.max()
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_
......
......@@ -111,8 +111,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
* @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 = T_min_ == other.min()
&& T_max_ == other.max()
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()
......
......@@ -20,6 +20,13 @@
#include <functional>
namespace curves {
template <typename T>
bool isApprox(const T a, const T b, const T eps = 1e-6)
{
return fabs(a - b) < eps;
}
/// \struct curve_abc.
/// \brief Represents a curve of dimension Dim.
/// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
......@@ -29,6 +36,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
typedef Point point_t;
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 boost::shared_ptr<curve_t> curve_ptr_t;
......@@ -68,19 +76,20 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
* @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 = (min() == other->min())
&& (max() == other->max())
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 ??
// check the value along the two curves
Numeric t = min();
time_t t = min();
while(t<= max()){
if(!(*this)(t).isApprox(other->operator()(t),prec)){
return false;
}
t += 0.01; // FIXME : define this step somewhere ??
t += inc;
}
// check if the derivatives are equals
for(size_t n = 1 ; n <= order ; ++n){
......@@ -89,7 +98,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
if(!derivate(t,n).isApprox(other->derivate(t,n),prec)){
return false;
}
t += 0.01; // FIXME : define this step somewhere ??
t += inc;
}
}
return true;
......
......@@ -80,8 +80,8 @@ class rotation_spline : public curve_abc_quat_t {
* @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 min_ == other.min_
&& max_ == other.max_
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)
......
......@@ -262,8 +262,8 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
* @return true is the two curves are approximately equals
*/
bool isApprox(const polynomial_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
return T_min_ == other.min()
&& T_max_ == other.max()
return curves::isApprox<num_t> (T_min_, other.min())
&& curves::isApprox<num_t> (T_max_, other.max())
&& dim_ == other.dim()
&& degree_ == other.degree()
&& coefficients_.isApprox(other.coefficients_,prec);
......
......@@ -158,8 +158,8 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
* @return true is the two curves are approximately equals
*/
bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
return T_min_ == other.min()
&& T_max_ == other.max()
return curves::isApprox<Numeric> (T_min_, other.min())
&& curves::isApprox<Numeric> (T_max_, other.max())
&& (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));
}
......
......@@ -115,8 +115,8 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
* @return true is the two curves are approximately equals
*/
bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
return T_min_ == other.min()
&& T_max_ == other.max()
return curves::isApprox<Numeric> (T_min_, other.min())
&& curves::isApprox<Numeric> (T_max_, other.max())
&& dim_ == other.dim()
&& init_rot_.toRotationMatrix().isApprox(other.init_rot_.toRotationMatrix(),prec)
&& end_rot_.toRotationMatrix().isApprox(other.end_rot_.toRotationMatrix(),prec);
......
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