dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

include/dart/collision/detail/CollisionDetector.hpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * 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_DETAIL_COLLISIONDETECTOR_HPP_
+#define DART_COLLISION_DETAIL_COLLISIONDETECTOR_HPP_
+
+#include <dart/collision/CollisionDetector.hpp>
+#include <dart/collision/CollisionGroup.hpp>
+
+namespace dart {
+namespace collision {
+
+//==============================================================================
+template <typename... Args>
+std::unique_ptr<CollisionGroup> CollisionDetector::createCollisionGroup(
+    const Args&... args)
+{
+  auto group = createCollisionGroup();
+
+  group->addShapeFramesOf(args...);
+
+  return group;
+}
+
+//==============================================================================
+template <typename... Args>
+std::shared_ptr<CollisionGroup>
+CollisionDetector::createCollisionGroupAsSharedPtr(const Args&... args)
+{
+  return std::shared_ptr<CollisionGroup>(
+      createCollisionGroup(args...).release());
+}
+
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_COLLISIONDETECTOR_HPP_

include/dart/collision/detail/CollisionGroup.hpp

@@ -0,0 +1,287 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * 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_DETAIL_COLLISIONGROUP_HPP_
+#define DART_COLLISION_DETAIL_COLLISIONGROUP_HPP_
+
+#include <dart/collision/CollisionGroup.hpp>
+
+#include <dart/dynamics/BodyNode.hpp>
+#include <dart/dynamics/Skeleton.hpp>
+
+namespace dart {
+namespace collision {
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::addShapeFramesOf(
+    const dynamics::ShapeFrame* shapeFrame, const Others*... others)
+{
+  addShapeFrame(shapeFrame);
+
+  addShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::addShapeFramesOf(
+    const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
+    const Others*... others)
+{
+  addShapeFrames(shapeFrames);
+
+  addShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::addShapeFramesOf(
+    const CollisionGroup* otherGroup, const Others*... others)
+{
+  assert(otherGroup);
+
+  if (otherGroup && this != otherGroup) {
+    for (const auto& info : otherGroup->mObjectInfoList)
+      addShapeFrame(info->mFrame);
+  }
+
+  addShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::addShapeFramesOf(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  assert(bodyNode);
+
+  bodyNode->eachShapeNodeWith<dynamics::CollisionAspect>(
+      [this](const dynamics::ShapeNode* shapeNode) {
+        addShapeFrame(shapeNode);
+      });
+
+  addShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::addShapeFramesOf(
+    const dynamics::MetaSkeleton* skel, const Others*... others)
+{
+  assert(skel);
+
+  auto numBodyNodes = skel->getNumBodyNodes();
+  for (auto i = 0u; i < numBodyNodes; ++i)
+    addShapeFramesOf(skel->getBodyNode(i));
+
+  addShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::subscribeTo(
+    const dynamics::ConstBodyNodePtr& bodyNode, const Others&... others)
+{
+  const auto inserted = mBodyNodeSources.insert(BodyNodeSources::value_type(
+      bodyNode.get(), BodyNodeSource(bodyNode.get(), bodyNode->getVersion())));
+
+  if (inserted.second) {
+    const BodyNodeSources::iterator& entry = inserted.first;
+    bodyNode->eachShapeNodeWith<dynamics::CollisionAspect>(
+        [&](const dynamics::ShapeNode* shapeNode) {
+          entry->second.mObjects.insert(
+              {shapeNode, addShapeFrameImpl(shapeNode, bodyNode.get())});
+        });
+  }
+
+  subscribeTo(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::subscribeTo(
+    const dynamics::ConstSkeletonPtr& skeleton, const Others&... others)
+{
+  const auto inserted = mSkeletonSources.insert(SkeletonSources::value_type(
+      skeleton.get(), SkeletonSource(skeleton, skeleton->getVersion())));
+
+  if (inserted.second) {
+    SkeletonSource& entry = inserted.first->second;
+
+    const std::size_t numBodies = skeleton->getNumBodyNodes();
+    for (std::size_t i = 0u; i < numBodies; ++i) {
+      const dynamics::BodyNode* bn = skeleton->getBodyNode(i);
+
+      auto& childInfo
+          = entry.mChildren
+                .insert(std::make_pair(
+                    bn, SkeletonSource::ChildInfo(bn->getVersion())))
+                .first->second;
+
+      bn->eachShapeNodeWith<dynamics::CollisionAspect>(
+          [&](const dynamics::ShapeNode* shapeNode) {
+            entry.mObjects.insert(
+                {shapeNode, addShapeFrameImpl(shapeNode, skeleton.get())});
+            childInfo.mFrames.insert(shapeNode);
+          });
+    }
+  }
+
+  subscribeTo(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::removeShapeFramesOf(
+    const dynamics::ShapeFrame* shapeFrame, const Others*... others)
+{
+  removeShapeFrame(shapeFrame);
+
+  removeShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::removeShapeFramesOf(
+    const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
+    const Others*... others)
+{
+  removeShapeFrames(shapeFrames);
+
+  removeShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::removeShapeFramesOf(
+    const CollisionGroup* otherGroup, const Others*... others)
+{
+  assert(otherGroup);
+
+  if (otherGroup) {
+    if (this == otherGroup) {
+      removeAllShapeFrames();
+      return;
+    }
+
+    for (const auto& info : otherGroup->mObjectInfoList)
+      removeShapeFrame(info->mFrame);
+  }
+
+  removeShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::removeShapeFramesOf(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  assert(bodyNode);
+
+  bodyNode->eachShapeNodeWith<dynamics::CollisionAspect>(
+      [&](const dynamics::ShapeNode* shapeNode) {
+        removeShapeFrame(shapeNode);
+      });
+
+  removeShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::removeShapeFramesOf(
+    const dynamics::MetaSkeleton* skel, const Others*... others)
+{
+  assert(skel);
+
+  auto numBodyNodes = skel->getNumBodyNodes();
+  for (auto i = 0u; i < numBodyNodes; ++i)
+    removeShapeFramesOf(skel->getBodyNode(i));
+
+  removeShapeFramesOf(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::unsubscribeFrom(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  auto it = mBodyNodeSources.find(bodyNode);
+  if (it != mBodyNodeSources.end()) {
+    for (const auto& entry : it->second.mObjects)
+      removeShapeFrameInternal(entry.first, bodyNode);
+
+    mBodyNodeSources.erase(it);
+  }
+
+  unsubscribeFrom(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+void CollisionGroup::unsubscribeFrom(
+    const dynamics::Skeleton* skeleton, const Others*... others)
+{
+  auto it = mSkeletonSources.find(skeleton);
+  if (it != mSkeletonSources.end()) {
+    for (const auto& entry : it->second.mObjects) {
+      removeShapeFrameInternal(
+          entry.first, static_cast<const dynamics::MetaSkeleton*>(skeleton));
+    }
+
+    mSkeletonSources.erase(it);
+  }
+
+  unsubscribeFrom(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  auto it = mBodyNodeSources.find(bodyNode);
+  return (it != mBodyNodeSources.end()) && isSubscribedTo(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::Skeleton* skeleton, const Others*... others)
+{
+  auto it = mSkeletonSources.find(skeleton);
+  return (it != mSkeletonSources.end()) && isSubscribedTo(others...);
+}
+
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_COLLISIONGROUP_HPP_

include/dart/collision/detail/Contact-impl.hpp

@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * 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_DETAIL_CONTACT_IMPL_HPP_
+#define DART_COLLISION_DETAIL_CONTACT_IMPL_HPP_
+
+#include <dart/collision/Contact.hpp>
+
+namespace dart {
+namespace collision {
+
+//==============================================================================
+constexpr double Contact::getNormalEpsilon()
+{
+  return 1e-6;
+}
+
+//==============================================================================
+constexpr double Contact::getNormalEpsilonSquared()
+{
+  return 1e-12;
+}
+
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_CONTACT_IMPL_HPP_

include/dart/collision/detail/UnorderedPairs.hpp

@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * 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_DETAIL_UNORDEREDPAIRS_HPP_
+#define DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
+
+#include <unordered_map>
+#include <unordered_set>
+
+namespace dart {
+namespace collision {
+namespace detail {
+
+template <class T>
+class UnorderedPairs
+{
+public:
+  /// Adds a pair to this container.
+  void addPair(const T* left, const T* right);
+
+  /// Removes a pair from this container.
+  void removePair(const T* left, const T* right);
+
+  /// Removes all the pairs from this container.
+  void removeAllPairs();
+
+  /// Returns true if this container contains the pair.
+  bool contains(const T* left, const T* right) const;
+
+private:
+  /// The actual container to store pairs.
+  ///
+  /// Each pair is stored so that the key of the std::unordered_map always has
+  /// a value less than every element in the std::unordered_set that is
+  /// associated with it.
+  std::unordered_map<const T*, std::unordered_set<const T*>> mList;
+};
+
+//==============================================================================
+template <class T>
+void UnorderedPairs<T>::addPair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  // Call insert in case an entry for bodyNodeLess doesn't exist. If it doesn't
+  // exist, it will be initialized with an empty set. If it does already exist,
+  // we will just get an iterator to the existing entry.
+  const auto itLess
+      = mList.insert(std::make_pair(less, std::unordered_set<const T*>()))
+            .first;
+
+  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
+  // pair already existed, this will do nothing.
+  itLess->second.insert(greater);
+}
+
+//==============================================================================
+template <class T>
+void UnorderedPairs<T>::removePair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* bodyNodeLess = left;
+  const auto* bodyNodeGreater = right;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  // Remove the pair only when it already exists
+  const auto resultLeft = mList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft) {
+    auto& associatedRights = resultLeft->second;
+    associatedRights.erase(bodyNodeGreater);
+
+    if (associatedRights.empty())
+      mList.erase(resultLeft);
+  }
+}
+
+//==============================================================================
+template <class T>
+void UnorderedPairs<T>::removeAllPairs()
+{
+  mList.clear();
+}
+
+//==============================================================================
+template <class T>
+bool UnorderedPairs<T>::contains(const T* left, const T* right) const
+{
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  const auto resultLeft = mList.find(less);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft) {
+    auto& associatedRights = resultLeft->second;
+
+    const auto resultRight = associatedRights.find(greater);
+    const bool foundRight = (resultRight != associatedRights.end());
+    if (foundRight)
+      return true;
+  }
+
+  return false;
+}
+
+} // namespace detail
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_