add missing files

Signed-off-by: Ian Chen <[email protected]>

dartsim/src/GzCollisionDetector.cc

@@ -0,0 +1,187 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#include <dart/collision/CollisionObject.hpp>
+
+#include "GzCollisionDetector.hh"
+
+using namespace dart;
+using namespace collision;
+
+/////////////////////////////////////////////////
+GzCollisionDetector::GzCollisionDetector()
+{
+}
+
+/////////////////////////////////////////////////
+void GzCollisionDetector::SetCollisionPairMaxContacts(
+    std::size_t _maxContacts)
+{
+  this->maxCollisionPairContacts = _maxContacts;
+}
+
+/////////////////////////////////////////////////
+std::size_t GzCollisionDetector::GetCollisionPairMaxContacts() const
+{
+  return this->maxCollisionPairContacts;
+}
+
+/////////////////////////////////////////////////
+void GzCollisionDetector::LimitCollisionPairMaxContacts(
+    CollisionResult *_result)
+{
+  if (this->maxCollisionPairContacts ==
+    std::numeric_limits<std::size_t>::max())
+    return;
+
+  auto allContacts = _result->getContacts();
+  _result->clear();
+
+
+  if (this->maxCollisionPairContacts == 0u)
+    return;
+
+  // A map of collision pairs and their contact info
+  // Contact info is stored in std::pair. The elements are:
+  // <contact count, index of last contact point (in _result)>
+  std::unordered_map<dart::collision::CollisionObject *,
+      std::unordered_map<dart::collision::CollisionObject *,
+      std::pair<std::size_t, std::size_t>>>
+      contactMap;
+
+  for (auto &contact : allContacts)
+  {
+    auto &[count, lastContactIdx] =
+        contactMap[contact.collisionObject1][contact.collisionObject2];
+    count++;
+    auto &[otherCount, otherLastContactIdx] =
+      contactMap[contact.collisionObject2][contact.collisionObject1];
+
+    std::size_t total =  count + otherCount;
+    if (total <= this->maxCollisionPairContacts)
+    {
+      if (total == this->maxCollisionPairContacts)
+      {
+        lastContactIdx = _result->getNumContacts();
+        otherLastContactIdx = lastContactIdx;
+      }
+      _result->addContact(contact);
+    }
+    else
+    {
+      // If too many contacts were generated, replace the last contact point
+      // of the collision pair with one that has a larger penetration depth
+      auto &c = _result->getContact(lastContactIdx);
+      if (contact.penetrationDepth > c.penetrationDepth)
+      {
+        c = contact;
+      }
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+GzOdeCollisionDetector::GzOdeCollisionDetector()
+  : OdeCollisionDetector(), GzCollisionDetector()
+{
+}
+
+/////////////////////////////////////////////////
+GzOdeCollisionDetector::Registrar<GzOdeCollisionDetector>
+    GzOdeCollisionDetector::mRegistrar{
+        GzOdeCollisionDetector::getStaticType(),
+        []() -> std::shared_ptr<GzOdeCollisionDetector> {
+          return GzOdeCollisionDetector::create();
+        }};
+
+/////////////////////////////////////////////////
+std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
+{
+  return std::shared_ptr<GzOdeCollisionDetector>(new GzOdeCollisionDetector());
+}
+
+/////////////////////////////////////////////////
+bool GzOdeCollisionDetector::collide(
+    CollisionGroup *_group,
+    const CollisionOption &_option,
+    CollisionResult *_result)
+{
+  bool ret = OdeCollisionDetector::collide(_group, _option, _result);
+  this->LimitCollisionPairMaxContacts(_result);
+  return ret;
+}
+
+/////////////////////////////////////////////////
+bool GzOdeCollisionDetector::collide(
+    CollisionGroup *_group1,
+    CollisionGroup *_group2,
+    const CollisionOption &_option,
+    CollisionResult *_result)
+{
+  bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result);
+  this->LimitCollisionPairMaxContacts(_result);
+  return ret;
+}
+
+/////////////////////////////////////////////////
+GzBulletCollisionDetector::GzBulletCollisionDetector()
+  : BulletCollisionDetector(), GzCollisionDetector()
+{
+}
+
+/////////////////////////////////////////////////
+GzBulletCollisionDetector::Registrar<GzBulletCollisionDetector>
+    GzBulletCollisionDetector::mRegistrar{
+        GzBulletCollisionDetector::getStaticType(),
+        []() -> std::shared_ptr<GzBulletCollisionDetector> {
+          return GzBulletCollisionDetector::create();
+        }};
+
+/////////////////////////////////////////////////
+std::shared_ptr<GzBulletCollisionDetector> GzBulletCollisionDetector::create()
+{
+  return std::shared_ptr<GzBulletCollisionDetector>(
+      new GzBulletCollisionDetector());
+}
+
+/////////////////////////////////////////////////
+bool GzBulletCollisionDetector::collide(
+    CollisionGroup *_group,
+    const CollisionOption &_option,
+    CollisionResult *_result)
+{
+  bool ret = BulletCollisionDetector::collide(_group, _option, _result);
+  this->LimitCollisionPairMaxContacts(_result);
+  return ret;
+}
+
+/////////////////////////////////////////////////
+bool GzBulletCollisionDetector::collide(
+    CollisionGroup *_group1,
+    CollisionGroup *_group2,
+    const CollisionOption &_option,
+    CollisionResult *_result)
+{
+  bool ret = BulletCollisionDetector::collide(
+      _group1, _group2, _option, _result);
+  this->LimitCollisionPairMaxContacts(_result);
+  return ret;
+}

dartsim/src/GzCollisionDetector.hh

@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_
+#define GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_
+
+#include <cstdio>
+#include <limits>
+#include <memory>
+
+#include <dart/collision/CollisionResult.hpp>
+#include <dart/collision/bullet/BulletCollisionDetector.hpp>
+#include <dart/collision/ode/OdeCollisionDetector.hpp>
+
+namespace dart {
+namespace collision {
+
+class GzCollisionDetector
+{
+  /// \brief Set the maximum number of contacts between a pair of collision
+  /// objects
+  /// \param[in] _maxContacts Maximum number of contacts between a pair of
+  /// collision objects.
+  public: virtual void SetCollisionPairMaxContacts(std::size_t _maxContacts);
+
+  /// \brief Get the maximum number of contacts between a pair of collision
+  /// objects
+  /// \return Maximum number of contacts between a pair of collision objects.
+  public: virtual std::size_t GetCollisionPairMaxContacts() const;
+
+  /// Constructor
+  protected: GzCollisionDetector();
+
+  /// \brief Limit max number of contacts between a pair of collision objects.
+  /// The function modifies the contacts vector inside the CollisionResult
+  /// object to cap the number of contacts for each collision pair based on the
+  /// maxCollisionPairContacts value
+  protected: virtual void LimitCollisionPairMaxContacts(
+      CollisionResult *_result);
+
+  /// \brief Maximum number of contacts between a pair of collision objects.
+  protected: std::size_t maxCollisionPairContacts =
+      std::numeric_limits<std::size_t>::max();
+};
+
+class GzOdeCollisionDetector :
+    public dart::collision::OdeCollisionDetector,
+    public dart::collision::GzCollisionDetector
+{
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr) override;
+
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group1,
+      CollisionGroup* group2,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr) override;
+
+  /// \brief Create the GzOdeCollisionDetector
+  public: static std::shared_ptr<GzOdeCollisionDetector> create();
+
+  /// Constructor
+  protected: GzOdeCollisionDetector();
+
+  private: static Registrar<GzOdeCollisionDetector> mRegistrar;
+};
+
+class GzBulletCollisionDetector :
+    public dart::collision::BulletCollisionDetector,
+    public dart::collision::GzCollisionDetector
+{
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr) override;
+
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group1,
+      CollisionGroup* group2,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr) override;
+
+  /// \brief Create the GzBulletCollisionDetector
+  public: static std::shared_ptr<GzBulletCollisionDetector> create();
+
+  /// Constructor
+  protected: GzBulletCollisionDetector();
+
+  private: static Registrar<GzBulletCollisionDetector> mRegistrar;
+};
+
+}
+}
+
+#endif