[solver] Fix method contains.

include/hpp/constraints/solver/by-substitution.hh

@@ -106,14 +106,6 @@ namespace hpp {
           return explicit_;
         }
 
-        /// Check whether a numerical constraint has been added
-        /// \param numericalConstraint numerical constraint
-        /// \return true if numerical constraint is already in the solver.
-        /// \note Comparison between constraints is performed by
-        /// function names. This means that two constraints with the
-        /// same function names are considered as equal.
-        virtual bool contains (const ImplicitPtr_t& numericalConstraint) const;
-
         /// Return the number of free variables
         size_type numberFreeVariables () const
         {

include/hpp/constraints/solver/hierarchical-iterative.hh

@@ -246,9 +246,6 @@ namespace hpp {
         /// Check whether a numerical constraint has been added
         /// \param numericalConstraint numerical constraint
         /// \return true if numerical constraint is already in the solver
-        /// \note Comparison between constraints is performed by
-        /// function names. This means that two constraints with the
-        /// same function names are considered as equal.
         virtual bool contains (const ImplicitPtr_t& numericalConstraint) const;
 
         /// Add an implicit constraint

src/solver/by-substitution.cc

@@ -113,16 +113,6 @@ namespace hpp {
         freeVariables (explicit_.notOutDers ().transpose ());
       }
 
-      bool BySubstitution::contains
-      (const ImplicitPtr_t& numericalConstraint) const
-      {
-        if (parent_t::contains (numericalConstraint)) return true;
-        ExplicitPtr_t expl (HPP_DYNAMIC_PTR_CAST (Explicit,
-                                                  numericalConstraint));
-        if (!expl) return false;
-        return explicit_.contains (expl);
-      }
-
       segments_t BySubstitution::implicitDof () const
       {
         const Eigen::MatrixXi& ioDep = explicit_.inOutDependencies();

src/solver/hierarchical-iterative.cc

@@ -224,24 +224,11 @@ namespace hpp {
       bool HierarchicalIterative::contains
       (const ImplicitPtr_t& numericalConstraint) const
       {
-        // Check that function is in stacks_
-        const DifferentiableFunctionPtr_t& f
-          (numericalConstraint->functionPtr ());
-        for (std::size_t i = 0; i < stacks_.size (); ++i) {
-          const ImplicitConstraintSet& ics (stacks_[i]);
-          assert (HPP_DYNAMIC_PTR_CAST (DifferentiableFunctionSet,
-                                        ics.functionPtr ()));
-          DifferentiableFunctionSetPtr_t dfs
-            (HPP_STATIC_PTR_CAST (DifferentiableFunctionSet,
-                                  ics.functionPtr ()));
-          const DifferentiableFunctionSet::Functions_t& fs (dfs->functions ());
-          for (std::size_t j = 0; j < fs.size(); ++j) {
-            if (*f == *fs[j]) {
-              return true;
-            }
-          }
-        }
-        return false;
+        return
+          find_if(constraints_.begin(), constraints_.end(),
+                  [&numericalConstraint](const ImplicitPtr_t& arg)
+                  { return *arg == *numericalConstraint; }) !=
+          constraints_.end();
       }
 
       bool HierarchicalIterative::add (const ImplicitPtr_t& constraint,

tests/solver-by-substitution.cc

@@ -207,6 +207,8 @@ void test_quadratic2 ()
   solver.add (expl1);
   solver.add (expl2);
   solver.explicitConstraintSetHasChanged();
+  assert(solver.contains(expl1));
+  assert(solver.contains(expl2));
 
   matrix_t M (N, N1 + N4);
   M << matrix_t::Identity(N1,N1), matrix_t::Zero(N1,N4),