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