Merge pull request #147 from florent-lamiraux/devel

Fix ConvexShapeContactComplement::isEqual method

include/hpp/constraints/convex-shape-contact.hh

@@ -154,21 +154,7 @@ namespace hpp {
                           const JointAndShapes_t& floorSurfaces,
                           const JointAndShapes_t& objectSurfaces);
 
-      bool isEqual(const DifferentiableFunction& other) const {
-        const ConvexShapeContact& castother = dynamic_cast<const ConvexShapeContact&>(other);
-        if (!DifferentiableFunction::isEqual(other))
-          return false;
-        
-        if (robot_ != castother.robot_)
-          return false;
-        if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
-          return false;
-        for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
-          if (floorConvexShapes_[i] != castother.floorConvexShapes_[i])
-            return false;
-        
-        return true;
-      }
+      bool isEqual(const DifferentiableFunction& other) const;
 
       private:
         /// Add a ConvexShape as an object.
@@ -270,12 +256,8 @@ namespace hpp {
                                     const JointAndShapes_t& floorSurfaces,
                                     const JointAndShapes_t& objectSurfaces);
 
-      bool isEqual(const DifferentiableFunction& other) const {
-        const ConvexShapeContactComplement& castother = dynamic_cast<const ConvexShapeContactComplement&>(other);
-        if (!DifferentiableFunction::isEqual(other))
-          return false;
-        return (*sibling_ != *(castother.sibling_));
-      }
+      bool isEqual(const DifferentiableFunction& other) const;
+
     private:
       void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument) const;
 
@@ -335,18 +317,8 @@ namespace hpp {
       virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg)
         const;
 
-      bool isEqual(const DifferentiableFunction& other) const {
-        const ConvexShapeContactHold& castother = dynamic_cast<const ConvexShapeContactHold&>(other);
-        if (!DifferentiableFunction::isEqual(other))
-          return false;
-        
-        if (constraint_ != castother.constraint_)
-          return false;
-        if (complement_ != castother.complement_)
-          return false;
-        
-        return true;
-      }
+      bool isEqual(const DifferentiableFunction& other) const;
+
     private:
       ConvexShapeContactPtr_t constraint_;
       ConvexShapeContactComplementPtr_t complement_;

include/hpp/constraints/differentiable-function.hh

@@ -175,18 +175,8 @@ namespace hpp {
           DevicePtr_t robot = DevicePtr_t (),
           value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
 
-      inline bool operator== (DifferentiableFunction const & other) const
-      {
-        try {
-          return isEqual(other) && other.isEqual(*this);
-        } catch (const std::bad_cast& exc) {
-          return false;
-        }
-      }
-      inline bool operator!= (DifferentiableFunction const & b) const
-      {
-        return !(*this == b);
-      }
+      bool operator== (DifferentiableFunction const & other) const;
+      bool operator!= (DifferentiableFunction const & b) const;
       //virtual bool isEqual(DifferentiableFunctionPtr_t const &, bool) const {return true;}
     protected:
       /// \brief Concrete class constructor should call this constructor.

include/hpp/constraints/matrix-view.hh

@@ -507,13 +507,6 @@ namespace Eigen {
         m_nbRows(BlockIndex::cardinal(rows)),
         m_nbCols(BlockIndex::cardinal(cols)), m_rows(rows), m_cols(cols)
       {
-# ifndef NDEBUG
-        // test that input is sorted
-        segments_t r (rows); BlockIndex::sort (r);
-        assert (r == rows);
-        segments_t c (cols); BlockIndex::sort (c);
-        assert (c == cols);
-#endif
       }
 
       /// Constructor by vectors of segments
@@ -526,13 +519,6 @@ namespace Eigen {
                     const size_type& nbCols, const ColIndices_t& cols) :
         m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols)
       {
-# ifndef NDEBUG
-        // test that input is sorted
-        segments_t r (rows); BlockIndex::sort (r);
-        assert (r == rows);
-        segments_t c (cols); BlockIndex::sort (c);
-        assert (c == cols);
-#endif
       }
 
       /// Constructor of single block

src/convex-shape-contact.cc

@@ -58,6 +58,22 @@ namespace hpp {
       computeRadius();
     }
 
+    bool ConvexShapeContact::isEqual(const DifferentiableFunction& other) const
+    {
+      const ConvexShapeContact& castother = dynamic_cast
+        <const ConvexShapeContact&>(other);
+      if (!DifferentiableFunction::isEqual(other))
+        return false;
+      if (robot_ != castother.robot_)
+        return false;
+      if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
+        return false;
+      for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
+        if (floorConvexShapes_[i] != castother.floorConvexShapes_[i])
+          return false;
+      return true;
+    }
+
     ConvexShapeContactPtr_t ConvexShapeContact::create (
            const std::string& name, DevicePtr_t robot,
            const JointAndShapes_t& floorSurfaces,
@@ -339,6 +355,17 @@ namespace hpp {
     {
     }
 
+    bool ConvexShapeContactComplement::isEqual
+    (const DifferentiableFunction& other) const
+    {
+      const ConvexShapeContactComplement& castother = dynamic_cast
+        <const ConvexShapeContactComplement&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        if (sibling_ == castother.sibling_) return true;
+        return (*sibling_ == *(castother.sibling_));
+    }
+
     std::pair < ConvexShapeContactPtr_t,
 		ConvexShapeContactComplementPtr_t >
     ConvexShapeContactComplement::createPair
@@ -495,5 +522,19 @@ namespace hpp {
       complement_->impl_jacobian(jacobian.bottomRows<3>(), arg);
     }
 
+    bool ConvexShapeContactHold::isEqual(const DifferentiableFunction& other)
+      const
+    {
+      const ConvexShapeContactHold& castother = dynamic_cast
+        <const ConvexShapeContactHold&>(other);
+      if (!DifferentiableFunction::isEqual(other))
+        return false;
+      if (constraint_ != castother.constraint_)
+        return false;
+      if (complement_ != castother.complement_)
+        return false;
+      return true;
+    }
+
   } // namespace constraints
 } // namespace hpp

src/differentiable-function.cc

@@ -204,6 +204,21 @@ namespace hpp {
           finiteDiffCentral(jacobian, x, FiniteDiffVectorSpaceOp(eps), *this);
       }
 
+    bool DifferentiableFunction::operator==
+    (DifferentiableFunction const & other) const
+    {
+      try {
+        return isEqual(other) && other.isEqual(*this);
+      } catch (const std::bad_cast& exc) {
+        return false;
+      }
+    }
+    bool DifferentiableFunction::operator!=
+    (DifferentiableFunction const & b) const
+    {
+      return !(*this == b);
+    }
+
     DifferentiableFunction::DifferentiableFunction
     (size_type sizeInput, size_type sizeInputDerivative,
      size_type sizeOutput, std::string name) :