Update to modifications in hpp-constraints

  - Refactoring of ConvexShapeContact.

src/problem-solver.cc

@@ -25,6 +25,7 @@
 #include <hpp/pinocchio/gripper.hh>
 
 #include <hpp/constraints/convex-shape-contact.hh>
+#include <hpp/constraints/explicit/convex-shape-contact.hh>
 
 #include <hpp/core/path-optimization/random-shortcut.hh>
 #include <hpp/core/path-optimization/partial-shortcut.hh>
@@ -243,25 +244,15 @@ namespace hpp {
      const StringList_t& surface2, const value_type& margin)
     {
       if (!robot_) throw std::runtime_error ("No robot loaded");
-      using constraints::ConvexShape;
-      using constraints::ConvexShapeContactPtr_t;
-      using constraints::ConvexShapeContactComplement;
-      using constraints::ConvexShapeContactComplementPtr_t;
-      std::string complementName (name + "/complement");
-      std::pair < ConvexShapeContactPtr_t,
-		  ConvexShapeContactComplementPtr_t > constraints
-	(ConvexShapeContactComplement::createPair
-	 (name, complementName, robot_));
-
-      JointAndShapes_t l;
+      JointAndShapes_t floorSurfaces, objectSurfaces, l;
       for (StringList_t::const_iterator it1 = surface1.begin ();
           it1 != surface1.end(); ++it1) {
         if (!robot_->jointAndShapes.has (*it1))
           throw std::runtime_error ("First list of triangles not found.");
         l = robot_->jointAndShapes.get (*it1);
-        for (JointAndShapes_t::const_iterator it = l.begin ();
-            it != l.end(); ++it) {
-          constraints.first->addObject (ConvexShape (it->second, it->first));
+        for (JointAndShapes_t::const_iterator it(l.begin()); it!=l.end();++it)
+        {
+          objectSurfaces.push_back(*it);
         }
       }
 
@@ -274,22 +265,23 @@ namespace hpp {
         else if (jointAndShapes.has (*it2))
           l = jointAndShapes.get (*it2);
         else throw std::runtime_error ("Second list of triangles not found.");
-        for (JointAndShapes_t::const_iterator it = l.begin ();
-            it != l.end(); ++it) {
-          constraints.first->addFloor (ConvexShape (it->second, it->first));
+        for (JointAndShapes_t::const_iterator it(l.begin()); it!=l.end();++it)
+        {
+          floorSurfaces.push_back(*it);
         }
       }
 
-      constraints.first->setNormalMargin (margin);
-
-      addNumericalConstraint (name, Implicit::create
-			      (constraints.first));
-      addNumericalConstraint (complementName, Implicit::create
-			      (constraints.second,
-                               ComparisonTypes_t
-                               (constraints.second->outputSize(),
-                                constraints::Equality))
-                              );
+      typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t;
+      Constraint_t::Constraints_t constraints
+        (Constraint_t::createConstraintAndComplement
+         (name, robot_, floorSurfaces, objectSurfaces, margin));
+
+      addNumericalConstraint(std::get<0>(constraints)->function().name(),
+                             std::get<0>(constraints));
+      addNumericalConstraint(std::get<1>(constraints)->function().name(),
+                             std::get<1>(constraints));
+      addNumericalConstraint(std::get<2>(constraints)->function().name(),
+                             std::get<2>(constraints));
     }
 
     void ProblemSolver::createPrePlacementConstraint
@@ -298,22 +290,15 @@ namespace hpp {
      const value_type& margin)
     {
       if (!robot_) throw std::runtime_error ("No robot loaded");
-      using constraints::ConvexShape;
-      using constraints::ConvexShapeContact;
-      using constraints::ConvexShapeContactPtr_t;
-
-      ConvexShapeContactPtr_t cvxShape = ConvexShapeContact::create (name, robot_);
-
-      JointAndShapes_t l;
+      JointAndShapes_t floorSurfaces, objectSurfaces, l;
       for (StringList_t::const_iterator it1 = surface1.begin ();
           it1 != surface1.end(); ++it1) {
         if (!robot_->jointAndShapes.has (*it1))
           throw std::runtime_error ("First list of triangles not found.");
         l = robot_->jointAndShapes.get (*it1);
-
-        for (JointAndShapes_t::const_iterator it = l.begin ();
-            it != l.end(); ++it) {
-          cvxShape->addObject (ConvexShape (it->second, it->first));
+        for (JointAndShapes_t::const_iterator it(l.begin()); it!=l.end();++it)
+        {
+          objectSurfaces.push_back(*it);
         }
       }
 
@@ -326,15 +311,16 @@ namespace hpp {
         else if (jointAndShapes.has (*it2))
           l = jointAndShapes.get (*it2);
         else throw std::runtime_error ("Second list of triangles not found.");
-
-        for (JointAndShapes_t::const_iterator it = l.begin ();
-            it != l.end(); ++it) {
-          cvxShape->addFloor (ConvexShape (it->second, it->first));
+        for (JointAndShapes_t::const_iterator it(l.begin()); it!=l.end();++it)
+        {
+          floorSurfaces.push_back(*it);
         }
       }
 
-      cvxShape->setNormalMargin (margin + width);
-
+      hpp::constraints::ConvexShapeContactPtr_t cvxShape
+        (hpp::constraints::ConvexShapeContact::create
+         (name, robot_, floorSurfaces, objectSurfaces));
+      cvxShape->setNormalMargin(margin + width);
       addNumericalConstraint (name, Implicit::create (cvxShape));
     }