Distance API

CHANGELOG.md

@@ -5,6 +5,7 @@
 * Collision detection
 
   * Fixed direction of contact normal of BulletCollisionDetector: [#763](https://github.com/dartsim/dart/pull/763)
+  * Added distance API: [#744](https://github.com/dartsim/dart/pull/744)
 
 * Dynamics
 
@@ -23,8 +24,8 @@
 
   * Fixed Doxyfile to show missing member functions in API documentation: [#768](https://github.com/dartsim/dart/pull/768)
   * Made building with SIMD optional: [#765](https://github.com/dartsim/dart/pull/765), [#760](https://github.com/dartsim/dart/pull/760)
-  * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)
   * Fixed typo: [#756](https://github.com/dartsim/dart/pull/756), [#755](https://github.com/dartsim/dart/pull/755)
+  * Added `virtual Shape::getType()` and deprecated `ShapeType Shape::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)
 
 ### DART 6.0.1 (2016-XX-XX)
 

dart/collision/CMakeLists.txt

@@ -16,6 +16,8 @@ set(dart_collision_srcs ${srcs} "${dart_collision_srcs};${detail_srcs}" PARENT_S
 
 # Generate header for this namespace
 dart_get_filename_components(header_names "collision headers" ${hdrs})
+# TODO: remove below line once the files are completely removed.
+list(REMOVE_ITEM header_names "Option.hpp" "Result.hpp")
 set(
   header_names
   ${header_names}

dart/collision/CollisionDetector.hpp

@@ -38,8 +38,10 @@
 #include <Eigen/Dense>
 
 #include "dart/collision/Contact.hpp"
-#include "dart/collision/Option.hpp"
-#include "dart/collision/Result.hpp"
+#include "dart/collision/CollisionOption.hpp"
+#include "dart/collision/CollisionResult.hpp"
+#include "dart/collision/DistanceOption.hpp"
+#include "dart/collision/DistanceResult.hpp"
 #include "dart/collision/SmartPointer.hpp"
 #include "dart/dynamics/SmartPointer.hpp"
 
@@ -109,6 +111,37 @@ class CollisionDetector : public std::enable_shared_from_this<CollisionDetector>
       const CollisionOption& option = CollisionOption(false, 1u, nullptr),
       CollisionResult* result = nullptr) = 0;
 
+  /// Get the minimum signed distance between the Shape pairs in the given
+  /// CollisionGroup.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  virtual double distance(
+      CollisionGroup* group,
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr) = 0;
+
+  /// Get the minimum signed distance between the Shape pairs where a pair
+  /// consist of two shapes from each groups (one from group1 and one from
+  /// group2).
+  ///
+  /// Note that the distance between shapes within the same CollisionGroup
+  /// are not accounted.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  virtual double distance(
+      CollisionGroup* group1,
+      CollisionGroup* group2,
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr) = 0;
+
 protected:
 
   class CollisionObjectManager;

dart/collision/CollisionGroup.cpp

@@ -169,11 +169,27 @@ bool CollisionGroup::collide(
 
 //==============================================================================
 bool CollisionGroup::collide(
-    CollisionGroup* other,
+    CollisionGroup* otherGroup,
     const CollisionOption& option,
     CollisionResult* result)
 {
-  return mCollisionDetector->collide(this, other, option, result);
+  return mCollisionDetector->collide(this, otherGroup, option, result);
+}
+
+//==============================================================================
+double CollisionGroup::distance(
+    const DistanceOption& option, DistanceResult* result)
+{
+  return mCollisionDetector->distance(this, option, result);
+}
+
+//==============================================================================
+double CollisionGroup::distance(
+    CollisionGroup* otherGroup,
+    const DistanceOption& option,
+    DistanceResult* result)
+{
+  return mCollisionDetector->distance(this, otherGroup, option, result);
 }
 
 //==============================================================================

dart/collision/CollisionGroup.hpp

@@ -35,8 +35,10 @@
 #include <map>
 #include <vector>
 #include "dart/collision/SmartPointer.hpp"
-#include "dart/collision/Option.hpp"
-#include "dart/collision/Result.hpp"
+#include "dart/collision/CollisionOption.hpp"
+#include "dart/collision/CollisionResult.hpp"
+#include "dart/collision/DistanceOption.hpp"
+#include "dart/collision/DistanceResult.hpp"
 #include "dart/dynamics/SmartPointer.hpp"
 
 namespace dart {
@@ -176,10 +178,39 @@ class CollisionGroup
   /// Return false if the engine type of the other CollisionGroup is different
   /// from this CollisionObject engine.
   bool collide(
-      CollisionGroup* group,
+      CollisionGroup* otherGroup,
       const CollisionOption& option = CollisionOption(false, 1u, nullptr),
       CollisionResult* result = nullptr);
 
+  /// Get the minimum signed distance between the Shape pairs in this
+  /// CollisionGroup.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  double distance(
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr);
+
+  /// Get the minimum signed distance between the Shape pairs where a pair
+  /// consist of two shapes from each groups (one from this CollisionGroup and
+  /// one from otherGroup).
+  ///
+  /// Note that the distance between shapes within the same CollisionGroup
+  /// are not accounted.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  double distance(
+      CollisionGroup* otherGroup,
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr);
+
 protected:
 
   /// Update engine data. This function should be called before the collision

dart/collision/Option.cpp → dart/collision/CollisionOption.cpp

@@ -29,7 +29,7 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "dart/collision/Option.hpp"
+#include "dart/collision/CollisionOption.hpp"
 
 namespace dart {
 namespace collision {

dart/collision/CollisionOption.hpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_COLLISIONOPTION_HPP_
+#define DART_COLLISION_COLLISIONOPTION_HPP_
+
+#include <cstddef>
+#include <memory>
+
+namespace dart {
+namespace collision {
+
+class CollisionFilter;
+
+struct CollisionOption
+{
+
+  /// Flag whether the collision detector computes contact information (contact
+  /// point, normal, and penetration depth). If it is set to false, only the
+  /// result of that which pairs are colliding will be stored in the
+  /// CollisionResult without the contact information.
+  bool enableContact;
+
+  /// Maximum number of contacts to detect. Once the contacts are found up to
+  /// this number, the collision checking will terminate at that moment. Set
+  /// this to 1 for binary check.
+  std::size_t maxNumContacts;
+
+  /// CollisionFilter
+  std::shared_ptr<CollisionFilter> collisionFilter;
+
+  /// Constructor
+  CollisionOption(
+      bool enableContact = true,
+      std::size_t maxNumContacts = 1000u,
+      const std::shared_ptr<CollisionFilter>& collisionFilter = nullptr);
+
+};
+
+}  // namespace collision
+}  // namespace dart
+
+#endif  // DART_COLLISION_COLLISIONOPTION_HPP_

dart/collision/Result.cpp → dart/collision/CollisionResult.cpp

@@ -29,9 +29,9 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "dart/collision/Result.hpp"
-#include "dart/collision/CollisionObject.hpp"
+#include "dart/collision/CollisionResult.hpp"
 
+#include "dart/collision/CollisionObject.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
 #include "dart/dynamics/ShapeNode.hpp"
 #include "dart/dynamics/BodyNode.hpp"

dart/collision/CollisionResult.hpp

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_COLLISIONRESULT_HPP_
+#define DART_COLLISION_COLLISIONRESULT_HPP_
+
+#include <vector>
+#include <unordered_set>
+#include "dart/collision/Contact.hpp"
+
+namespace dart {
+
+namespace dynamics {
+
+class BodyNode;
+class ShapeFrame;
+
+} // namespace dynamics
+
+namespace collision {
+
+class CollisionResult
+{
+public:
+
+  /// Add one contact
+  void addContact(const Contact& contact);
+
+  /// Return number of contacts
+  std::size_t getNumContacts() const;
+
+  /// Return the index-th contact
+  Contact& getContact(std::size_t index);
+
+  /// Return (const) the index-th contact
+  const Contact& getContact(std::size_t index) const;
+
+  /// 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
+}  // namespace dart
+
+#endif  // DART_COLLISION_COLLISIONRESULT_HPP_