Merge branch 'main' into issue/452_aiscene_custom_deleter

Author: jslee02

cmake/DARTFindDependencies.cmake

@@ -46,10 +46,10 @@ if(ASSIMP_FOUND)
       return 1;
     }
     "
-    ASSIMP_AISCENE_CTOR_DTOR_DEFINED
+    DART_ASSIMP_AISCENE_CTOR_DTOR_DEFINED
   )
 
-  if(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED)
+  if(NOT DART_ASSIMP_AISCENE_CTOR_DTOR_DEFINED)
     if(DART_VERBOSE)
       message(
         WARNING
@@ -72,10 +72,10 @@ if(ASSIMP_FOUND)
       return 1;
     }
     "
-    ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED
+    DART_ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED
   )
 
-  if(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED)
+  if(NOT DART_ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED)
     if(DART_VERBOSE)
       message(
         WARNING
@@ -97,18 +97,18 @@ endif()
 dart_find_package(octomap)
 if(OCTOMAP_FOUND OR octomap_FOUND)
   if(NOT DEFINED octomap_VERSION)
-    set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+    set(DART_HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
     message(WARNING "Looking for octomap - octomap_VERSION is not defined, "
         "please install octomap with version information"
     )
   else()
-    set(HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE)
+    set(DART_HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE)
     if(DART_VERBOSE)
       message(STATUS "Looking for octomap - version ${octomap_VERSION} found")
     endif()
   endif()
 else()
-  set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+  set(DART_HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
   message(WARNING "Looking for octomap - NOT found, to use VoxelGridShape, "
       "please install octomap"
   )

cmake/dart_defs.cmake

@@ -1391,12 +1391,14 @@ macro(dart_check_optional_package variable component dependency)
   mark_as_advanced(DART_SKIP_${variable})
   if(${${variable}_FOUND} AND NOT ${DART_SKIP_${variable}})
     set(HAVE_${variable} TRUE CACHE BOOL "Check if ${variable} found." FORCE)
+    set(DART_HAVE_${variable} TRUE CACHE BOOL "Check if ${variable} found." FORCE)
     if(DART_VERBOSE)
       message(STATUS "Looking for ${dependency} - version ${${variable}_VERSION}"
                      " found")
     endif()
   else()
     set(HAVE_${variable} FALSE CACHE BOOL "Check if ${variable} found." FORCE)
+    set(DART_HAVE_${variable} FALSE CACHE BOOL "Check if ${variable} found." FORCE)
     if(NOT ${${variable}_FOUND})
       if(ARGV3) # version
         message(WARNING "Looking for ${dependency} - NOT found, to use"

dart/collision/CollisionGroup.cpp

@@ -39,6 +39,7 @@
 #include "dart/dynamics/Skeleton.hpp"
 
 #include <cassert>
+#include <cstdint>
 
 namespace dart {
 namespace collision {
@@ -321,6 +322,31 @@ void CollisionGroup::removeDeletedShapeFrames()
   mObserver.mDeletedFrames.clear();
 }
 
+//==============================================================================
+std::size_t CollisionGroup::computeMetaSkeletonVersion(
+    const dynamics::MetaSkeleton& metaSkeleton)
+{
+  if (const auto* skeleton
+      = dynamic_cast<const dynamics::Skeleton*>(&metaSkeleton)) {
+    return skeleton->getVersion();
+  }
+
+  const std::size_t numBodies = metaSkeleton.getNumBodyNodes();
+  std::size_t seed = numBodies;
+
+  for (std::size_t i = 0u; i < numBodies; ++i) {
+    const auto* bodyNode = metaSkeleton.getBodyNode(i);
+    const auto bodyNodeHash
+        = static_cast<std::size_t>(reinterpret_cast<std::uintptr_t>(bodyNode));
+    const auto bodyNodeVersion = bodyNode ? bodyNode->getVersion() : 0u;
+
+    seed ^= bodyNodeHash + 0x9e3779b97f4a7c15ULL + (seed << 6) + (seed >> 2);
+    seed ^= bodyNodeVersion + 0x9e3779b97f4a7c15ULL + (seed << 6) + (seed >> 2);
+  }
+
+  return seed;
+}
+
 //==============================================================================
 void CollisionGroup::updateEngineData()
 {
@@ -449,18 +475,12 @@ bool CollisionGroup::updateSkeletonSource(SkeletonSources::value_type& entry)
     // This skeleton no longer exists, so we should remove all its contents from
     // the CollisionGroup.
     for (const auto& object : source.mObjects)
-      removeShapeFrameInternal(object.second->mFrame, object.first);
+      removeShapeFrameInternal(object.second->mFrame, entry.first);
 
     return true;
   }
 
-  // Note: Right now we can only subscribe to Skeletons, not MetaSkeletons, so
-  // we can safely do a static_cast here. Eventually this static_cast should not
-  // be needed, if we can figure out a versioning system for MetaSkeleton.
-  const dynamics::Skeleton* skeleton
-      = dynamic_cast<const dynamics::Skeleton*>(meta.get());
-
-  const std::size_t currentSkeletonVersion = skeleton->getVersion();
+  const std::size_t currentSkeletonVersion = computeMetaSkeletonVersion(*meta);
   // If the version hasn't changed, then there will be nothing to update.
   if (currentSkeletonVersion == source.mLastKnownVersion)
     return false;
@@ -473,8 +493,8 @@ bool CollisionGroup::updateSkeletonSource(SkeletonSources::value_type& entry)
   // Check each child to see if its version number has changed. If it has, then
   // check to see if any of its ShapeFrames need to be updated, added, or
   // removed from the CollisionGroup.
-  for (std::size_t i = 0; i < skeleton->getNumBodyNodes(); ++i) {
-    const dynamics::BodyNode* bn = skeleton->getBodyNode(i);
+  for (std::size_t i = 0; i < meta->getNumBodyNodes(); ++i) {
+    const dynamics::BodyNode* bn = meta->getBodyNode(i);
     const std::size_t currentVersion = bn->getVersion();
 
     unusedChildren.erase(bn);
@@ -541,7 +561,8 @@ bool CollisionGroup::updateSkeletonSource(SkeletonSources::value_type& entry)
     }
   }
 
-  // Remove from this group any BodyNodes that no longer belong to the skeleton
+  // Remove from this group any BodyNodes that no longer belong to the meta
+  // skeleton
   for (const auto& unusedChild : unusedChildren) {
     for (const dynamics::ShapeFrame* unusedFrame : unusedChild.second.mFrames) {
       updateNeeded = true;

dart/collision/CollisionGroup.hpp

@@ -133,18 +133,15 @@ class DART_API CollisionGroup
   void subscribeTo(
       const dynamics::ConstBodyNodePtr& bodyNode, const Others&... others);
 
-  /// Add ShapeFrames of skeleton, and also subscribe to the Skeleton so that
-  /// the results from this CollisionGroup automatically reflect any changes
-  /// that are made to skeleton.
+  /// Add ShapeFrames of metaSkeleton, and also subscribe so that the results
+  /// from this CollisionGroup automatically reflect any changes that are made
+  /// to metaSkeleton.
   ///
   /// This does likewise for the objects in ...others.
-  //
-  // TODO(MXG): Figure out how to determine a version number for MetaSkeletons
-  // so that this function can accept a ConstMetaSkeletonPtr instead of only a
-  // ConstSkeletonPtr.
   template <typename... Others>
   void subscribeTo(
-      const dynamics::ConstSkeletonPtr& skeleton, const Others&... others);
+      const dynamics::ConstMetaSkeletonPtr& metaSkeleton,
+      const Others&... others);
 
   /// Do nothing. This function is for terminating the recursive variadic
   /// template.
@@ -222,7 +219,7 @@ class DART_API CollisionGroup
   /// removed if no other source is requesting them for this group.
   template <typename... Others>
   void unsubscribeFrom(
-      const dynamics::Skeleton* skeleton, const Others*... others);
+      const dynamics::MetaSkeleton* skeleton, const Others*... others);
 
   /// Do nothing. This function is for terminating the recursive variadic
   /// template.
@@ -236,7 +233,7 @@ class DART_API CollisionGroup
   /// Check if this is subscribed to skeleton and the other sources
   template <typename... Others>
   bool isSubscribedTo(
-      const dynamics::Skeleton* skeleton, const Others*... others);
+      const dynamics::MetaSkeleton* skeleton, const Others*... others);
 
   /// Return true. This function is for terminating the recursive variadic
   /// template
@@ -471,6 +468,11 @@ class DART_API CollisionGroup
   friend class ShapeFrameObserver;
   void handleShapeFrameDestruction(const dynamics::ShapeFrame* shapeFrame);
 
+  /// Compute a version tag for MetaSkeletons even when they do not expose a
+  /// VersionCounter interface.
+  static std::size_t computeMetaSkeletonVersion(
+      const dynamics::MetaSkeleton& metaSkeleton);
+
   /// Set this to true to have this CollisionGroup check for updates
   /// automatically. Default is true.
   bool mUpdateAutomatically;

dart/collision/RaycastOption.cpp

@@ -36,11 +36,23 @@ namespace dart {
 namespace collision {
 
 //==============================================================================
-RaycastOption::RaycastOption(bool enableAllHits, bool sortByClosest)
-  : mEnableAllHits(enableAllHits), mSortByClosest(sortByClosest)
+RaycastOption::RaycastOption(
+    bool enableAllHits, bool sortByClosest, RaycastFilter filter)
+  : mEnableAllHits(enableAllHits),
+    mSortByClosest(sortByClosest),
+    mFilter(std::move(filter))
 {
   // Do nothing
 }
 
+//==============================================================================
+bool RaycastOption::passesFilter(const CollisionObject* object) const
+{
+  if (!mFilter)
+    return true;
+
+  return mFilter(object);
+}
+
 } // namespace collision
 } // namespace dart

dart/collision/RaycastOption.hpp

@@ -35,23 +35,35 @@
 
 #include <dart/Export.hpp>
 
+#include <functional>
 #include <memory>
 
 #include <cstddef>
 
 namespace dart {
 namespace collision {
 
+class CollisionObject;
+
 struct DART_API RaycastOption
 {
+  using RaycastFilter = std::function<bool(const CollisionObject*)>;
+
   /// Constructor
-  RaycastOption(bool enableAllHits = false, bool sortByClosest = false);
+  RaycastOption(
+      bool enableAllHits = false,
+      bool sortByClosest = false,
+      RaycastFilter filter = nullptr);
+
+  /// Returns true when the filter is not set or allows the object.
+  bool passesFilter(const CollisionObject* object) const;
 
   bool mEnableAllHits;
 
   bool mSortByClosest;
 
-  // TODO(JS): Add filter
+  /// Optional filter to reject hits from specific collision objects.
+  RaycastFilter mFilter;
 };
 
 } // namespace collision