diff --git a/CHANGELOG.md b/CHANGELOG.md index 2895ac29a78fd..fa11e7e7f438f 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/dart/collision/CMakeLists.txt b/dart/collision/CMakeLists.txt index 102820e29a478..6a8c71558a974 100644 --- a/dart/collision/CMakeLists.txt +++ b/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} diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index fdea44d86baf7..e210657927c85 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -38,8 +38,10 @@ #include #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 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; diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index f89f5b93a4feb..9b5914e44a40d 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/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); } //============================================================================== diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 6936244ca1c95..cfed379fab75e 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -35,8 +35,10 @@ #include #include #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 diff --git a/dart/collision/Option.cpp b/dart/collision/CollisionOption.cpp similarity index 97% rename from dart/collision/Option.cpp rename to dart/collision/CollisionOption.cpp index 59d01ac586208..17b41575743e8 100644 --- a/dart/collision/Option.cpp +++ b/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 { diff --git a/dart/collision/CollisionOption.hpp b/dart/collision/CollisionOption.hpp new file mode 100644 index 0000000000000..1f476db0f4266 --- /dev/null +++ b/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 +#include + +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; + + /// Constructor + CollisionOption( + bool enableContact = true, + std::size_t maxNumContacts = 1000u, + const std::shared_ptr& collisionFilter = nullptr); + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONOPTION_HPP_ diff --git a/dart/collision/Result.cpp b/dart/collision/CollisionResult.cpp similarity index 99% rename from dart/collision/Result.cpp rename to dart/collision/CollisionResult.cpp index 4e33f57aca11b..3bb837b5faac4 100644 --- a/dart/collision/Result.cpp +++ b/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" diff --git a/dart/collision/CollisionResult.hpp b/dart/collision/CollisionResult.hpp new file mode 100644 index 0000000000000..abcc1a26f7952 --- /dev/null +++ b/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 +#include +#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& getContacts() const; + + /// Return the set of BodyNodes that are in collision + const std::unordered_set& + getCollidingBodyNodes() const; + + /// Return the set of ShapeFrames that are in collision + const std::unordered_set& + 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 mContacts; + + /// Set of BodyNodes that are colliding + std::unordered_set mCollidingBodyNodes; + + /// Set of ShapeFrames that are colliding + std::unordered_set mCollidingShapeFrames; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONRESULT_HPP_ diff --git a/dart/collision/DistanceFilter.cpp b/dart/collision/DistanceFilter.cpp new file mode 100644 index 0000000000000..68363a979b20f --- /dev/null +++ b/dart/collision/DistanceFilter.cpp @@ -0,0 +1,94 @@ +/* + * 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. + */ + +#include "dart/collision/DistanceFilter.hpp" + +#include "dart/dynamics/BodyNode.hpp" +#include "dart/collision/CollisionObject.hpp" + +namespace dart { +namespace collision { + +//============================================================================== +bool BodyNodeDistanceFilter::needDistance( + const collision::CollisionObject* object1, + const collision::CollisionObject* object2) const +{ + if (object1 == object2) + return false; + + auto shapeNode1 = object1->getShapeFrame()->asShapeNode(); + auto shapeNode2 = object2->getShapeFrame()->asShapeNode(); + + if (!shapeNode1 || !shapeNode2) + return true; + // We assume that non-ShapeNode is always being checked collision. + + auto bodyNode1 = shapeNode1->getBodyNodePtr(); + auto bodyNode2 = shapeNode2->getBodyNodePtr(); + + if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable()) + return false; + + if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton()) + { + auto skeleton = bodyNode1->getSkeleton(); + + if (!skeleton->isEnabledSelfCollisionCheck()) + return false; + + if (!skeleton->isEnabledAdjacentBodyCheck()) + { + if (areAdjacentBodies(bodyNode1, bodyNode2)) + return false; + } + } + + return true; +} + +//============================================================================== +bool BodyNodeDistanceFilter::areAdjacentBodies( + const dynamics::BodyNode* bodyNode1, + const dynamics::BodyNode* bodyNode2) const +{ + if ((bodyNode1->getParentBodyNode() == bodyNode2) + || (bodyNode2->getParentBodyNode() == bodyNode1)) + { + assert(bodyNode1->getSkeleton() == bodyNode2->getSkeleton()); + return true; + } + + return false; +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/DistanceFilter.hpp b/dart/collision/DistanceFilter.hpp new file mode 100644 index 0000000000000..4207abb13b851 --- /dev/null +++ b/dart/collision/DistanceFilter.hpp @@ -0,0 +1,63 @@ +/* + * 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_DISTANCEFILTER_HPP_ +#define DART_COLLISION_DISTANCEFILTER_HPP_ + +namespace dart { + +namespace dynamics { +class BodyNode; +} // namespace dynamics + +namespace collision { + +class CollisionObject; + +struct DistanceFilter +{ + virtual bool needDistance(const CollisionObject* object1, + const CollisionObject* object2) const = 0; +}; + +struct BodyNodeDistanceFilter : DistanceFilter +{ + bool needDistance(const CollisionObject* object1, + const CollisionObject* object2) const override; + + bool areAdjacentBodies(const dynamics::BodyNode* bodyNode1, + const dynamics::BodyNode* bodyNode2) const; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DISTANCEFILTER_HPP_ diff --git a/dart/collision/DistanceOption.cpp b/dart/collision/DistanceOption.cpp new file mode 100644 index 0000000000000..3ad424f8cf426 --- /dev/null +++ b/dart/collision/DistanceOption.cpp @@ -0,0 +1,50 @@ +/* + * 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. + */ + +#include "dart/collision/DistanceOption.hpp" + +namespace dart { +namespace collision { + +//============================================================================== +DistanceOption::DistanceOption( + bool enableNearestPoints, + double minDistance, + const std::shared_ptr& distanceFilter) + : enableNearestPoints(enableNearestPoints), + distanceLowerBound(minDistance), + distanceFilter(distanceFilter) +{ + // Do nothing +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/DistanceOption.hpp b/dart/collision/DistanceOption.hpp new file mode 100644 index 0000000000000..2eb5f9bf8438e --- /dev/null +++ b/dart/collision/DistanceOption.hpp @@ -0,0 +1,78 @@ +/* + * 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_DISTANCE_OPTION_HPP_ +#define DART_COLLISION_DISTANCE_OPTION_HPP_ + +#include +#include + +namespace dart { +namespace collision { + +class DistanceFilter; + +struct DistanceOption +{ + /// Whether to calculate the nearest points. + /// + /// The default is false. + /// \sa DistanceResult::nearestPoint1, DistanceResult::nearestPoint2 + bool enableNearestPoints; + + /// Stopping criteria for distance calculation in broadphase. + /// + /// This option is used for early termination of distance calculate that stops + /// as soon as a distance is found that is equal to or less than the lower + /// bound. If you want to check all the shape pairs without the early + /// termination then set this value to -inf. + /// + /// The default value is 0.0. + double distanceLowerBound; + + /// Distance filter for excluding ShapeFrame pairs from distance calculation + /// in broadphase. + /// + /// If nullptr, every pairs of ShapeFrames in the CollisionGroup(s) are + /// checked. The default is nullptr. \sa DistanceFilter + std::shared_ptr distanceFilter; + + /// Constructor + DistanceOption( + bool enableNearestPoints = false, + double distanceLowerBound = 0.0, + const std::shared_ptr& distanceFilter = nullptr); +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DISTANCE_OPTION_HPP_ diff --git a/dart/collision/DistanceResult.cpp b/dart/collision/DistanceResult.cpp new file mode 100644 index 0000000000000..983be9341cb8f --- /dev/null +++ b/dart/collision/DistanceResult.cpp @@ -0,0 +1,81 @@ +/* + * 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. + */ + +#include "dart/collision/DistanceResult.hpp" + +#define DART_DEFAULT_MIN_DISTANCE (0.0) +#define DART_DEFAULT_UNCLAMPED_MIN_DISTANCE (0.0) + +namespace dart { +namespace collision { + +//============================================================================== +DistanceResult::DistanceResult() + : minDistance(DART_DEFAULT_MIN_DISTANCE), + unclampedMinDistance(0.0), + shapeFrame1(nullptr), + shapeFrame2(nullptr), + nearestPoint1(Eigen::Vector3d::Zero()), + nearestPoint2(Eigen::Vector3d::Zero()) +{ + // Do nothing +} + +//============================================================================== +void DistanceResult::clear() +{ + minDistance = DART_DEFAULT_MIN_DISTANCE; + unclampedMinDistance = DART_DEFAULT_UNCLAMPED_MIN_DISTANCE; + + nearestPoint1.setZero(); + nearestPoint2.setZero(); + + shapeFrame1 = nullptr; + shapeFrame2 = nullptr; +} + +//============================================================================== +bool DistanceResult::found() const +{ + if (!shapeFrame1 || !shapeFrame2) + return false; + + return true; +} + +//============================================================================== +bool DistanceResult::isMinDistanceClamped() const +{ + return found() && (minDistance == unclampedMinDistance); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/DistanceResult.hpp b/dart/collision/DistanceResult.hpp new file mode 100644 index 0000000000000..e42f5a3056cb1 --- /dev/null +++ b/dart/collision/DistanceResult.hpp @@ -0,0 +1,122 @@ +/* + * 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_DISTANCE_RESULT_HPP_ +#define DART_COLLISION_DISTANCE_RESULT_HPP_ + +#include + +namespace dart { + +namespace dynamics { +class ShapeFrame; +} // namesapce dynamics + +namespace collision { + +struct DistanceResult +{ + /// Minimum \b singed distance between the checked Shape pairs. + /// + /// If no shape pair was checked (the collision group was empty or all pairs + /// were excluded by the filter) then this value will be remained the default + /// value 0.0. You can check if a valid distance was found using + /// DistanceResult::found() that returns true when it's found. + /// + /// The result minimum distance is clamped by + /// DistanceOption::distanceLowerBound + /// (min. distance >= distance lower bound). You can still get the unclamped + /// distance from DistanceResult::unclampedMinDistance. + /// + /// If the minimum distance is negative value, that means the distance is + /// the negative penetration depth measured from the shapes that are in + /// collision. + double minDistance; + + /// Unclamped minimum distance that is the first distance equal to or less + /// than DistanceOption::distanceLowerBound. + /// + /// \warning This value is implementation defined, which means the value could + /// be different depending on various factors (e.g., the collision detector + /// and even the version of the collision detector), and we don't intend this + /// value to be (reasonably) indentical over thoes factors. This valude is for + /// debugging purpose or advanced use. + double unclampedMinDistance; + + /// First ShapeFrame of the minDistance + /// + /// If no shape pair was checked then shapeFrame1 will be nullptr. + const dynamics::ShapeFrame* shapeFrame1; + + /// Second ShapeFrame of the minDistance + /// + /// If no shape pair was checked then shapeFrame2 will be nullptr. + const dynamics::ShapeFrame* shapeFrame2; + + /// The nearest point on DistanceResult::shapeFrame1 expressed in the world + /// coordinates. + /// + /// The point is calculated only when DistanceOption::enableNearestPoints is + /// true. + /// + /// The distance between the two nearest points is equal to + /// unclampedMinDistance rather than minDistance. + Eigen::Vector3d nearestPoint1; + + /// The nearest point on DistanceResult::shapeFrame2 expressed the world + /// coordinates. + /// + /// The point is calculated only when DistanceOption::enableNearestPoints is + /// true. + /// + /// The distance between the two nearest points is equal to + /// unclampedMinDistance rather than minDistance. + Eigen::Vector3d nearestPoint2; + + /// Constructor + DistanceResult(); + + /// Clear the result + void clear(); + + /// Get true if the result is valid (at least one shape pair was checked + /// and the result values are good to use). + bool found() const; + + /// Get true if the minimum distance was not clamped + /// (minDistance == unclampedMinDistance). + bool isMinDistanceClamped() const; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DISTANCE_RESULT_HPP_ diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index ca6aeafdf74e1..f8ddf462bd3a6 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -32,40 +32,9 @@ #ifndef DART_COLLISION_OPTION_HPP_ #define DART_COLLISION_OPTION_HPP_ -#include -#include +#warning "This header has been deprecated in DART 6.1. "\ + "Please include CollisionOption.hpp intead." -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; - - /// Constructor - CollisionOption( - bool enableContact = true, - std::size_t maxNumContacts = 1000u, - const std::shared_ptr& collisionFilter = nullptr); - -}; - -} // namespace collision -} // namespace dart +#include "dart/collision/CollisionOption.hpp" #endif // DART_COLLISION_OPTION_HPP_ diff --git a/dart/collision/Result.hpp b/dart/collision/Result.hpp index 1c9d5e3afd85c..2596165bacd7a 100644 --- a/dart/collision/Result.hpp +++ b/dart/collision/Result.hpp @@ -32,79 +32,9 @@ #ifndef DART_COLLISION_RESULT_HPP_ #define DART_COLLISION_RESULT_HPP_ -#include -#include -#include "dart/collision/Contact.hpp" +#warning "This header has been deprecated in DART 6.1. "\ + "Please include CollisionResult.hpp intead." -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& getContacts() const; - - /// Return the set of BodyNodes that are in collision - const std::unordered_set& - getCollidingBodyNodes() const; - - /// Return the set of ShapeFrames that are in collision - const std::unordered_set& - 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 mContacts; - - /// Set of BodyNodes that are colliding - std::unordered_set mCollidingBodyNodes; - - /// Set of ShapeFrames that are colliding - std::unordered_set mCollidingShapeFrames; - -}; - -} // namespace collision -} // namespace dart +#include "dart/collision/CollisionResult.hpp" #endif // DART_COLLISION_RESULT_HPP_ diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index ec824284f132b..2836850c460cd 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -313,6 +313,31 @@ bool BulletCollisionDetector::collide( } } +//============================================================================== +double BulletCollisionDetector::distance( + CollisionGroup* /*group*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[BulletCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Returning 0.0.\n"; + + return 0.0; +} + +//============================================================================== +double BulletCollisionDetector::distance( + CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[BulletCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Returning.\n"; + + return 0.0; +} + //============================================================================== BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 7711d9b0dfaed..0fae18868592c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -82,6 +82,19 @@ class BulletCollisionDetector : public CollisionDetector const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; + // Documentation inherited + double distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + double distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + protected: /// Constructor diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 0de1d015452f4..6e28052bf38b4 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -224,6 +224,31 @@ bool DARTCollisionDetector::collide( return collisionFound; } +//============================================================================== +double DARTCollisionDetector::distance( + CollisionGroup* /*group*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[DARTCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Returning 0.0.\n"; + + return 0.0; +} + +//============================================================================== +double DARTCollisionDetector::distance( + CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[DARTCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Returning 0.0.\n"; + + return 0.0; +} + //============================================================================== DARTCollisionDetector::DARTCollisionDetector() : CollisionDetector() diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 336b2caa72fca..c316ac7482343 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -71,6 +71,19 @@ class DARTCollisionDetector : public CollisionDetector const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; + // Documentation inherited + double distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + double distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + protected: /// Constructor diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 53bc8e10d2cda..39e595d504927 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include "dart/common/Console.hpp" #include "dart/collision/CollisionObject.hpp" #include "dart/collision/CollisionFilter.hpp" +#include "dart/collision/DistanceFilter.hpp" #include "dart/collision/fcl/FCLTypes.hpp" #include "dart/collision/fcl/FCLCollisionObject.hpp" #include "dart/collision/fcl/FCLCollisionGroup.hpp" @@ -66,6 +68,12 @@ namespace { bool collisionCallback( fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); +bool distanceCallback( + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata, + fcl::FCL_REAL& dist); + void postProcessFCL( const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, @@ -80,6 +88,13 @@ void postProcessDART( const CollisionOption& option, CollisionResult& result); +void interpreteDistanceResult( + const fcl::DistanceResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const DistanceOption& option, + DistanceResult& result); + int evalContactPosition(const fcl::Contact& fclContact, const fcl::BVHModel* mesh1, const fcl::BVHModel* mesh2, @@ -114,7 +129,10 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); void convertOption( - const CollisionOption& fclOption, fcl::CollisionRequest& request); + const CollisionOption& option, fcl::CollisionRequest& request); + +void convertOption( + const DistanceOption& option, fcl::DistanceRequest& request); Contact convertContact( const fcl::Contact& fclContact, @@ -182,6 +200,37 @@ struct FCLCollisionCallbackData } }; +struct FCLDistanceCallbackData +{ + /// FCL distance request + fcl::DistanceRequest fclRequest; + + /// FCL distance result + fcl::DistanceResult fclResult; + + /// Distance option of DART + const DistanceOption& option; + + /// Minimum distance identical to DistanceResult::minDistnace if result is not + /// nullptr. + double unclampedMinDistance; + + /// Distance result of DART + DistanceResult* result; + + /// @brief Whether the distance iteration can stop + bool done; + + FCLDistanceCallbackData( + const DistanceOption& option, DistanceResult* result) + : option(option), + result(result), + done(false) + { + convertOption(option, fclRequest); + } +}; + //============================================================================== // Create a cube mesh for collision detection template @@ -710,6 +759,59 @@ bool FCLCollisionDetector::collide( return collData.isCollision(); } +//============================================================================== +double FCLCollisionDetector::distance( + CollisionGroup* group, + const DistanceOption& option, + DistanceResult* result) +{ + if (result) + result->clear(); + + if (!checkGroupValidity(this, group)) + return 0.0; + + auto casted = static_cast(group); + casted->updateEngineData(); + + FCLDistanceCallbackData distData(option, result); + + casted->getFCLCollisionManager()->distance(&distData, distanceCallback); + + return std::max(distData.unclampedMinDistance, option.distanceLowerBound); +} + +//============================================================================== +double FCLCollisionDetector::distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option, + DistanceResult* result) +{ + if (result) + result->clear(); + + if (!checkGroupValidity(this, group1)) + return 0.0; + + if (!checkGroupValidity(this, group2)) + return 0.0; + + auto casted1 = static_cast(group1); + auto casted2 = static_cast(group2); + casted1->updateEngineData(); + casted2->updateEngineData(); + + FCLDistanceCallbackData distData(option, result); + + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); + + broadPhaseAlg1->distance(broadPhaseAlg2, &distData, distanceCallback); + + return std::max(distData.unclampedMinDistance, option.distanceLowerBound); +} + //============================================================================== void FCLCollisionDetector::setPrimitiveShapeType( FCLCollisionDetector::PrimitiveShape type) @@ -1033,6 +1135,64 @@ bool collisionCallback( return collData->done; } +//============================================================================== +bool distanceCallback( + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* ddata, + fcl::FCL_REAL& dist) +{ + auto* distData = static_cast(ddata); + + const auto& fclRequest = distData->fclRequest; + auto& fclResult = distData->fclResult; + auto* result = distData->result; + const auto& option = distData->option; + const auto& filter = option.distanceFilter; + + if (distData->done) + { + dist = distData->unclampedMinDistance; + return true; + } + + // Filtering + if (filter) + { + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); + assert(userData1); + assert(userData2); + + auto collisionObject1 = userData1->mCollisionObject; + auto collisionObject2 = userData2->mCollisionObject; + assert(collisionObject1); + assert(collisionObject2); + + if (!filter->needDistance(collisionObject2, collisionObject1)) + return distData->done; + } + + // Clear previous results + fclResult.clear(); + + // Perform narrow-phase check + fcl::distance(o1, o2, fclRequest, fclResult); + + // Store the minimum distance just in case result is nullptr. + distData->unclampedMinDistance = fclResult.min_distance; + + if (result) + interpreteDistanceResult(fclResult, o1, o2, option, *result); + + if (distData->unclampedMinDistance <= option.distanceLowerBound) + distData->done = true; + + return distData->done; +} + //============================================================================== Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2) { @@ -1283,6 +1443,39 @@ void postProcessDART( } } +//============================================================================== +void interpreteDistanceResult( + const fcl::DistanceResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const DistanceOption& option, + DistanceResult& result) +{ + result.unclampedMinDistance = fclResult.min_distance; + result.minDistance + = std::max(fclResult.min_distance, option.distanceLowerBound); + + const auto* userData1 + = static_cast(o1->getUserData()); + const auto* userData2 + = static_cast(o2->getUserData()); + assert(userData1); + assert(userData2); + assert(userData1->mCollisionObject); + assert(userData2->mCollisionObject); + + result.shapeFrame1 = userData1->mCollisionObject->getShapeFrame(); + result.shapeFrame2 = userData2->mCollisionObject->getShapeFrame(); + + if (option.enableNearestPoints) + { + result.nearestPoint1 + = FCLTypes::convertVector3(fclResult.nearest_points[0]); + result.nearestPoint2 + = FCLTypes::convertVector3(fclResult.nearest_points[1]); + } +} + //============================================================================== int evalContactPosition( const fcl::Contact& fclContact, @@ -1408,15 +1601,21 @@ bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol) } //============================================================================== -void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& request) +void convertOption(const CollisionOption& option, fcl::CollisionRequest& request) { - request.num_max_contacts = fclOption.maxNumContacts; - request.enable_contact = fclOption.enableContact; + request.num_max_contacts = option.maxNumContacts; + request.enable_contact = option.enableContact; #if FCL_VERSION_AT_LEAST(0,3,0) request.gjk_solver_type = fcl::GST_LIBCCD; #endif } +//============================================================================== +void convertOption(const DistanceOption& option, fcl::DistanceRequest& request) +{ + request.enable_nearest_points = option.enableNearestPoints; +} + //============================================================================== Contact convertContact(const fcl::Contact& fclContact, fcl::CollisionObject* o1, diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index d37c0e58ccb8f..899dbebbfc2b3 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -108,6 +108,19 @@ class FCLCollisionDetector : public CollisionDetector const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; + // Documentation inherited + double distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + double distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 81b5c89ce828b..1e70f555758d0 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -49,7 +49,7 @@ #include "dart/common/Subject.hpp" #include "dart/dynamics/SimpleFrame.hpp" #include "dart/dynamics/Skeleton.hpp" -#include "dart/collision/Option.hpp" +#include "dart/collision/CollisionOption.hpp" #include "dart/simulation/Recording.hpp" namespace dart { diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 1e44453e171f2..081e46524393a 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -344,7 +344,7 @@ void setGeometry(const BodyNodePtr& bn) std::shared_ptr box(new BoxShape( Eigen::Vector3d(default_width, default_depth, default_height))); - // Create a shpae node for visualization and collision checking + // Create a shape node for visualization and collision checking auto shapeNode = bn->createShapeNodeWith(box); shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 1a64cd473fab8..122aa3454e3ae 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -273,7 +273,7 @@ void setGeometry(const BodyNodePtr& bn) std::shared_ptr box(new BoxShape( Eigen::Vector3d(default_width, default_depth, default_height))); - // Create a shpae node for visualization and collision checking + // Create a shape node for visualization and collision checking auto shapeNode = bn->createShapeNodeWith(box); shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp new file mode 100644 index 0000000000000..665129e58b8d8 --- /dev/null +++ b/unittests/testDistance.cpp @@ -0,0 +1,297 @@ +/* + * 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. + */ + +#include +#include +#include "dart/dart.hpp" +#include "TestHelpers.hpp" + +using namespace dart; + +//============================================================================== +void testBasicInterface(const std::shared_ptr& cd, + double tol = 1e-12) +{ + if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) + { + dtwarn << "Aborting test: distance check is not supported by " + << cd->getType() << ".\n"; + return; + } + + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + + ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto group12 = cd->createCollisionGroup(group1.get(), group2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + EXPECT_EQ(group12->getNumShapeFrames(), 2u); + + collision::DistanceOption option; + option.enableNearestPoints = true; + EXPECT_TRUE(option.enableNearestPoints == true); + EXPECT_TRUE(option.distanceLowerBound == 0.0); + EXPECT_TRUE(option.distanceFilter == nullptr); + + collision::DistanceResult result; + EXPECT_TRUE(result.found() == false); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + group1->distance(group2.get(), option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE(result.found() == true); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + group12->distance(option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE(result.found() == true); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + cd->distance(group1.get(), group2.get(), option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE(result.found() == true); +} + +//============================================================================== +TEST(Distance, testBasicInterface) +{ + auto fcl = FCLCollisionDetector::create(); + testBasicInterface(fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testBasicInterface(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testBasicInterface(dart); +} + +//============================================================================== +void testOptions(const std::shared_ptr& cd, + double tol = 1e-12) +{ + if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) + { + dtwarn << "Aborting test: distance check is not supported by " + << cd->getType() << ".\n"; + return; + } + + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + + ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto group12 = cd->createCollisionGroup(group1.get(), group2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + EXPECT_EQ(group12->getNumShapeFrames(), 2u); + + collision::DistanceOption option; + collision::DistanceResult result; + + EXPECT_TRUE(option.distanceFilter == nullptr); + EXPECT_TRUE(option.enableNearestPoints == false); + + EXPECT_TRUE(result.found() == false); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + group12->distance(option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_EQ(result.nearestPoint1, Eigen::Vector3d::Zero()); + EXPECT_EQ(result.nearestPoint2, Eigen::Vector3d::Zero()); + EXPECT_TRUE(result.found() == true); + + option.enableNearestPoints = true; + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + group12->distance(option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE(result.found() == true); + + option.enableNearestPoints = true; + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + group12->distance(option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.25); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) + || result.nearestPoint1.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) + || result.nearestPoint2.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol)); + EXPECT_TRUE(result.found() == true); +} + +//============================================================================== +TEST(Distance, Options) +{ + auto fcl = FCLCollisionDetector::create(); + testOptions(fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testOptions(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testOptions(dart); +} + +//============================================================================== +void testSphereSphere(const std::shared_ptr& cd, + double tol = 1e-12) +{ + if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) + { + dtwarn << "Aborting test: distance check is not supported by " + << cd->getType() << ".\n"; + return; + } + + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + + ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto group12 = cd->createCollisionGroup(group1.get(), group2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + EXPECT_EQ(group12->getNumShapeFrames(), 2u); + + collision::DistanceOption option; + collision::DistanceResult result; + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); + group12->distance(option, &result); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); + EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() + || result.shapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() + || result.shapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE(result.nearestPoint1.isApprox(Eigen::Vector3d::Zero(), tol)); + EXPECT_TRUE(result.nearestPoint2.isApprox(Eigen::Vector3d::Zero(), tol)); +} + +//============================================================================== +TEST(Distance, SphereSphere) +{ + auto fcl = FCLCollisionDetector::create(); + testSphereSphere(fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testSphereSphere(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testSphereSphere(dart); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}