Merge pull request #2057 from mwoehlke-kitware/split-result-collector

Separate ResultCollector base
RobotLocomotion · Apr 12, 2016 · b06552b · b06552b
2 parents 2a0a91a + 6c0eee2
commit b06552b
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 42 deletions.
diff --git a/drake/systems/plants/collision/BulletResultCollector.cpp b/drake/systems/plants/collision/BulletResultCollector.cpp
@@ -1,31 +1,16 @@
-#include <iostream>
-#include <btBulletCollisionCommon.h>
+#include "drake/systems/plants/collision/BulletResultCollector.h"
 
 #include "drake/systems/plants/collision/DrakeCollision.h"
-#include "BulletResultCollector.h"
-
-using namespace std;
-using namespace Eigen;
 
 namespace DrakeCollision {
-Vector3d toVector3d(const Vector3d& vec) { return vec; }
-
-Vector3d toVector3d(const btVector3& bt_vec) {
-  Vector3d vec;
+Eigen::Vector3d toVector3d(const btVector3& bt_vec) {
+  Eigen::Vector3d vec;
   vec(0) = (double)bt_vec.getX();
   vec(1) = (double)bt_vec.getY();
   vec(2) = (double)bt_vec.getZ();
   return vec;
 }
 
-void ResultCollector::addPointPairResult(const PointPair& result) {
-  pts.push_back(result);
-}
-
-PointPair ResultCollector::minDistPoint() {
-  return *min_element(pts.begin(), pts.end());
-}
-
 btScalar BulletResultCollector::addSingleResult(
     btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,
     int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap,

diff --git a/drake/systems/plants/collision/BulletResultCollector.h b/drake/systems/plants/collision/BulletResultCollector.h
@@ -2,31 +2,13 @@
 #define DRAKE_SYSTEMS_PLANTS_COLLISION_BULLETRESULTCOLLECTOR_H_
 
 #include "drake/systems/plants/collision/DrakeCollision.h"
+#include "drake/systems/plants/collision/ResultCollector.h"
+
+#include <btBulletCollisionCommon.h>
 
 namespace DrakeCollision {
-Eigen::Vector3d toVector3d(const Eigen::Vector3d& vec);
 Eigen::Vector3d toVector3d(const btVector3& bt_vec);
 
-class ResultCollector {
- public:
-  virtual ~ResultCollector() {}
-
-  virtual void addPointPairResult(const PointPair& result);
-
-  inline void addSingleResult(const ElementId idA, const ElementId idB,
-                              const Eigen::Vector3d& ptA,
-                              const Eigen::Vector3d& ptB,
-                              const Eigen::Vector3d& normal, double distance) {
-    addPointPairResult(PointPair(idA, idB, ptA, ptB, normal, distance));
-  }
-
-  std::vector<PointPair> getResults() const { return pts; }
-
-  PointPair minDistPoint();
-
-  std::vector<PointPair> pts;
-};
-
 class BulletResultCollector : public ResultCollector,
                               public btCollisionWorld::ContactResultCallback {
  public:
@@ -50,8 +32,6 @@ class BulletResultCollector : public ResultCollector,
   int curr_bodyA_idx;
   int curr_bodyB_idx;
 };
-
-typedef std::shared_ptr<ResultCollector> ResultCollShPtr;
 }
 
 #endif  // DRAKE_SYSTEMS_PLANTS_COLLISION_BULLETRESULTCOLLECTOR_H_
diff --git a/drake/systems/plants/collision/CMakeLists.txt b/drake/systems/plants/collision/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(drakeCollision_SRC_FILES DrakeCollision.cpp Element.cpp point_pair.cc Model.cpp)
+set(drakeCollision_SRC_FILES DrakeCollision.cpp Element.cpp point_pair.cc Model.cpp ResultCollector.cpp)
 pods_find_pkg_config(bullet)
 
 if (bullet_FOUND)

diff --git a/drake/systems/plants/collision/ResultCollector.cpp b/drake/systems/plants/collision/ResultCollector.cpp
@@ -0,0 +1,13 @@
+#include "drake/systems/plants/collision/ResultCollector.h"
+
+#include "drake/systems/plants/collision/DrakeCollision.h"
+
+namespace DrakeCollision {
+void ResultCollector::addPointPairResult(const PointPair& result) {
+  pts.push_back(result);
+}
+
+PointPair ResultCollector::minDistPoint() const {
+  return *std::min_element(pts.begin(), pts.end());
+}
+}
diff --git a/drake/systems/plants/collision/ResultCollector.h b/drake/systems/plants/collision/ResultCollector.h
@@ -0,0 +1,31 @@
+#ifndef DRAKE_SYSTEMS_PLANTS_COLLISION_RESULTCOLLECTOR_H_
+#define DRAKE_SYSTEMS_PLANTS_COLLISION_RESULTCOLLECTOR_H_
+
+#include "drake/systems/plants/collision/DrakeCollision.h"
+
+namespace DrakeCollision {
+class ResultCollector {
+ public:
+  virtual ~ResultCollector() {}
+
+  virtual void addPointPairResult(const PointPair& result);
+
+  inline void addSingleResult(const ElementId idA, const ElementId idB,
+                              const Eigen::Vector3d& ptA,
+                              const Eigen::Vector3d& ptB,
+                              const Eigen::Vector3d& normal,
+                              const double distance) {
+    addPointPairResult(PointPair(idA, idB, ptA, ptB, normal, distance));
+  }
+
+  std::vector<PointPair> getResults() const { return pts; }
+
+  PointPair minDistPoint() const;
+
+  std::vector<PointPair> pts;
+};
+
+typedef std::shared_ptr<ResultCollector> ResultCollShPtr;
+}
+
+#endif  // DRAKE_SYSTEMS_PLANTS_COLLISION_RESULTCOLLECTOR_H_