From c227537c6afe13357fdeb6bdf29f4a85eed15943 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 4 Nov 2014 15:05:21 +0100
Subject: [PATCH] Add contact triangles to Object and ProblemSolver

---
 include/hpp/manipulation/fwd.hh            |  8 ++++++++
 include/hpp/manipulation/object.hh         | 16 ++++++++++++++++
 include/hpp/manipulation/problem-solver.hh |  7 +++++++
 src/problem-solver.cc                      | 15 +++++++++++++++
 4 files changed, 46 insertions(+)

diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index ba664e6..503966d 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/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
 
diff --git a/include/hpp/manipulation/object.hh b/include/hpp/manipulation/object.hh
index f5e420e..24fe19c 100644
--- a/include/hpp/manipulation/object.hh
+++ b/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
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 423b64a..01e0b3d 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/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
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 194c5cf..49a99be 100644
--- a/src/problem-solver.cc
+++ b/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
-- 
GitLab