From 51d31a1e35a306cf597f2f3b46ea4239876e6093 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 19 Jul 2016 19:01:15 -0400 Subject: [PATCH 01/10] Implementing API for signed distance query --- dart/collision/CollisionDetector.cpp | 28 ++++++ dart/collision/CollisionDetector.hpp | 15 +++ dart/collision/DistanceFilter.cpp | 94 ++++++++++++++++++ dart/collision/DistanceFilter.hpp | 63 ++++++++++++ dart/collision/DistanceOption.cpp | 47 +++++++++ dart/collision/DistanceOption.hpp | 59 ++++++++++++ dart/collision/DistanceResult.cpp | 39 ++++++++ dart/collision/DistanceResult.hpp | 67 +++++++++++++ dart/collision/fcl/FCLCollisionDetector.cpp | 101 ++++++++++++++++++++ dart/collision/fcl/FCLCollisionDetector.hpp | 13 +++ unittests/testDistance.cpp | 56 +++++++++++ 11 files changed, 582 insertions(+) create mode 100644 dart/collision/DistanceFilter.cpp create mode 100644 dart/collision/DistanceFilter.hpp create mode 100644 dart/collision/DistanceOption.cpp create mode 100644 dart/collision/DistanceOption.hpp create mode 100644 dart/collision/DistanceResult.cpp create mode 100644 dart/collision/DistanceResult.hpp create mode 100644 unittests/testDistance.cpp diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index d8acb67c3ec09..bc2d95017f019 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -49,6 +49,34 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } +//============================================================================== +bool CollisionDetector::distance( + CollisionGroup* /*group*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[CollisionDetector::distance] [" << getType() << "] does not " + << "support (signed) distance queries. Ignoring this query.\n"; + + // Note: Only FCL supports distance query. + + return false; +} + +//============================================================================== +bool CollisionDetector::distance(CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[CollisionDetector::distance] [" << getType() << "] does not " + << "support (signed) distance queries. Ignoring this query.\n"; + + // Note: Only FCL supports distance query. + + return false; +} + //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index fdea44d86baf7..8ee6f931f7168 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -40,6 +40,8 @@ #include "dart/collision/Contact.hpp" #include "dart/collision/Option.hpp" #include "dart/collision/Result.hpp" +#include "dart/collision/DistanceOption.hpp" +#include "dart/collision/DistanceResult.hpp" #include "dart/collision/SmartPointer.hpp" #include "dart/dynamics/SmartPointer.hpp" @@ -109,6 +111,19 @@ class CollisionDetector : public std::enable_shared_from_this const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) = 0; + /// Perform collision check for a single group. + virtual bool distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(nullptr), + DistanceResult* result = nullptr); + + /// Perform collision check for two groups. + virtual bool distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option = DistanceOption(nullptr), + DistanceResult* result = nullptr); + protected: class CollisionObjectManager; 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..de262c8eb038d --- /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..7ebbc6cf20a27 --- /dev/null +++ b/dart/collision/DistanceOption.cpp @@ -0,0 +1,47 @@ +/* + * 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, + const std::shared_ptr& distanceFilter) + : enableNearestPoints(enableNearestPoints), + 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..523998e54bdd1 --- /dev/null +++ b/dart/collision/DistanceOption.hpp @@ -0,0 +1,59 @@ +/* + * 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 +{ + bool enableNearestPoints; + + /// Distance filter + std::shared_ptr distanceFilter; + + /// Constructor + DistanceOption( + bool enableNearestPoints = false, + 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..7d809b80d4579 --- /dev/null +++ b/dart/collision/DistanceResult.cpp @@ -0,0 +1,39 @@ +/* + * 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" + +namespace dart { +namespace collision { + + +} // namespace collision +} // namespace dart diff --git a/dart/collision/DistanceResult.hpp b/dart/collision/DistanceResult.hpp new file mode 100644 index 0000000000000..f200c7945aa96 --- /dev/null +++ b/dart/collision/DistanceResult.hpp @@ -0,0 +1,67 @@ +/* + * 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 +#include +#include "dart/collision/Contact.hpp" + +namespace dart { +namespace collision { + +struct DistanceResult +{ + double mMinimumDistance; + + Eigen::Vector3d mNearestPoint1; + Eigen::Vector3d mNearestPoint2; + + const dynamics::ShapeFrame* mShapeFrame1; + const dynamics::ShapeFrame* mShapeFrame2; + + void clear() + { + mMinimumDistance = 0.0; + + mNearestPoint1.setZero(); + mNearestPoint2.setZero(); + + mShapeFrame1 = nullptr; + mShapeFrame2 = nullptr; + } +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DISTANCE_RESULT_HPP_ diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 6d5173c060b05..d1e0fed252375 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,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, @@ -181,6 +188,33 @@ struct FCLCollisionCallbackData } }; +struct FCLDistanceCallbackData +{ + FCLDistanceCallbackData( + const DistanceOption& option, DistanceResult* result) + : option(option), + result(result), + done(false) + { + // convertOption(...); + } + + /// FCL distance request + fcl::DistanceRequest fclRequest; + + /// FCL distance result + fcl::DistanceResult fclResult; + + /// Distance option of DART + const DistanceOption& option; + + /// Distance result of DART + DistanceResult* result; + + /// @brief Whether the distance iteration can stop + bool done; +}; + //============================================================================== // Create a cube mesh for collision detection template @@ -713,6 +747,40 @@ bool FCLCollisionDetector::collide( return collData.isCollision(); } +//============================================================================== +bool FCLCollisionDetector::distance( + CollisionGroup* group, + const DistanceOption& option, + DistanceResult* result) +{ + if (result) + result->clear(); + + if (!checkGroupValidity(this, group)) + return false; + + auto casted = static_cast(group); + casted->updateEngineData(); + + FCLDistanceCallbackData distData(option, result); + + casted->getFCLCollisionManager()->distance(&distData, distanceCallback); + + return false; // TODO(JS): not sure what should be returned (if found any?) +} + +//============================================================================== +bool FCLCollisionDetector::distance( + CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + // TODO(JS): Not implemented + + return false; +} + //============================================================================== void FCLCollisionDetector::setPrimitiveShapeType( FCLCollisionDetector::PrimitiveShape type) @@ -1030,6 +1098,39 @@ 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; + + if (distdata->done) + { + dist = fclResult.min_distance; + return true; + } + + fcl::distance(o1, o2, fclRequest, fclResult); + + dist = fclResult.min_distance; + + // TODO(JS): the data should be transformed into the DART's data structure + + if (dist <= 0) + { + // in collision or in touch + return true; + } + + return distdata->done; +} + //============================================================================== Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2) { diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index b808619022801..f6e0c5bb5d16f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -107,6 +107,19 @@ class FCLCollisionDetector : public CollisionDetector const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; + // Documentation inherited + bool distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + bool distance( + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option = DistanceOption(nullptr), + DistanceResult* result = nullptr) override; + /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp new file mode 100644 index 0000000000000..afbc9fa80b82b --- /dev/null +++ b/unittests/testDistance.cpp @@ -0,0 +1,56 @@ +/* + * 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; + +//============================================================================== +TEST(Distance, Basic) +{ + auto fclCd = collision::FCLCollisionDetector::create(); + auto bulletCd = collision::BulletCollisionDetector::create(); + auto dartCd = collision::DARTCollisionDetector::create(); + + EXPECT_TRUE(fclCd->distance(nullptr)); + EXPECT_FALSE(bulletCd->distance(nullptr)); + EXPECT_FALSE(dartCd->distance(nullptr)); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 554a01da94d10a3794035623088be56e8a6e9814 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 09:50:36 -0400 Subject: [PATCH 02/10] Implementing API for signed distance query -- passed simplest test --- dart/collision/CollisionDetector.hpp | 4 +- dart/collision/CollisionGroup.cpp | 20 +++- dart/collision/CollisionGroup.hpp | 18 ++- dart/collision/DistanceResult.cpp | 31 +++++ dart/collision/DistanceResult.hpp | 11 +- dart/collision/fcl/FCLCollisionDetector.cpp | 86 ++++++++++++-- dart/collision/fcl/FCLCollisionDetector.hpp | 4 +- unittests/testDistance.cpp | 120 +++++++++++++++++++- 8 files changed, 269 insertions(+), 25 deletions(-) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 8ee6f931f7168..ec285c42fada8 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -114,14 +114,14 @@ class CollisionDetector : public std::enable_shared_from_this /// Perform collision check for a single group. virtual bool distance( CollisionGroup* group, - const DistanceOption& option = DistanceOption(nullptr), + const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr); /// Perform collision check for two groups. virtual bool distance( CollisionGroup* group1, CollisionGroup* group2, - const DistanceOption& option = DistanceOption(nullptr), + const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr); protected: diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index f89f5b93a4feb..bfdde3ba240c0 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); +} + +//============================================================================== +bool CollisionGroup::distance( + const DistanceOption& option, DistanceResult* result) +{ + return mCollisionDetector->distance(this, option, result); +} + +//============================================================================== +bool 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..9c96462d210e2 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -37,6 +37,8 @@ #include "dart/collision/SmartPointer.hpp" #include "dart/collision/Option.hpp" #include "dart/collision/Result.hpp" +#include "dart/collision/DistanceOption.hpp" +#include "dart/collision/DistanceResult.hpp" #include "dart/dynamics/SmartPointer.hpp" namespace dart { @@ -176,10 +178,24 @@ 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); + /// Perform distance check within this CollisionGroup. + bool distance( + const DistanceOption& option = DistanceOption(false, nullptr), + DistanceResult* result = nullptr); + + /// Perform distance check with other CollisionGroup. + /// + /// Return false if the engine type of the other CollisionGroup is different + /// from this CollisionObject engine. + bool distance( + CollisionGroup* otherGroup, + const DistanceOption& option = DistanceOption(false, nullptr), + DistanceResult* result = nullptr); + protected: /// Update engine data. This function should be called before the collision diff --git a/dart/collision/DistanceResult.cpp b/dart/collision/DistanceResult.cpp index 7d809b80d4579..ff67990c6df07 100644 --- a/dart/collision/DistanceResult.cpp +++ b/dart/collision/DistanceResult.cpp @@ -34,6 +34,37 @@ namespace dart { namespace collision { +//============================================================================== +DistanceResult::DistanceResult() + : mMinimumDistance(0.0), + mNearestPoint1(Eigen::Vector3d::Zero()), + mNearestPoint2(Eigen::Vector3d::Zero()), + mShapeFrame1(nullptr), + mShapeFrame2(nullptr) +{ + // Do nothing +} + +//============================================================================== +void DistanceResult::clear() +{ + mMinimumDistance = 0.0; + + mNearestPoint1.setZero(); + mNearestPoint2.setZero(); + + mShapeFrame1 = nullptr; + mShapeFrame2 = nullptr; +} + +//============================================================================== +bool DistanceResult::found() const +{ + if (!mShapeFrame1 || !mShapeFrame2) + return false; + + return true; +} } // namespace collision } // namespace dart diff --git a/dart/collision/DistanceResult.hpp b/dart/collision/DistanceResult.hpp index f200c7945aa96..18b94cf3de9d7 100644 --- a/dart/collision/DistanceResult.hpp +++ b/dart/collision/DistanceResult.hpp @@ -49,16 +49,11 @@ struct DistanceResult const dynamics::ShapeFrame* mShapeFrame1; const dynamics::ShapeFrame* mShapeFrame2; - void clear() - { - mMinimumDistance = 0.0; + DistanceResult(); - mNearestPoint1.setZero(); - mNearestPoint2.setZero(); + void clear(); - mShapeFrame1 = nullptr; - mShapeFrame2 = nullptr; - } + bool found() const; }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index d1e0fed252375..18fc534499518 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -45,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" @@ -86,6 +87,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, @@ -1105,30 +1113,63 @@ bool distanceCallback( void* ddata, fcl::FCL_REAL& dist) { - auto* distdata = static_cast(ddata); + auto* distData = static_cast(ddata); - const auto& fclRequest = distdata->fclRequest; - auto& fclResult = distdata->fclResult; + 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) + if (distData->done) { dist = fclResult.min_distance; 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); - dist = fclResult.min_distance; + if (result) + { + interpreteDistanceResult(fclResult, o1, o2, option, *result); + } + else + { + // TODO(JS): Not implemented + } // TODO(JS): the data should be transformed into the DART's data structure if (dist <= 0) { // in collision or in touch - return true; + distData->done = true; } - return distdata->done; + return distData->done; } //============================================================================== @@ -1381,6 +1422,37 @@ void postProcessDART( } } +//============================================================================== +void interpreteDistanceResult( + const fcl::DistanceResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const DistanceOption& option, + DistanceResult& result) +{ + result.mMinimumDistance = fclResult.min_distance; + + 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.mShapeFrame1 = userData1->mCollisionObject->getShapeFrame(); + result.mShapeFrame2 = userData2->mCollisionObject->getShapeFrame(); + + if (option.enableNearestPoints) + { + result.mNearestPoint1 + = FCLTypes::convertVector3(fclResult.nearest_points[0]); + result.mNearestPoint2 + = FCLTypes::convertVector3(fclResult.nearest_points[1]); + } +} + //============================================================================== int evalContactPosition( const fcl::Contact& fclContact, diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index f6e0c5bb5d16f..f6d0a5bd458d1 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -110,14 +110,14 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited bool distance( CollisionGroup* group, - const DistanceOption& option = DistanceOption(nullptr), + const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited bool distance( CollisionGroup* group1, CollisionGroup* group2, - const DistanceOption& option = DistanceOption(nullptr), + const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr) override; /// Set primitive shape type diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index afbc9fa80b82b..55a3371255336 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -43,9 +43,123 @@ TEST(Distance, Basic) auto bulletCd = collision::BulletCollisionDetector::create(); auto dartCd = collision::DARTCollisionDetector::create(); - EXPECT_TRUE(fclCd->distance(nullptr)); - EXPECT_FALSE(bulletCd->distance(nullptr)); - EXPECT_FALSE(dartCd->distance(nullptr)); +// EXPECT_TRUE(fclCd->distance(nullptr)); +// EXPECT_FALSE(bulletCd->distance(nullptr)); +// EXPECT_FALSE(dartCd->distance(nullptr)); +} + +//============================================================================== +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)); + EXPECT_FALSE(group12->distance(option, &result)); + EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.0); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + EXPECT_FALSE(group12->distance(option, &result)); + EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.25); +} + +//============================================================================== +TEST(Distance, SphereSphere) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + testSphereSphere(fcl_mesh_dart); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testSphereSphere(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testSphereSphere(dart); +} + +//============================================================================== +void testBoxBox(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 BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, 0.0); + Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.0, 0.0); + simpleFrame1->setTranslation(pos1); + simpleFrame2->setTranslation(pos2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + + collision::DistanceOption option; + collision::DistanceResult result; + +// EXPECT_TRUE(group1->distance(group2.get(), option, &result)); + + Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); + Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); + + +} + +//============================================================================== +TEST(Distance, BoxBox) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + testBoxBox(fcl_mesh_dart); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testBoxBox(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testBoxBox(dart); } //============================================================================== From 784bcbb3a0e30fb22247f9a7d62d5a1991da1911 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 14:00:45 -0400 Subject: [PATCH 03/10] Distance enable/disable computing nearest points --- dart/collision/CollisionDetector.cpp | 8 +- dart/collision/CollisionDetector.hpp | 4 +- dart/collision/CollisionGroup.cpp | 4 +- dart/collision/CollisionGroup.hpp | 4 +- dart/collision/fcl/FCLCollisionDetector.cpp | 45 +++++--- dart/collision/fcl/FCLCollisionDetector.hpp | 4 +- unittests/testDistance.cpp | 116 +++++++++++++++++++- 7 files changed, 149 insertions(+), 36 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index bc2d95017f019..d9df6931a5d55 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -50,7 +50,7 @@ CollisionDetector::createCollisionGroupAsSharedPtr() } //============================================================================== -bool CollisionDetector::distance( +void CollisionDetector::distance( CollisionGroup* /*group*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) @@ -60,11 +60,11 @@ bool CollisionDetector::distance( // Note: Only FCL supports distance query. - return false; + return; } //============================================================================== -bool CollisionDetector::distance(CollisionGroup* /*group1*/, +void CollisionDetector::distance(CollisionGroup* /*group1*/, CollisionGroup* /*group2*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) @@ -74,7 +74,7 @@ bool CollisionDetector::distance(CollisionGroup* /*group1*/, // Note: Only FCL supports distance query. - return false; + return; } //============================================================================== diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index ec285c42fada8..db5b1e94935bf 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -112,13 +112,13 @@ class CollisionDetector : public std::enable_shared_from_this CollisionResult* result = nullptr) = 0; /// Perform collision check for a single group. - virtual bool distance( + virtual void distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr); /// Perform collision check for two groups. - virtual bool distance( + virtual void distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option = DistanceOption(false, nullptr), diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index bfdde3ba240c0..ff32ad5ab08ea 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -177,14 +177,14 @@ bool CollisionGroup::collide( } //============================================================================== -bool CollisionGroup::distance( +void CollisionGroup::distance( const DistanceOption& option, DistanceResult* result) { return mCollisionDetector->distance(this, option, result); } //============================================================================== -bool CollisionGroup::distance( +void CollisionGroup::distance( CollisionGroup* otherGroup, const DistanceOption& option, DistanceResult* result) diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 9c96462d210e2..2f3a92bb5c2de 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -183,7 +183,7 @@ class CollisionGroup CollisionResult* result = nullptr); /// Perform distance check within this CollisionGroup. - bool distance( + void distance( const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr); @@ -191,7 +191,7 @@ class CollisionGroup /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool distance( + void distance( CollisionGroup* otherGroup, const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr); diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 18fc534499518..874af668176dd 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -128,7 +128,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, @@ -198,15 +201,6 @@ struct FCLCollisionCallbackData struct FCLDistanceCallbackData { - FCLDistanceCallbackData( - const DistanceOption& option, DistanceResult* result) - : option(option), - result(result), - done(false) - { - // convertOption(...); - } - /// FCL distance request fcl::DistanceRequest fclRequest; @@ -221,6 +215,15 @@ struct FCLDistanceCallbackData /// @brief Whether the distance iteration can stop bool done; + + FCLDistanceCallbackData( + const DistanceOption& option, DistanceResult* result) + : option(option), + result(result), + done(false) + { + convertOption(option, fclRequest); + } }; //============================================================================== @@ -756,7 +759,7 @@ bool FCLCollisionDetector::collide( } //============================================================================== -bool FCLCollisionDetector::distance( +void FCLCollisionDetector::distance( CollisionGroup* group, const DistanceOption& option, DistanceResult* result) @@ -765,7 +768,7 @@ bool FCLCollisionDetector::distance( result->clear(); if (!checkGroupValidity(this, group)) - return false; + return; auto casted = static_cast(group); casted->updateEngineData(); @@ -774,11 +777,11 @@ bool FCLCollisionDetector::distance( casted->getFCLCollisionManager()->distance(&distData, distanceCallback); - return false; // TODO(JS): not sure what should be returned (if found any?) + return; } //============================================================================== -bool FCLCollisionDetector::distance( +void FCLCollisionDetector::distance( CollisionGroup* /*group1*/, CollisionGroup* /*group2*/, const DistanceOption& /*option*/, @@ -786,7 +789,7 @@ bool FCLCollisionDetector::distance( { // TODO(JS): Not implemented - return false; + return; } //============================================================================== @@ -1578,15 +1581,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 f6d0a5bd458d1..0e0857c4375cf 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -108,13 +108,13 @@ class FCLCollisionDetector : public CollisionDetector CollisionResult* result = nullptr) override; // Documentation inherited - bool distance( + void distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited - bool distance( + void distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option = DistanceOption(false, nullptr), diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index 55a3371255336..edfc363f779b6 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -81,21 +81,27 @@ void testSphereSphere(const std::shared_ptr& cd, result.clear(); simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); - EXPECT_FALSE(group12->distance(option, &result)); + group12->distance(option, &result); EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.0); + EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() + || result.mShapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() + || result.mShapeFrame2 == simpleFrame2.get()); + EXPECT_EQ(result.mNearestPoint1, Eigen::Vector3d::Zero()); + EXPECT_EQ(result.mNearestPoint2, Eigen::Vector3d::Zero()); result.clear(); simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); simpleFrame2->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); - EXPECT_FALSE(group12->distance(option, &result)); + group12->distance(option, &result); EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.25); } //============================================================================== TEST(Distance, SphereSphere) { - auto fcl_mesh_dart = FCLCollisionDetector::create(); - testSphereSphere(fcl_mesh_dart); + auto fcl = FCLCollisionDetector::create(); + testSphereSphere(fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); @@ -150,8 +156,8 @@ void testBoxBox(const std::shared_ptr& cd, //============================================================================== TEST(Distance, BoxBox) { - auto fcl_mesh_dart = FCLCollisionDetector::create(); - testBoxBox(fcl_mesh_dart); + auto fcl = FCLCollisionDetector::create(); + testBoxBox(fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); @@ -162,6 +168,104 @@ TEST(Distance, BoxBox) testBoxBox(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.mMinimumDistance, 0.0); + EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() + || result.mShapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() + || result.mShapeFrame2 == simpleFrame2.get()); + EXPECT_EQ(result.mNearestPoint1, Eigen::Vector3d::Zero()); + EXPECT_EQ(result.mNearestPoint2, 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.mMinimumDistance, 0.0); + EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() + || result.mShapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() + || result.mShapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.mNearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.mNearestPoint2.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.mMinimumDistance, 0.25); + EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() + || result.mShapeFrame1 == simpleFrame2.get()); + EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() + || result.mShapeFrame2 == simpleFrame2.get()); + EXPECT_TRUE( + result.mNearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) + || result.mNearestPoint1.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol)); + EXPECT_TRUE( + result.mNearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) + || result.mNearestPoint2.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); +} + //============================================================================== int main(int argc, char* argv[]) { From a2a226356a166666d2ff73ffaed31e25792e0ace Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 14:23:50 -0400 Subject: [PATCH 04/10] Implement group-group distance check and deprecate [Option/Result].hpp (use Collision[Option/Result].hpp instead) --- dart/collision/CMakeLists.txt | 2 + dart/collision/CollisionDetector.hpp | 4 +- dart/collision/CollisionGroup.hpp | 4 +- .../{Option.cpp => CollisionOption.cpp} | 2 +- dart/collision/CollisionOption.hpp | 71 +++++ .../{Result.cpp => CollisionResult.cpp} | 4 +- dart/collision/CollisionResult.hpp | 110 +++++++ dart/collision/DistanceResult.cpp | 22 +- dart/collision/DistanceResult.hpp | 22 +- dart/collision/Option.hpp | 37 +-- dart/collision/Result.hpp | 76 +---- dart/collision/fcl/FCLCollisionDetector.cpp | 52 ++-- dart/simulation/World.hpp | 2 +- unittests/testDistance.cpp | 268 ++++++++++++------ 14 files changed, 432 insertions(+), 244 deletions(-) rename dart/collision/{Option.cpp => CollisionOption.cpp} (97%) create mode 100644 dart/collision/CollisionOption.hpp rename dart/collision/{Result.cpp => CollisionResult.cpp} (99%) create mode 100644 dart/collision/CollisionResult.hpp 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 db5b1e94935bf..0613a680af190 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -38,8 +38,8 @@ #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" diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 2f3a92bb5c2de..c5be856208bd4 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -35,8 +35,8 @@ #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" 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/DistanceResult.cpp b/dart/collision/DistanceResult.cpp index ff67990c6df07..e0b7fc8ed03a6 100644 --- a/dart/collision/DistanceResult.cpp +++ b/dart/collision/DistanceResult.cpp @@ -36,11 +36,11 @@ namespace collision { //============================================================================== DistanceResult::DistanceResult() - : mMinimumDistance(0.0), - mNearestPoint1(Eigen::Vector3d::Zero()), - mNearestPoint2(Eigen::Vector3d::Zero()), - mShapeFrame1(nullptr), - mShapeFrame2(nullptr) + : minimumDistance(0.0), + nearestPoint1(Eigen::Vector3d::Zero()), + nearestPoint2(Eigen::Vector3d::Zero()), + shapeFrame1(nullptr), + shapeFrame2(nullptr) { // Do nothing } @@ -48,19 +48,19 @@ DistanceResult::DistanceResult() //============================================================================== void DistanceResult::clear() { - mMinimumDistance = 0.0; + minimumDistance = 0.0; - mNearestPoint1.setZero(); - mNearestPoint2.setZero(); + nearestPoint1.setZero(); + nearestPoint2.setZero(); - mShapeFrame1 = nullptr; - mShapeFrame2 = nullptr; + shapeFrame1 = nullptr; + shapeFrame2 = nullptr; } //============================================================================== bool DistanceResult::found() const { - if (!mShapeFrame1 || !mShapeFrame2) + if (!shapeFrame1 || !shapeFrame2) return false; return true; diff --git a/dart/collision/DistanceResult.hpp b/dart/collision/DistanceResult.hpp index 18b94cf3de9d7..06f21b9806785 100644 --- a/dart/collision/DistanceResult.hpp +++ b/dart/collision/DistanceResult.hpp @@ -32,27 +32,33 @@ #ifndef DART_COLLISION_DISTANCE_RESULT_HPP_ #define DART_COLLISION_DISTANCE_RESULT_HPP_ -#include -#include -#include "dart/collision/Contact.hpp" +#include namespace dart { + +namespace dynamics { +class ShapeFrame; +} // namesapce dynamics + namespace collision { struct DistanceResult { - double mMinimumDistance; + double minimumDistance; - Eigen::Vector3d mNearestPoint1; - Eigen::Vector3d mNearestPoint2; + Eigen::Vector3d nearestPoint1; + Eigen::Vector3d nearestPoint2; - const dynamics::ShapeFrame* mShapeFrame1; - const dynamics::ShapeFrame* mShapeFrame2; + const dynamics::ShapeFrame* shapeFrame1; + const dynamics::ShapeFrame* shapeFrame2; + /// Constructor DistanceResult(); + /// Clear (initialize) the result void clear(); + /// Return true if the result is valid bool found() const; }; 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/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 874af668176dd..45aeb19296310 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -782,12 +782,31 @@ void FCLCollisionDetector::distance( //============================================================================== void FCLCollisionDetector::distance( - CollisionGroup* /*group1*/, - CollisionGroup* /*group2*/, - const DistanceOption& /*option*/, - DistanceResult* /*result*/) + CollisionGroup* group1, + CollisionGroup* group2, + const DistanceOption& option, + DistanceResult* result) { - // TODO(JS): Not implemented + if (result) + result->clear(); + + if (!checkGroupValidity(this, group1)) + return; + + if (!checkGroupValidity(this, group2)) + return; + + 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; } @@ -1156,21 +1175,18 @@ bool distanceCallback( fcl::distance(o1, o2, fclRequest, fclResult); if (result) - { interpreteDistanceResult(fclResult, o1, o2, option, *result); - } - else - { - // TODO(JS): Not implemented - } - - // TODO(JS): the data should be transformed into the DART's data structure + // TODO(JS): Not sure if nullptr result would make any sense for distance + // check if (dist <= 0) { // in collision or in touch distData->done = true; } + // TODO(JS): Do we don't care what's the actual minimum distance when + // collision is found? Or should we continue to the narrow check to check + // every pairs? return distData->done; } @@ -1433,7 +1449,7 @@ void interpreteDistanceResult( const DistanceOption& option, DistanceResult& result) { - result.mMinimumDistance = fclResult.min_distance; + result.minimumDistance = fclResult.min_distance; const auto* userData1 = static_cast(o1->getUserData()); @@ -1444,14 +1460,14 @@ void interpreteDistanceResult( assert(userData1->mCollisionObject); assert(userData2->mCollisionObject); - result.mShapeFrame1 = userData1->mCollisionObject->getShapeFrame(); - result.mShapeFrame2 = userData2->mCollisionObject->getShapeFrame(); + result.shapeFrame1 = userData1->mCollisionObject->getShapeFrame(); + result.shapeFrame2 = userData2->mCollisionObject->getShapeFrame(); if (option.enableNearestPoints) { - result.mNearestPoint1 + result.nearestPoint1 = FCLTypes::convertVector3(fclResult.nearest_points[0]); - result.mNearestPoint2 + result.nearestPoint2 = FCLTypes::convertVector3(fclResult.nearest_points[1]); } } 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/unittests/testDistance.cpp b/unittests/testDistance.cpp index edfc363f779b6..33e7be142a290 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -37,20 +37,8 @@ using namespace dart; //============================================================================== -TEST(Distance, Basic) -{ - auto fclCd = collision::FCLCollisionDetector::create(); - auto bulletCd = collision::BulletCollisionDetector::create(); - auto dartCd = collision::DARTCollisionDetector::create(); - -// EXPECT_TRUE(fclCd->distance(nullptr)); -// EXPECT_FALSE(bulletCd->distance(nullptr)); -// EXPECT_FALSE(dartCd->distance(nullptr)); -} - -//============================================================================== -void testSphereSphere(const std::shared_ptr& cd, - double tol = 1e-12) +void testBasicInterface(const std::shared_ptr& cd, + double tol = 1e-12) { if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) { @@ -76,45 +64,77 @@ void testSphereSphere(const std::shared_ptr& cd, EXPECT_EQ(group12->getNumShapeFrames(), 2u); collision::DistanceOption option; + option.enableNearestPoints = true; + EXPECT_TRUE(option.distanceFilter == nullptr); + EXPECT_TRUE(option.enableNearestPoints == true); + 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)); - group12->distance(option, &result); - EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.0); - EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() - || result.mShapeFrame1 == simpleFrame2.get()); - EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() - || result.mShapeFrame2 == simpleFrame2.get()); - EXPECT_EQ(result.mNearestPoint1, Eigen::Vector3d::Zero()); - EXPECT_EQ(result.mNearestPoint2, Eigen::Vector3d::Zero()); + group1->distance(group2.get(), option, &result); + EXPECT_DOUBLE_EQ(result.minimumDistance, 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(1.0, 0.0, 0.0)); + simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); group12->distance(option, &result); - EXPECT_DOUBLE_EQ(result.mMinimumDistance, 0.25); + EXPECT_DOUBLE_EQ(result.minimumDistance, 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.minimumDistance, 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, SphereSphere) +TEST(Distance, testBasicInterface) { auto fcl = FCLCollisionDetector::create(); - testSphereSphere(fcl); + testBasicInterface(fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); - testSphereSphere(bullet); + testBasicInterface(bullet); #endif auto dart = DARTCollisionDetector::create(); - testSphereSphere(dart); + testBasicInterface(dart); } //============================================================================== -void testBoxBox(const std::shared_ptr& cd, - double tol = 1e-12) +void testOptions(const std::shared_ptr& cd, + double tol = 1e-12) { if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) { @@ -126,51 +146,93 @@ void testBoxBox(const std::shared_ptr& cd, auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); - ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + 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); - Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, 0.0); - Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.0, 0.0); - simpleFrame1->setTranslation(pos1); - simpleFrame2->setTranslation(pos2); - 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(group1->distance(group2.get(), option, &result)); + EXPECT_TRUE(option.distanceFilter == nullptr); + EXPECT_TRUE(option.enableNearestPoints == false); - Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); - Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); + 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.minimumDistance, 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.minimumDistance, 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.minimumDistance, 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, BoxBox) +TEST(Distance, Options) { auto fcl = FCLCollisionDetector::create(); - testBoxBox(fcl); + testOptions(fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); - testBoxBox(bullet); + testOptions(bullet); #endif auto dart = DARTCollisionDetector::create(); - testBoxBox(dart); + testOptions(dart); } //============================================================================== -void testOptions(const std::shared_ptr& cd, - double tol = 1e-12) +void testSphereSphere(const std::shared_ptr& cd, + double tol = 1e-12) { if (cd->getType() != collision::FCLCollisionDetector::getStaticType()) { @@ -198,72 +260,94 @@ void testOptions(const std::shared_ptr& cd, 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.mMinimumDistance, 0.0); - EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() - || result.mShapeFrame1 == simpleFrame2.get()); - EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() - || result.mShapeFrame2 == simpleFrame2.get()); - EXPECT_EQ(result.mNearestPoint1, Eigen::Vector3d::Zero()); - EXPECT_EQ(result.mNearestPoint2, Eigen::Vector3d::Zero()); - EXPECT_TRUE(result.found() == true); + EXPECT_DOUBLE_EQ(result.minimumDistance, 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()); - 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.mMinimumDistance, 0.0); - EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() - || result.mShapeFrame1 == simpleFrame2.get()); - EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() - || result.mShapeFrame2 == simpleFrame2.get()); - EXPECT_TRUE( - result.mNearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)); - EXPECT_TRUE( - result.mNearestPoint2.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.mMinimumDistance, 0.25); - EXPECT_TRUE(result.mShapeFrame1 == simpleFrame1.get() - || result.mShapeFrame1 == simpleFrame2.get()); - EXPECT_TRUE(result.mShapeFrame2 == simpleFrame1.get() - || result.mShapeFrame2 == simpleFrame2.get()); - EXPECT_TRUE( - result.mNearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) - || result.mNearestPoint1.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol)); - EXPECT_TRUE( - result.mNearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol) - || result.mNearestPoint2.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol)); - EXPECT_TRUE(result.found() == true); + EXPECT_DOUBLE_EQ(result.minimumDistance, 0.25); } //============================================================================== -TEST(Distance, Options) +TEST(Distance, SphereSphere) { auto fcl = FCLCollisionDetector::create(); - testOptions(fcl); + testSphereSphere(fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); - testOptions(bullet); + testSphereSphere(bullet); #endif auto dart = DARTCollisionDetector::create(); - testOptions(dart); + testSphereSphere(dart); +} + +//============================================================================== +void testBoxBox(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 BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, 0.0); + Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.0, 0.0); + simpleFrame1->setTranslation(pos1); + simpleFrame2->setTranslation(pos2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + + collision::DistanceOption option; + collision::DistanceResult result; + +// EXPECT_TRUE(group1->distance(group2.get(), option, &result)); + + Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); + Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); + + +} + +//============================================================================== +TEST(Distance, BoxBox) +{ + auto fcl = FCLCollisionDetector::create(); + testBoxBox(fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testBoxBox(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testBoxBox(dart); } //============================================================================== From fd53ab64bcf12e017e809c05a68bd64a9df74186 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 14:56:07 -0400 Subject: [PATCH 05/10] Remove incomplete tests --- unittests/testDistance.cpp | 66 ++------------------------------------ 1 file changed, 2 insertions(+), 64 deletions(-) diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index 33e7be142a290..8fc5352a6925d 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -269,14 +269,8 @@ void testSphereSphere(const std::shared_ptr& cd, || 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()); - - 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.minimumDistance, 0.25); + EXPECT_TRUE(result.nearestPoint1.isApprox(Eigen::Vector3d::Zero(), tol)); + EXPECT_TRUE(result.nearestPoint2.isApprox(Eigen::Vector3d::Zero(), tol)); } //============================================================================== @@ -294,62 +288,6 @@ TEST(Distance, SphereSphere) testSphereSphere(dart); } -//============================================================================== -void testBoxBox(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 BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); - simpleFrame1->setShape(shape1); - simpleFrame2->setShape(shape2); - - Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, 0.0); - Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.0, 0.0); - simpleFrame1->setTranslation(pos1); - simpleFrame2->setTranslation(pos2); - - auto group1 = cd->createCollisionGroup(simpleFrame1.get()); - auto group2 = cd->createCollisionGroup(simpleFrame2.get()); - - EXPECT_EQ(group1->getNumShapeFrames(), 1u); - EXPECT_EQ(group2->getNumShapeFrames(), 1u); - - collision::DistanceOption option; - collision::DistanceResult result; - -// EXPECT_TRUE(group1->distance(group2.get(), option, &result)); - - Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); - Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); - - -} - -//============================================================================== -TEST(Distance, BoxBox) -{ - auto fcl = FCLCollisionDetector::create(); - testBoxBox(fcl); - -#if HAVE_BULLET_COLLISION - auto bullet = BulletCollisionDetector::create(); - testBoxBox(bullet); -#endif - - auto dart = DARTCollisionDetector::create(); - testBoxBox(dart); -} - //============================================================================== int main(int argc, char* argv[]) { From a4007cfadb835f9b432dc2be3806e5bce0361495 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 16:05:59 -0400 Subject: [PATCH 06/10] Make distance() to pure virtual and add minimum distance option --- dart/collision/CollisionDetector.cpp | 28 ------------------- dart/collision/CollisionDetector.hpp | 8 +++--- dart/collision/CollisionGroup.hpp | 4 +-- dart/collision/DistanceOption.cpp | 5 +++- dart/collision/DistanceOption.hpp | 3 ++ .../bullet/BulletCollisionDetector.cpp | 25 +++++++++++++++++ .../bullet/BulletCollisionDetector.hpp | 13 +++++++++ dart/collision/dart/DARTCollisionDetector.cpp | 25 +++++++++++++++++ dart/collision/dart/DARTCollisionDetector.hpp | 13 +++++++++ dart/collision/fcl/FCLCollisionDetector.cpp | 8 +----- dart/collision/fcl/FCLCollisionDetector.hpp | 4 +-- unittests/testDistance.cpp | 3 +- 12 files changed, 94 insertions(+), 45 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index d9df6931a5d55..d8acb67c3ec09 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -49,34 +49,6 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } -//============================================================================== -void CollisionDetector::distance( - CollisionGroup* /*group*/, - const DistanceOption& /*option*/, - DistanceResult* /*result*/) -{ - dtwarn << "[CollisionDetector::distance] [" << getType() << "] does not " - << "support (signed) distance queries. Ignoring this query.\n"; - - // Note: Only FCL supports distance query. - - return; -} - -//============================================================================== -void CollisionDetector::distance(CollisionGroup* /*group1*/, - CollisionGroup* /*group2*/, - const DistanceOption& /*option*/, - DistanceResult* /*result*/) -{ - dtwarn << "[CollisionDetector::distance] [" << getType() << "] does not " - << "support (signed) distance queries. Ignoring this query.\n"; - - // Note: Only FCL supports distance query. - - return; -} - //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 0613a680af190..8f19440a486dc 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -114,15 +114,15 @@ class CollisionDetector : public std::enable_shared_from_this /// Perform collision check for a single group. virtual void distance( CollisionGroup* group, - const DistanceOption& option = DistanceOption(false, nullptr), - DistanceResult* result = nullptr); + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) = 0; /// Perform collision check for two groups. virtual void distance( CollisionGroup* group1, CollisionGroup* group2, - const DistanceOption& option = DistanceOption(false, nullptr), - DistanceResult* result = nullptr); + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) = 0; protected: diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index c5be856208bd4..7fd2a8007e128 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -184,7 +184,7 @@ class CollisionGroup /// Perform distance check within this CollisionGroup. void distance( - const DistanceOption& option = DistanceOption(false, nullptr), + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr); /// Perform distance check with other CollisionGroup. @@ -193,7 +193,7 @@ class CollisionGroup /// from this CollisionObject engine. void distance( CollisionGroup* otherGroup, - const DistanceOption& option = DistanceOption(false, nullptr), + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr); protected: diff --git a/dart/collision/DistanceOption.cpp b/dart/collision/DistanceOption.cpp index 7ebbc6cf20a27..ebfa1f6bd16be 100644 --- a/dart/collision/DistanceOption.cpp +++ b/dart/collision/DistanceOption.cpp @@ -35,9 +35,12 @@ namespace dart { namespace collision { //============================================================================== -DistanceOption::DistanceOption(bool enableNearestPoints, +DistanceOption::DistanceOption( + bool enableNearestPoints, + double minimumDistance, const std::shared_ptr& distanceFilter) : enableNearestPoints(enableNearestPoints), + minimumDistance(minimumDistance), distanceFilter(distanceFilter) { // Do nothing diff --git a/dart/collision/DistanceOption.hpp b/dart/collision/DistanceOption.hpp index 523998e54bdd1..fb2b7b2f9deae 100644 --- a/dart/collision/DistanceOption.hpp +++ b/dart/collision/DistanceOption.hpp @@ -44,12 +44,15 @@ struct DistanceOption { bool enableNearestPoints; + double minimumDistance; + /// Distance filter std::shared_ptr distanceFilter; /// Constructor DistanceOption( bool enableNearestPoints = false, + double minimumDistance = 0.0, const std::shared_ptr& distanceFilter = nullptr); }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index d424aa0518264..d57ca6a42e667 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -310,6 +310,31 @@ bool BulletCollisionDetector::collide( } } +//============================================================================== +void BulletCollisionDetector::distance( + CollisionGroup* /*group*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[BulletCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Ignoring this query.\n"; + + return; +} + +//============================================================================== +void BulletCollisionDetector::distance( + CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[BulletCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Ignoring this query.\n"; + + return; +} + //============================================================================== BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 7711d9b0dfaed..63563cdc68e12 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 + void distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + void 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 b051b40e58d32..62f450e5fed6a 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -223,6 +223,31 @@ bool DARTCollisionDetector::collide( return collisionFound; } +//============================================================================== +void DARTCollisionDetector::distance( + CollisionGroup* /*group*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[DARTCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Ignoring this query.\n"; + + return; +} + +//============================================================================== +void DARTCollisionDetector::distance( + CollisionGroup* /*group1*/, + CollisionGroup* /*group2*/, + const DistanceOption& /*option*/, + DistanceResult* /*result*/) +{ + dtwarn << "[DARTCollisionDetector::distance] This collision detector does " + << "not support (signed) distance queries. Ignoring this query.\n"; + + return; +} + //============================================================================== DARTCollisionDetector::DARTCollisionDetector() : CollisionDetector() diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 336b2caa72fca..b4a1c8cbab08a 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 + void distance( + CollisionGroup* group, + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), + DistanceResult* result = nullptr) override; + + // Documentation inherited + void 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 1494279fb0fed..83422474a20ff 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -1187,14 +1187,8 @@ bool distanceCallback( // TODO(JS): Not sure if nullptr result would make any sense for distance // check - if (dist <= 0) - { - // in collision or in touch + if (dist <= option.minimumDistance) distData->done = true; - } - // TODO(JS): Do we don't care what's the actual minimum distance when - // collision is found? Or should we continue to the narrow check to check - // every pairs? return distData->done; } diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 447c1b2d94dfc..071d9e034cd43 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -111,14 +111,14 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited void distance( CollisionGroup* group, - const DistanceOption& option = DistanceOption(false, nullptr), + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited void distance( CollisionGroup* group1, CollisionGroup* group2, - const DistanceOption& option = DistanceOption(false, nullptr), + const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; /// Set primitive shape type diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index 8fc5352a6925d..0635f32fd35b1 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -65,8 +65,9 @@ void testBasicInterface(const std::shared_ptr& cd, collision::DistanceOption option; option.enableNearestPoints = true; - EXPECT_TRUE(option.distanceFilter == nullptr); EXPECT_TRUE(option.enableNearestPoints == true); + EXPECT_TRUE(option.minimumDistance == 0.0); + EXPECT_TRUE(option.distanceFilter == nullptr); collision::DistanceResult result; EXPECT_TRUE(result.found() == false); From 19d06e159cb5b1f2e17087495cc6e6e729fdbfe4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 17:30:01 -0400 Subject: [PATCH 07/10] Update comments for distance API --- dart/collision/CollisionDetector.hpp | 23 +++++++++++++++++-- dart/collision/CollisionGroup.hpp | 25 +++++++++++++++++---- dart/collision/DistanceFilter.hpp | 2 +- dart/collision/DistanceOption.cpp | 2 +- dart/collision/DistanceOption.hpp | 11 ++++++--- dart/collision/DistanceResult.cpp | 6 ++--- dart/collision/DistanceResult.hpp | 20 ++++++++++++++--- dart/collision/fcl/FCLCollisionDetector.cpp | 2 +- unittests/testDistance.cpp | 2 +- 9 files changed, 74 insertions(+), 19 deletions(-) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 8f19440a486dc..47d3309f6daa5 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -111,13 +111,32 @@ class CollisionDetector : public std::enable_shared_from_this const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) = 0; - /// Perform collision check for a single group. + /// Measure the minimum signed distance between two Shapes of ShapeFrames in + /// the given CollisionGroup, and store which ShapeFrames are. The results are + /// stored in the given DistanceResult. + /// + /// The minimum signed distance meaning can be varied depending on the + /// DistanceOption::minimumDistanceThreshold passing in. By default, the + /// minimum signed distance means either of the non-negative minimum positive + /// (when there are no penetrations between shapes) or the firstly found + /// negative distance (penetration). Please see DistanceOption for the details + /// and other options. virtual void distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) = 0; - /// Perform collision check for two groups. + /// Measure the minimum signed distance between two Shapes of ShapeFrames one + /// from group1 and one from group2, and store which ShapeFrames are. Note + /// that the distance between ShapeFrames within the same CollisionGroup are + /// note measured. The results are stored in the given DistanceResult. + /// + /// The minimum signed distance meaning can be varied depending on the + /// DistanceOption::minimumDistanceThreshold passing in. By default, the + /// minimum signed distance means either of the non-negative minimum positive + /// (when there are no penetrations between shapes) or the firstly found + /// negative distance (penetration). Please see DistanceOption for the details + /// and other options. virtual void distance( CollisionGroup* group1, CollisionGroup* group2, diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 7fd2a8007e128..532d2de3f7c35 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -182,15 +182,32 @@ class CollisionGroup const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr); - /// Perform distance check within this CollisionGroup. + /// Measure the minimum signed distance between two Shapes of ShapeFrames in + /// this CollisionGroup, and store which ShapeFrames are. The results are + /// stored in the given DistanceResult. + /// + /// The minimum signed distance meaning can be varied depending on the + /// DistanceOption::minimumDistanceThreshold passing in. By default, the + /// minimum signed distance means either of the non-negative minimum positive + /// (when there are no penetrations between shapes) or the firstly found + /// negative distance (penetration). Please see DistanceOption for the details + /// and other options. void distance( const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr); - /// Perform distance check with other CollisionGroup. + /// Measure the minimum signed distance between two Shapes of ShapeFrames one + /// from this CollisionGroup and one from other CollisionGroup, and store + /// which ShapeFrames are. Note that the distance between ShapeFrames within + /// the same CollisionGroup are note measured. The results are stored in the + /// given DistanceResult. /// - /// Return false if the engine type of the other CollisionGroup is different - /// from this CollisionObject engine. + /// The minimum signed distance meaning can be varied depending on the + /// DistanceOption::minimumDistanceThreshold passing in. By default, the + /// minimum signed distance means either of the non-negative minimum positive + /// (when there are no penetrations between shapes) or the firstly found + /// negative distance (penetration). Please see DistanceOption for the details + /// and other options. void distance( CollisionGroup* otherGroup, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), diff --git a/dart/collision/DistanceFilter.hpp b/dart/collision/DistanceFilter.hpp index de262c8eb038d..4207abb13b851 100644 --- a/dart/collision/DistanceFilter.hpp +++ b/dart/collision/DistanceFilter.hpp @@ -51,7 +51,7 @@ struct DistanceFilter struct BodyNodeDistanceFilter : DistanceFilter { bool needDistance(const CollisionObject* object1, - const CollisionObject* object2) const override; + const CollisionObject* object2) const override; bool areAdjacentBodies(const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2) const; diff --git a/dart/collision/DistanceOption.cpp b/dart/collision/DistanceOption.cpp index ebfa1f6bd16be..9e068a3f26c62 100644 --- a/dart/collision/DistanceOption.cpp +++ b/dart/collision/DistanceOption.cpp @@ -40,7 +40,7 @@ DistanceOption::DistanceOption( double minimumDistance, const std::shared_ptr& distanceFilter) : enableNearestPoints(enableNearestPoints), - minimumDistance(minimumDistance), + minimumDistanceThreshold(minimumDistance), distanceFilter(distanceFilter) { // Do nothing diff --git a/dart/collision/DistanceOption.hpp b/dart/collision/DistanceOption.hpp index fb2b7b2f9deae..c1c38f55382c4 100644 --- a/dart/collision/DistanceOption.hpp +++ b/dart/collision/DistanceOption.hpp @@ -42,17 +42,22 @@ class DistanceFilter; struct DistanceOption { + /// Whether to return the nearest points bool enableNearestPoints; - double minimumDistance; + /// Stopping criteria for further distance check in broadphase. The distance + /// check stop as soon as a distance equal to or less than this value is + /// found. + double minimumDistanceThreshold; - /// Distance filter + /// Filter for excluding pairs of ShapeFrames from distance check. If nullptr, + /// every pairs of ShapeFrames in the CollisionGroup(s) are checked. std::shared_ptr distanceFilter; /// Constructor DistanceOption( bool enableNearestPoints = false, - double minimumDistance = 0.0, + double minimumDistanceThreshold = 0.0, const std::shared_ptr& distanceFilter = nullptr); }; diff --git a/dart/collision/DistanceResult.cpp b/dart/collision/DistanceResult.cpp index e0b7fc8ed03a6..6217a11cf9363 100644 --- a/dart/collision/DistanceResult.cpp +++ b/dart/collision/DistanceResult.cpp @@ -37,10 +37,10 @@ namespace collision { //============================================================================== DistanceResult::DistanceResult() : minimumDistance(0.0), - nearestPoint1(Eigen::Vector3d::Zero()), - nearestPoint2(Eigen::Vector3d::Zero()), shapeFrame1(nullptr), - shapeFrame2(nullptr) + shapeFrame2(nullptr), + nearestPoint1(Eigen::Vector3d::Zero()), + nearestPoint2(Eigen::Vector3d::Zero()) { // Do nothing } diff --git a/dart/collision/DistanceResult.hpp b/dart/collision/DistanceResult.hpp index 06f21b9806785..801fbf9a2e30b 100644 --- a/dart/collision/DistanceResult.hpp +++ b/dart/collision/DistanceResult.hpp @@ -44,14 +44,28 @@ namespace collision { struct DistanceResult { + /// Minimum "singed" distance between two Shapes. If the two Shapes are in + /// collision, the distance is negative. + /// + /// The minimum signed distance meaning can be varied depending on the + /// DistanceOption::minimumDistance. Please see + /// DistanceOption::minimumDistanceThreshold for further details. double minimumDistance; - Eigen::Vector3d nearestPoint1; - Eigen::Vector3d nearestPoint2; - + /// First ShapeFrame const dynamics::ShapeFrame* shapeFrame1; + + /// Second ShapeFrame const dynamics::ShapeFrame* shapeFrame2; + /// The nearest point on shapeFrame1. The point is calculated only when + /// DistanceOption::enableNearestPoints is true, which is false by default. + Eigen::Vector3d nearestPoint1; + + /// The nearest point on shapeFrame2. The point is calculated only when + /// DistanceOption::enableNearestPoints is true, which is false by default. + Eigen::Vector3d nearestPoint2; + /// Constructor DistanceResult(); diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 904d7e6097e5a..fdba0a886cb34 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -1175,7 +1175,7 @@ bool distanceCallback( // TODO(JS): Not sure if nullptr result would make any sense for distance // check - if (dist <= option.minimumDistance) + if (dist <= option.minimumDistanceThreshold) distData->done = true; return distData->done; diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index 0635f32fd35b1..b15d0e7f9cae1 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -66,7 +66,7 @@ void testBasicInterface(const std::shared_ptr& cd, collision::DistanceOption option; option.enableNearestPoints = true; EXPECT_TRUE(option.enableNearestPoints == true); - EXPECT_TRUE(option.minimumDistance == 0.0); + EXPECT_TRUE(option.minimumDistanceThreshold == 0.0); EXPECT_TRUE(option.distanceFilter == nullptr); collision::DistanceResult result; From a2881d9a390ff746b83e5dc3464838f6f012c35d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 17:50:48 -0400 Subject: [PATCH 08/10] Update changelog and more comments on distance API --- CHANGELOG.md | 6 +++++- dart/collision/DistanceResult.hpp | 10 ++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 945e1b8606cc9..6ce9c232afb31 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,9 +2,13 @@ ### DART 6.1.0 (2016-XX-XX) +* Collision detection + + * Added distance API: [#744](https://github.com/dartsim/dart/pull/744) + * Misc improvements and bug fixes - * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724) + * 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/DistanceResult.hpp b/dart/collision/DistanceResult.hpp index 801fbf9a2e30b..a5805b5f5671d 100644 --- a/dart/collision/DistanceResult.hpp +++ b/dart/collision/DistanceResult.hpp @@ -58,12 +58,14 @@ struct DistanceResult /// Second ShapeFrame const dynamics::ShapeFrame* shapeFrame2; - /// The nearest point on shapeFrame1. The point is calculated only when - /// DistanceOption::enableNearestPoints is true, which is false by default. + /// The nearest point on shapeFrame1 w.r.t. the world coordinates. The point + /// is calculated only when DistanceOption::enableNearestPoints is true, which + /// is false by default. Eigen::Vector3d nearestPoint1; - /// The nearest point on shapeFrame2. The point is calculated only when - /// DistanceOption::enableNearestPoints is true, which is false by default. + /// The nearest point on shapeFrame2 w.r.t. the world coordinates. The point + /// is calculated only when DistanceOption::enableNearestPoints is true, which + /// is false by default. Eigen::Vector3d nearestPoint2; /// Constructor From b38d9429f58b155e7cd690b92948509182feed29 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 6 Sep 2016 22:55:40 -0400 Subject: [PATCH 09/10] Fix typo --- CHANGELOG.md | 2 +- dart/dynamics/Shape.hpp | 2 +- tutorials/tutorialMultiPendulum-Finished.cpp | 2 +- tutorials/tutorialMultiPendulum.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4b2d58472fe59..a62ddf68764f4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,7 +25,7 @@ * 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) * 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 Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724) + * 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/dynamics/Shape.hpp b/dart/dynamics/Shape.hpp index b0fc9f4c5d21c..9af0341e40b80 100644 --- a/dart/dynamics/Shape.hpp +++ b/dart/dynamics/Shape.hpp @@ -156,7 +156,7 @@ class Shape : public virtual common::Subject private: /// \deprecated Deprecated in 6.1. Please use getType() instead. - /// \brief Type of primitive shpae + /// \brief Type of primitive shape ShapeType mType; }; 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()); From c239ad0b3d4cf30f51a2368980f12bcac4ac26e5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 26 Sep 2016 10:35:08 -0400 Subject: [PATCH 10/10] Update distance API according to the discussion made in #744 --- dart/collision/CollisionDetector.hpp | 41 ++++++----- dart/collision/CollisionGroup.cpp | 4 +- dart/collision/CollisionGroup.hpp | 42 ++++++----- dart/collision/DistanceOption.cpp | 4 +- dart/collision/DistanceOption.hpp | 27 ++++--- dart/collision/DistanceResult.cpp | 15 +++- dart/collision/DistanceResult.hpp | 70 ++++++++++++++----- .../bullet/BulletCollisionDetector.cpp | 12 ++-- .../bullet/BulletCollisionDetector.hpp | 4 +- dart/collision/dart/DARTCollisionDetector.cpp | 12 ++-- dart/collision/dart/DARTCollisionDetector.hpp | 4 +- dart/collision/fcl/FCLCollisionDetector.cpp | 31 ++++---- dart/collision/fcl/FCLCollisionDetector.hpp | 4 +- unittests/testDistance.cpp | 16 ++--- 14 files changed, 175 insertions(+), 111 deletions(-) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 47d3309f6daa5..e210657927c85 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -111,33 +111,32 @@ class CollisionDetector : public std::enable_shared_from_this const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) = 0; - /// Measure the minimum signed distance between two Shapes of ShapeFrames in - /// the given CollisionGroup, and store which ShapeFrames are. The results are - /// stored in the given DistanceResult. + /// Get the minimum signed distance between the Shape pairs in the given + /// CollisionGroup. /// - /// The minimum signed distance meaning can be varied depending on the - /// DistanceOption::minimumDistanceThreshold passing in. By default, the - /// minimum signed distance means either of the non-negative minimum positive - /// (when there are no penetrations between shapes) or the firstly found - /// negative distance (penetration). Please see DistanceOption for the details - /// and other options. - virtual void distance( + /// 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; - /// Measure the minimum signed distance between two Shapes of ShapeFrames one - /// from group1 and one from group2, and store which ShapeFrames are. Note - /// that the distance between ShapeFrames within the same CollisionGroup are - /// note measured. The results are stored in the given DistanceResult. + /// 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 minimum signed distance meaning can be varied depending on the - /// DistanceOption::minimumDistanceThreshold passing in. By default, the - /// minimum signed distance means either of the non-negative minimum positive - /// (when there are no penetrations between shapes) or the firstly found - /// negative distance (penetration). Please see DistanceOption for the details - /// and other options. - virtual void distance( + /// 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), diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index ff32ad5ab08ea..9b5914e44a40d 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -177,14 +177,14 @@ bool CollisionGroup::collide( } //============================================================================== -void CollisionGroup::distance( +double CollisionGroup::distance( const DistanceOption& option, DistanceResult* result) { return mCollisionDetector->distance(this, option, result); } //============================================================================== -void CollisionGroup::distance( +double CollisionGroup::distance( CollisionGroup* otherGroup, const DistanceOption& option, DistanceResult* result) diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 532d2de3f7c35..cfed379fab75e 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -182,33 +182,31 @@ class CollisionGroup const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr); - /// Measure the minimum signed distance between two Shapes of ShapeFrames in - /// this CollisionGroup, and store which ShapeFrames are. The results are - /// stored in the given DistanceResult. + /// Get the minimum signed distance between the Shape pairs in this + /// CollisionGroup. /// - /// The minimum signed distance meaning can be varied depending on the - /// DistanceOption::minimumDistanceThreshold passing in. By default, the - /// minimum signed distance means either of the non-negative minimum positive - /// (when there are no penetrations between shapes) or the firstly found - /// negative distance (penetration). Please see DistanceOption for the details - /// and other options. - void distance( + /// 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); - /// Measure the minimum signed distance between two Shapes of ShapeFrames one - /// from this CollisionGroup and one from other CollisionGroup, and store - /// which ShapeFrames are. Note that the distance between ShapeFrames within - /// the same CollisionGroup are note measured. The results are stored in the - /// given DistanceResult. + /// 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 minimum signed distance meaning can be varied depending on the - /// DistanceOption::minimumDistanceThreshold passing in. By default, the - /// minimum signed distance means either of the non-negative minimum positive - /// (when there are no penetrations between shapes) or the firstly found - /// negative distance (penetration). Please see DistanceOption for the details - /// and other options. - void distance( + /// 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); diff --git a/dart/collision/DistanceOption.cpp b/dart/collision/DistanceOption.cpp index 9e068a3f26c62..3ad424f8cf426 100644 --- a/dart/collision/DistanceOption.cpp +++ b/dart/collision/DistanceOption.cpp @@ -37,10 +37,10 @@ namespace collision { //============================================================================== DistanceOption::DistanceOption( bool enableNearestPoints, - double minimumDistance, + double minDistance, const std::shared_ptr& distanceFilter) : enableNearestPoints(enableNearestPoints), - minimumDistanceThreshold(minimumDistance), + distanceLowerBound(minDistance), distanceFilter(distanceFilter) { // Do nothing diff --git a/dart/collision/DistanceOption.hpp b/dart/collision/DistanceOption.hpp index c1c38f55382c4..2eb5f9bf8438e 100644 --- a/dart/collision/DistanceOption.hpp +++ b/dart/collision/DistanceOption.hpp @@ -42,22 +42,33 @@ class DistanceFilter; struct DistanceOption { - /// Whether to return the nearest points + /// Whether to calculate the nearest points. + /// + /// The default is false. + /// \sa DistanceResult::nearestPoint1, DistanceResult::nearestPoint2 bool enableNearestPoints; - /// Stopping criteria for further distance check in broadphase. The distance - /// check stop as soon as a distance equal to or less than this value is - /// found. - double minimumDistanceThreshold; + /// 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; - /// Filter for excluding pairs of ShapeFrames from distance check. If nullptr, - /// every pairs of ShapeFrames in the CollisionGroup(s) are checked. + /// 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 minimumDistanceThreshold = 0.0, + double distanceLowerBound = 0.0, const std::shared_ptr& distanceFilter = nullptr); }; diff --git a/dart/collision/DistanceResult.cpp b/dart/collision/DistanceResult.cpp index 6217a11cf9363..983be9341cb8f 100644 --- a/dart/collision/DistanceResult.cpp +++ b/dart/collision/DistanceResult.cpp @@ -31,12 +31,16 @@ #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() - : minimumDistance(0.0), + : minDistance(DART_DEFAULT_MIN_DISTANCE), + unclampedMinDistance(0.0), shapeFrame1(nullptr), shapeFrame2(nullptr), nearestPoint1(Eigen::Vector3d::Zero()), @@ -48,7 +52,8 @@ DistanceResult::DistanceResult() //============================================================================== void DistanceResult::clear() { - minimumDistance = 0.0; + minDistance = DART_DEFAULT_MIN_DISTANCE; + unclampedMinDistance = DART_DEFAULT_UNCLAMPED_MIN_DISTANCE; nearestPoint1.setZero(); nearestPoint2.setZero(); @@ -66,5 +71,11 @@ bool DistanceResult::found() const 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 index a5805b5f5671d..e42f5a3056cb1 100644 --- a/dart/collision/DistanceResult.hpp +++ b/dart/collision/DistanceResult.hpp @@ -44,38 +44,76 @@ namespace collision { struct DistanceResult { - /// Minimum "singed" distance between two Shapes. If the two Shapes are in - /// collision, the distance is negative. + /// Minimum \b singed distance between the checked Shape pairs. /// - /// The minimum signed distance meaning can be varied depending on the - /// DistanceOption::minimumDistance. Please see - /// DistanceOption::minimumDistanceThreshold for further details. - double minimumDistance; + /// 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 + /// First ShapeFrame of the minDistance + /// + /// If no shape pair was checked then shapeFrame1 will be nullptr. const dynamics::ShapeFrame* shapeFrame1; - /// Second ShapeFrame + /// Second ShapeFrame of the minDistance + /// + /// If no shape pair was checked then shapeFrame2 will be nullptr. const dynamics::ShapeFrame* shapeFrame2; - /// The nearest point on shapeFrame1 w.r.t. the world coordinates. The point - /// is calculated only when DistanceOption::enableNearestPoints is true, which - /// is false by default. + /// 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 shapeFrame2 w.r.t. the world coordinates. The point - /// is calculated only when DistanceOption::enableNearestPoints is true, which - /// is false by default. + /// 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 (initialize) the result + /// Clear the result void clear(); - /// Return true if the result is valid + /// 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 diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 49a02ecea3582..2836850c460cd 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -314,28 +314,28 @@ bool BulletCollisionDetector::collide( } //============================================================================== -void BulletCollisionDetector::distance( +double BulletCollisionDetector::distance( CollisionGroup* /*group*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) { dtwarn << "[BulletCollisionDetector::distance] This collision detector does " - << "not support (signed) distance queries. Ignoring this query.\n"; + << "not support (signed) distance queries. Returning 0.0.\n"; - return; + return 0.0; } //============================================================================== -void BulletCollisionDetector::distance( +double BulletCollisionDetector::distance( CollisionGroup* /*group1*/, CollisionGroup* /*group2*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) { dtwarn << "[BulletCollisionDetector::distance] This collision detector does " - << "not support (signed) distance queries. Ignoring this query.\n"; + << "not support (signed) distance queries. Returning.\n"; - return; + return 0.0; } //============================================================================== diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 63563cdc68e12..0fae18868592c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -83,13 +83,13 @@ class BulletCollisionDetector : public CollisionDetector CollisionResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 7bba1d520f0de..6e28052bf38b4 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -225,28 +225,28 @@ bool DARTCollisionDetector::collide( } //============================================================================== -void DARTCollisionDetector::distance( +double DARTCollisionDetector::distance( CollisionGroup* /*group*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) { dtwarn << "[DARTCollisionDetector::distance] This collision detector does " - << "not support (signed) distance queries. Ignoring this query.\n"; + << "not support (signed) distance queries. Returning 0.0.\n"; - return; + return 0.0; } //============================================================================== -void DARTCollisionDetector::distance( +double DARTCollisionDetector::distance( CollisionGroup* /*group1*/, CollisionGroup* /*group2*/, const DistanceOption& /*option*/, DistanceResult* /*result*/) { dtwarn << "[DARTCollisionDetector::distance] This collision detector does " - << "not support (signed) distance queries. Ignoring this query.\n"; + << "not support (signed) distance queries. Returning 0.0.\n"; - return; + return 0.0; } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index b4a1c8cbab08a..c316ac7482343 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -72,13 +72,13 @@ class DARTCollisionDetector : public CollisionDetector CollisionResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 1715126e0c0b3..39e595d504927 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -211,6 +211,10 @@ struct FCLDistanceCallbackData /// 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; @@ -756,7 +760,7 @@ bool FCLCollisionDetector::collide( } //============================================================================== -void FCLCollisionDetector::distance( +double FCLCollisionDetector::distance( CollisionGroup* group, const DistanceOption& option, DistanceResult* result) @@ -765,7 +769,7 @@ void FCLCollisionDetector::distance( result->clear(); if (!checkGroupValidity(this, group)) - return; + return 0.0; auto casted = static_cast(group); casted->updateEngineData(); @@ -774,11 +778,11 @@ void FCLCollisionDetector::distance( casted->getFCLCollisionManager()->distance(&distData, distanceCallback); - return; + return std::max(distData.unclampedMinDistance, option.distanceLowerBound); } //============================================================================== -void FCLCollisionDetector::distance( +double FCLCollisionDetector::distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option, @@ -788,10 +792,10 @@ void FCLCollisionDetector::distance( result->clear(); if (!checkGroupValidity(this, group1)) - return; + return 0.0; if (!checkGroupValidity(this, group2)) - return; + return 0.0; auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); @@ -805,7 +809,7 @@ void FCLCollisionDetector::distance( broadPhaseAlg1->distance(broadPhaseAlg2, &distData, distanceCallback); - return; + return std::max(distData.unclampedMinDistance, option.distanceLowerBound); } //============================================================================== @@ -1148,7 +1152,7 @@ bool distanceCallback( if (distData->done) { - dist = fclResult.min_distance; + dist = distData->unclampedMinDistance; return true; } @@ -1177,12 +1181,13 @@ bool distanceCallback( // 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); - // TODO(JS): Not sure if nullptr result would make any sense for distance - // check - if (dist <= option.minimumDistanceThreshold) + if (distData->unclampedMinDistance <= option.distanceLowerBound) distData->done = true; return distData->done; @@ -1446,7 +1451,9 @@ void interpreteDistanceResult( const DistanceOption& option, DistanceResult& result) { - result.minimumDistance = fclResult.min_distance; + result.unclampedMinDistance = fclResult.min_distance; + result.minDistance + = std::max(fclResult.min_distance, option.distanceLowerBound); const auto* userData1 = static_cast(o1->getUserData()); diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 607ff2a4f73a9..899dbebbfc2b3 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -109,13 +109,13 @@ class FCLCollisionDetector : public CollisionDetector CollisionResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; // Documentation inherited - void distance( + double distance( CollisionGroup* group1, CollisionGroup* group2, const DistanceOption& option = DistanceOption(false, 0.0, nullptr), diff --git a/unittests/testDistance.cpp b/unittests/testDistance.cpp index b15d0e7f9cae1..665129e58b8d8 100644 --- a/unittests/testDistance.cpp +++ b/unittests/testDistance.cpp @@ -66,7 +66,7 @@ void testBasicInterface(const std::shared_ptr& cd, collision::DistanceOption option; option.enableNearestPoints = true; EXPECT_TRUE(option.enableNearestPoints == true); - EXPECT_TRUE(option.minimumDistanceThreshold == 0.0); + EXPECT_TRUE(option.distanceLowerBound == 0.0); EXPECT_TRUE(option.distanceFilter == nullptr); collision::DistanceResult result; @@ -76,7 +76,7 @@ void testBasicInterface(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -91,7 +91,7 @@ void testBasicInterface(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -106,7 +106,7 @@ void testBasicInterface(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -172,7 +172,7 @@ void testOptions(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -186,7 +186,7 @@ void testOptions(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -202,7 +202,7 @@ void testOptions(const std::shared_ptr& cd, 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.minimumDistance, 0.25); + EXPECT_DOUBLE_EQ(result.minDistance, 0.25); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get() @@ -265,7 +265,7 @@ void testSphereSphere(const std::shared_ptr& cd, 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.minimumDistance, 0.0); + EXPECT_DOUBLE_EQ(result.minDistance, 0.0); EXPECT_TRUE(result.shapeFrame1 == simpleFrame1.get() || result.shapeFrame1 == simpleFrame2.get()); EXPECT_TRUE(result.shapeFrame2 == simpleFrame1.get()