Made changes according to the discussion in PR #668

dartsim · Apr 15, 2016 · 5d0d9ff · 5d0d9ff
1 parent d22b556
commit 5d0d9ff
Show file tree

Hide file tree

Showing 11 changed files with 198 additions and 25 deletions.
diff --git a/dart/collision/Contact.cpp b/dart/collision/Contact.cpp
@@ -3,6 +3,7 @@
  * All rights reserved.
  *
  * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *            Michael X. Grey <mxgrey@gatech.edu>
  *
  * Georgia Tech Graphics Lab and Humanoid Robotics Lab
  *
@@ -39,5 +40,21 @@
 namespace dart {
 namespace collision {
 
+//==============================================================================
+Contact::Contact()
+  : point(Eigen::Vector3d::Zero()),
+    normal(Eigen::Vector3d::Zero()),
+    force(Eigen::Vector3d::Zero()),
+    collisionObject1(nullptr),
+    collisionObject2(nullptr),
+    penetrationDepth(0),
+    triID1(0),
+    triID2(0),
+    userData(nullptr)
+{
+  // TODO(MXG): Consider using NaN instead of zero for uninitialized quantities
+  // Do nothing
+}
+
 }  // namespace collision
 }  // namespace dart
diff --git a/dart/collision/Contact.h b/dart/collision/Contact.h
@@ -47,6 +47,9 @@ namespace collision {
 /// Contact information
 struct Contact
 {
+  /// Default constructor
+  Contact();
+
   /// Contact point w.r.t. the world frame
   Eigen::Vector3d point;
 

diff --git a/dart/collision/Result.cpp b/dart/collision/Result.cpp
@@ -35,6 +35,11 @@
  */
 
 #include "dart/collision/Result.h"
+#include "dart/collision/CollisionObject.h"
+
+#include "dart/dynamics/ShapeFrame.h"
+#include "dart/dynamics/ShapeNode.h"
+#include "dart/dynamics/BodyNode.h"
 
 namespace dart {
 namespace collision {
@@ -43,6 +48,8 @@ namespace collision {
 void CollisionResult::addContact(const Contact& contact)
 {
   mContacts.push_back(contact);
+  addObject(contact.collisionObject1);
+  addObject(contact.collisionObject2);
 }
 
 //==============================================================================
@@ -73,16 +80,74 @@ const std::vector<Contact>& CollisionResult::getContacts() const
   return mContacts;
 }
 
+//==============================================================================
+const std::unordered_set<const dynamics::BodyNode*>
+CollisionResult::getCollidingBodyNodes() const
+{
+  return mCollidingBodyNodes;
+}
+
+//==============================================================================
+const std::unordered_set<const dynamics::ShapeFrame*>
+CollisionResult::getCollidingShapeFrames() const
+{
+  return mCollidingShapeFrames;
+}
+
+//==============================================================================
+bool CollisionResult::inCollision(const dynamics::BodyNode* bn) const
+{
+  return (mCollidingBodyNodes.find(bn) != mCollidingBodyNodes.end());
+}
+
+//==============================================================================
+bool CollisionResult::inCollision(const dynamics::ShapeFrame* frame) const
+{
+  return (mCollidingShapeFrames.find(frame) != mCollidingShapeFrames.end());
+}
+
 //==============================================================================
 bool CollisionResult::isCollision() const
 {
   return !mContacts.empty();
 }
 
+//==============================================================================
+CollisionResult::operator bool() const
+{
+  return isCollision();
+}
+
 //==============================================================================
 void CollisionResult::clear()
 {
   mContacts.clear();
+  mCollidingShapeFrames.clear();
+  mCollidingBodyNodes.clear();
+}
+
+//==============================================================================
+void CollisionResult::addObject(CollisionObject* object)
+{
+  if(!object)
+  {
+    dterr << "[CollisionResult::addObject] Attempting to add a collision with "
+          << "a nullptr object to a CollisionResult instance. This is not "
+          << "allowed. Please report this as a bug!";
+    assert(false);
+    return;
+  }
+
+  const dynamics::ShapeFrame* frame = object->getShapeFrame();
+  mCollidingShapeFrames.insert(frame);
+
+  if(frame->isShapeNode())
+  {
+    const dynamics::ShapeNode* node =
+        dynamic_cast<const dynamics::ShapeNode*>(frame);
+
+    mCollidingBodyNodes.insert(node->getBodyNodePtr());
+  }
 }
 
 }  // namespace collision

diff --git a/dart/collision/Result.h b/dart/collision/Result.h
@@ -38,9 +38,18 @@
 #define DART_COLLISION_RESULT_H_
 
 #include <vector>
+#include <unordered_set>
 #include "dart/collision/Contact.h"
 
 namespace dart {
+
+namespace dynamics {
+
+class BodyNode;
+class ShapeFrame;
+
+} // namespace dynamics
+
 namespace collision {
 
 class CollisionResult
@@ -62,17 +71,42 @@ class CollisionResult
   /// Return contacts
   const std::vector<Contact>& getContacts() const;
 
+  /// Return the set of BodyNodes that are in collision
+  const std::unordered_set<const dynamics::BodyNode*>
+  getCollidingBodyNodes() const;
+
+  /// Return the set of ShapeFrames that are in collision
+  const std::unordered_set<const dynamics::ShapeFrame*>
+  getCollidingShapeFrames() const;
+
+  /// Returns true if the given BodyNode is in collision
+  bool inCollision(const dynamics::BodyNode* bn) const;
+
+  /// Returns true if the given ShapeFrame is in collision
+  bool inCollision(const dynamics::ShapeFrame* frame) const;
+
   /// Return binary collision result
   bool isCollision() const;
 
+  /// Implicitly converts this CollisionResult to the value of isCollision()
+  operator bool() const;
+
   /// Clear all the contacts
   void clear();
 
 protected:
 
+  void addObject(CollisionObject* object);
+
   /// List of contact information for each contact
   std::vector<Contact> mContacts;
 
+  /// Set of BodyNodes that are colliding
+  std::unordered_set<const dynamics::BodyNode*> mCollidingBodyNodes;
+
+  /// Set of ShapeFrames that are colliding
+  std::unordered_set<const dynamics::ShapeFrame*> mCollidingShapeFrames;
+
 };
 
 }  // namespace collision