-
Notifications
You must be signed in to change notification settings - Fork 43
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Michael Carroll <[email protected]>
- Loading branch information
Showing
86 changed files
with
759 additions
and
1,833 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,112 @@ | ||
| /* | ||
| * 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 <dart/collision/CollisionObject.hpp> | ||
|
|
||
| #include "GzOdeCollisionDetector.hh" | ||
|
|
||
| using namespace dart; | ||
| using namespace collision; | ||
|
|
||
| ///////////////////////////////////////////////// | ||
| GzOdeCollisionDetector::GzOdeCollisionDetector() | ||
| : OdeCollisionDetector() | ||
| { | ||
| } | ||
|
|
||
| ///////////////////////////////////////////////// | ||
| 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; | ||
| } | ||
|
|
||
| ///////////////////////////////////////////////// | ||
| void GzOdeCollisionDetector::SetCollisionPairMaxContacts( | ||
| std::size_t _maxContacts) | ||
| { | ||
| this->maxCollisionPairContacts = _maxContacts; | ||
| } | ||
|
|
||
| ///////////////////////////////////////////////// | ||
| std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const | ||
| { | ||
| return this->maxCollisionPairContacts; | ||
| } | ||
|
|
||
| ///////////////////////////////////////////////// | ||
| void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( | ||
| CollisionResult *_result) | ||
| { | ||
| if (this->maxCollisionPairContacts == | ||
| std::numeric_limits<std::size_t>::max()) | ||
| return; | ||
|
|
||
| auto allContacts = _result->getContacts(); | ||
| _result->clear(); | ||
|
|
||
| std::unordered_map<dart::collision::CollisionObject *, | ||
| std::unordered_map<dart::collision::CollisionObject *, std::size_t>> | ||
| contactMap; | ||
|
|
||
| for (auto &contact : allContacts) | ||
| { | ||
| auto &count = | ||
| contactMap[contact.collisionObject1][contact.collisionObject2]; | ||
| count++; | ||
| auto &otherCount = | ||
| contactMap[contact.collisionObject2][contact.collisionObject1]; | ||
| std::size_t total = count + otherCount; | ||
| if (total <= this->maxCollisionPairContacts) | ||
| { | ||
| _result->addContact(contact); | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,73 @@ | ||
| /* | ||
| * 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 <limits> | ||
| #include <memory> | ||
|
|
||
| #include <dart/collision/ode/OdeCollisionDetector.hpp> | ||
|
|
||
| namespace dart { | ||
| namespace collision { | ||
|
|
||
| class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector | ||
| { | ||
| // 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 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: 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: std::size_t GetCollisionPairMaxContacts() const; | ||
|
|
||
|
|
||
| /// \brief Create the GzOdeCollisionDetector | ||
| public: static std::shared_ptr<GzOdeCollisionDetector> create(); | ||
|
|
||
| /// Constructor | ||
| protected: GzOdeCollisionDetector(); | ||
|
|
||
| /// \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 | ||
| private: void LimitCollisionPairMaxContacts(CollisionResult *_result); | ||
|
|
||
| /// \brief Maximum number of contacts between a pair of collision objects. | ||
| private: std::size_t maxCollisionPairContacts = | ||
| std::numeric_limits<std::size_t>::max(); | ||
|
|
||
| private: static Registrar<GzOdeCollisionDetector> mRegistrar; | ||
| }; | ||
|
|
||
| } | ||
| } |
Oops, something went wrong.