Add contact triangles to Object and ProblemSolver

include/hpp/manipulation/fwd.hh

@@ -23,6 +23,10 @@
 # include <map>
 # include <hpp/constraints/fwd.hh>
 
+namespace fcl {
+  class TriangleP;
+}
+
 namespace hpp {
   namespace manipulation {
     typedef model::Device Device;
@@ -92,6 +96,10 @@ namespace hpp {
     typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
     typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
     typedef std::map <std::string, LockedDofPtr_t> LockedDofConstraintMap_t;
+
+    typedef fcl::TriangleP Triangle;
+    typedef std::list <Triangle> TriangleList;
+    typedef std::map <std::string, TriangleList> TriangleMap;
   } // namespace manipulation
 } // namespace hpp
 

include/hpp/manipulation/object.hh

@@ -49,6 +49,21 @@ namespace hpp {
 	return shPtr;
       }
 
+      void addContactTriangle (std::string name, const TriangleList& t)
+      {
+        contacts_[name] = t;
+      }
+
+      TriangleList contactTriangles (std::string name)
+      {
+        return contacts_ [name];
+      }
+
+      const TriangleMap& contactTriangles () const
+      {
+        return contacts_;
+      }
+
       /// \name Object handles
       /// \{
 
@@ -90,6 +105,7 @@ namespace hpp {
     private:
       Handles_t handles_;
       ObjectWkPtr_t weakPtr_;
+      TriangleMap contacts_;
     }; // class Object
   } // namespace manipulation
 } // namespace hpp

include/hpp/manipulation/problem-solver.hh

@@ -161,6 +161,12 @@ namespace hpp {
 	return problem_;
       }
 
+      void addContactTriangles (const std::string name, const TriangleList triangles);
+
+      TriangleList contactTriangles (const std::string name);
+
+      const TriangleMap& contactTriangles () const;
+
     protected:
       void initializeProblem (ProblemPtr_t problem);
 
@@ -174,6 +180,7 @@ namespace hpp {
       RobotsandObjects_t robotsAndObjects_;
       GraspsMap_t graspsMap_;
       LockedDofConstraintMap_t lockedDofConstraintMap_;
+      TriangleMap contactTriangles_;
     }; // class ProblemSolver
   } // namespace manipulation
 } // namespace hpp

src/problem-solver.cc

@@ -179,5 +179,20 @@ namespace hpp {
       if (constraintGraph_) r->constraintGraph (constraintGraph_);
       roadmap (r);
     }
+
+    void ProblemSolver::addContactTriangles (const std::string name, const TriangleList triangles)
+    {
+      contactTriangles_ [name] = triangles;
+    }
+
+    TriangleList ProblemSolver::contactTriangles (const std::string name)
+    {
+      return contactTriangles_ [name];
+    }
+
+    const TriangleMap& ProblemSolver::contactTriangles () const
+    {
+      return contactTriangles_;
+    }
   } // namespace manipulation
 } // namespace hpp