diff --git a/CHANGELOG.md b/CHANGELOG.md index b5ce72f4d4200..c5f0c26a0f421 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### [DART 6.9.0 (20XX-XX-XX)](https://github.com/dartsim/dart/milestone/52?closed=1) +* Collision Detection + + * Added raycast query to BulletCollisionDetector: [#1309](https://github.com/dartsim/dart/pull/1309) + * Parser * Changed URDF parser to use URDF material color when specified: [#1295](https://github.com/dartsim/dart/pull/1295) diff --git a/dart/collision/CMakeLists.txt b/dart/collision/CMakeLists.txt index 6c457ed29ac2e..c846bdd1c30bb 100644 --- a/dart/collision/CMakeLists.txt +++ b/dart/collision/CMakeLists.txt @@ -41,5 +41,9 @@ add_subdirectory(ode) add_subdirectory(bullet) dart_format_add( + RaycastOption.hpp + RaycastOption.cpp + RaycastResult.hpp + RaycastResult.cpp SmartPointer.hpp ) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 931f5bf896609..7e2043aed98f0 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -56,6 +56,19 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } +//============================================================================== +bool CollisionDetector::raycast( + CollisionGroup* /*group*/, + const Eigen::Vector3d& /*from*/, + const Eigen::Vector3d& /*to*/, + const RaycastOption& /*option*/, + RaycastResult* /*result*/) +{ + dtwarn << "[CollisionDetector] Raycast is not supported by '" << getType() + << "'\n"; + return false; +} + //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index bf282fe506709..302c5863f42ef 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -44,6 +44,8 @@ #include "dart/collision/CollisionResult.hpp" #include "dart/collision/DistanceOption.hpp" #include "dart/collision/DistanceResult.hpp" +#include "dart/collision/RaycastOption.hpp" +#include "dart/collision/RaycastResult.hpp" #include "dart/collision/SmartPointer.hpp" #include "dart/dynamics/SmartPointer.hpp" @@ -156,6 +158,21 @@ class CollisionDetector : public std::enable_shared_from_this const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) = 0; + /// Perform raycast to a collision group. + /// + /// \param[in] group The collision group the ray will be casted onto. + /// \param[in] from The start point of the ray in world coordinates. + /// \param[in] to The end point of the ray in world coordinates. + /// \param[in] option The raycast option. + /// \param[in] result The raycast result. + /// \return True if the ray hit an collision object. + virtual bool raycast( + CollisionGroup* group, + const Eigen::Vector3d& from, + const Eigen::Vector3d& to, + const RaycastOption& option = RaycastOption(), + RaycastResult* result = nullptr); + protected: class CollisionObjectManager; diff --git a/dart/collision/RaycastOption.cpp b/dart/collision/RaycastOption.cpp new file mode 100644 index 0000000000000..5c1e743b2bf4f --- /dev/null +++ b/dart/collision/RaycastOption.cpp @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/RaycastOption.hpp" + +namespace dart { +namespace collision { + +//============================================================================== +RaycastOption::RaycastOption(bool enableAllHits, bool sortByClosest) + : mEnableAllHits(enableAllHits), mSortByClosest(sortByClosest) +{ + // Do nothing +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/RaycastOption.hpp b/dart/collision/RaycastOption.hpp new file mode 100644 index 0000000000000..4dc7187bee7bc --- /dev/null +++ b/dart/collision/RaycastOption.hpp @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_RAYCASTOPTION_HPP_ +#define DART_COLLISION_RAYCASTOPTION_HPP_ + +#include +#include + +namespace dart { +namespace collision { + +struct RaycastOption +{ + /// Constructor + RaycastOption(bool enableAllHits = false, bool sortByClosest = false); + + bool mEnableAllHits; + + bool mSortByClosest; + + // TODO(JS): Add filter +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_RAYCASTOPTION_HPP_ diff --git a/dart/collision/RaycastResult.cpp b/dart/collision/RaycastResult.cpp new file mode 100644 index 0000000000000..676047579b370 --- /dev/null +++ b/dart/collision/RaycastResult.cpp @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/RaycastResult.hpp" + +namespace dart { +namespace collision { + +//============================================================================== +RaycastResult::RaycastResult() +{ + // Do nothing +} + +//============================================================================== +void RaycastResult::clear() +{ + mRayHits.clear(); +} + +//============================================================================== +bool RaycastResult::hasHit() const +{ + return !mRayHits.empty(); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/RaycastResult.hpp b/dart/collision/RaycastResult.hpp new file mode 100644 index 0000000000000..1b97649e83623 --- /dev/null +++ b/dart/collision/RaycastResult.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_RAYCASTRESULT_HPP_ +#define DART_COLLISION_RAYCASTRESULT_HPP_ + +#include +#include + +namespace dart { +namespace collision { + +class CollisionObject; + +struct RayHit +{ + /// The collision object the ray hit + const CollisionObject* mCollisionObject; + + /// The normal at the hit point in the world coordinates + Eigen::Vector3d mNormal; + + /// The hit point in the world coordinates + Eigen::Vector3d mPoint; + + /// The fraction from "from" point to "to" point + double mFraction; +}; + +struct RaycastResult +{ + /// Constructor + RaycastResult(); + + /// Clear the result + void clear(); + + /// Returns true if there is a hit + bool hasHit() const; + + bool mHasHit; + + std::vector mRayHits; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_RAYCASTRESULT_HPP_ diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index cf6d47fcc1e37..8333c4b89be05 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -33,6 +33,8 @@ // Must be included before any Bullet headers. #include "dart/config.hpp" +#include + #include "dart/collision/bullet/BulletCollisionDetector.hpp" #include @@ -73,6 +75,16 @@ void reportContacts(btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result); +void reportRayHits( + const btCollisionWorld::ClosestRayResultCallback callback, + const RaycastOption& option, + RaycastResult& result); + +void reportRayHits( + const btCollisionWorld::AllHitsRayResultCallback callback, + const RaycastOption& option, + RaycastResult& result); + std::unique_ptr createBulletEllipsoidMesh( float sizeX, float sizeY, float sizeZ); @@ -84,6 +96,7 @@ std::unique_ptr createBulletCollisionShapeFromAssimpMesh(const template std::unique_ptr createBulletCollisionShapeFromHeightmap( const HeightmapShapeT* heightMap); + } // anonymous namespace //============================================================================== @@ -322,6 +335,61 @@ double BulletCollisionDetector::distance( return 0.0; } +//============================================================================== +bool BulletCollisionDetector::raycast( + CollisionGroup* group, + const Eigen::Vector3d& from, + const Eigen::Vector3d& to, + const RaycastOption& option, + RaycastResult* result) +{ + if (result) + result->clear(); + + // Check if 'this' is the collision engine of 'group'. + if (!checkGroupValidity(this, group)) + return false; + + auto castedGroup = static_cast(group); + auto collisionWorld = castedGroup->getBulletCollisionWorld(); + + const auto btFrom = convertVector3(from); + const auto btTo = convertVector3(to); + + if (option.mEnableAllHits) + { + auto callback = btCollisionWorld::AllHitsRayResultCallback(btFrom, btTo); + castedGroup->updateEngineData(); + collisionWorld->rayTest(btFrom, btTo, callback); + + if (result) + { + reportRayHits(callback, option, *result); + return result->hasHit(); + } + else + { + return callback.hasHit(); + } + } + else + { + auto callback = btCollisionWorld::ClosestRayResultCallback(btFrom, btTo); + castedGroup->updateEngineData(); + collisionWorld->rayTest(btFrom, btTo, callback); + + if (result) + { + reportRayHits(callback, option, *result); + return result->hasHit(); + } + else + { + return callback.hasHit(); + } + } +} + //============================================================================== BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() @@ -694,6 +762,77 @@ void reportContacts( } } +//============================================================================== +RayHit convertRayHit( + const btCollisionObject* btCollObj, + btVector3 hitPointWorld, + btVector3 hitNormalWorld, + btScalar closestHitFraction) +{ + RayHit rayHit; + assert(btCollObj); + const auto* userPointer = btCollObj->getUserPointer(); + assert(userPointer); + const auto* collObj = static_cast(userPointer); + assert(collObj); + rayHit.mCollisionObject = collObj; + rayHit.mPoint = convertVector3(hitPointWorld); + rayHit.mNormal = convertVector3(hitNormalWorld); + rayHit.mFraction = static_cast(closestHitFraction); + + return rayHit; +} + +//============================================================================== +void reportRayHits( + const btCollisionWorld::ClosestRayResultCallback callback, + const RaycastOption& /*option*/, + RaycastResult& result) +{ + const auto rayHit = convertRayHit( + callback.m_collisionObject, + callback.m_hitPointWorld, + callback.m_hitNormalWorld, + callback.m_closestHitFraction); + + result.mRayHits.clear(); + result.mRayHits.reserve(1); + result.mRayHits.emplace_back(rayHit); +} + +//============================================================================== +struct FractionLess +{ + bool operator()(const RayHit& a, const RayHit& b) + { + return a.mFraction < b.mFraction; + } +}; + +//============================================================================== +void reportRayHits( + const btCollisionWorld::AllHitsRayResultCallback callback, + const RaycastOption& option, + RaycastResult& result) +{ + result.mRayHits.clear(); + result.mRayHits.reserve( + static_cast(callback.m_hitPointWorld.size())); + + for (auto i = 0; i < callback.m_hitPointWorld.size(); ++i) + { + const auto rayHit = convertRayHit( + callback.m_collisionObjects[i], + callback.m_hitPointWorld[i], + callback.m_hitNormalWorld[i], + callback.m_hitFractions[i]); + result.mRayHits.emplace_back(rayHit); + } + + if (option.mSortByClosest) + std::sort(result.mRayHits.begin(), result.mRayHits.end(), FractionLess()); +} + //============================================================================== std::unique_ptr createBulletEllipsoidMesh( float sizeX, float sizeY, float sizeZ) diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 79d590fb7680c..dfea47977f99c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -46,8 +46,6 @@ namespace dart { namespace collision { -class BulletCollisionObject; - class BulletCollisionDetector : public CollisionDetector { public: @@ -58,7 +56,7 @@ class BulletCollisionDetector : public CollisionDetector static std::shared_ptr create(); /// Constructor - virtual ~BulletCollisionDetector(); + ~BulletCollisionDetector() override; // Documentation inherited std::shared_ptr cloneWithoutCollisionObjects() const @@ -99,6 +97,14 @@ class BulletCollisionDetector : public CollisionDetector const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr) override; + // Documentation inherited + bool raycast( + CollisionGroup* group, + const Eigen::Vector3d& from, + const Eigen::Vector3d& to, + const RaycastOption& option = RaycastOption(), + RaycastResult* result = nullptr) override; + protected: /// Constructor diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index b24247b3c4298..b12ea3ed429f2 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -16,6 +16,11 @@ if(TARGET dart-collision-bullet) target_link_libraries(test_Distance dart-collision-bullet) endif() +dart_add_test("comprehensive" test_Raycast) +if(TARGET dart-collision-bullet) + target_link_libraries(test_Raycast dart-collision-bullet) +endif() + if(TARGET dart-utils) dart_add_test("comprehensive" test_Collision) @@ -58,4 +63,5 @@ endif() dart_format_add( test_InverseKinematics.cpp test_MultiObjectiveOptimization.cpp + test_Raycast.cpp ) diff --git a/unittests/comprehensive/test_Raycast.cpp b/unittests/comprehensive/test_Raycast.cpp new file mode 100644 index 0000000000000..8fbdd1bd9698b --- /dev/null +++ b/unittests/comprehensive/test_Raycast.cpp @@ -0,0 +1,214 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include "dart/collision/fcl/fcl.hpp" +#include "dart/dart.hpp" +#if HAVE_BULLET +#include "dart/collision/bullet/bullet.hpp" +#endif +#include "TestHelpers.hpp" + +using namespace dart; + +//============================================================================== +void testBasicInterface(const std::shared_ptr& cd) +{ + if (cd->getType() != collision::BulletCollisionDetector::getStaticType()) + { + dtwarn << "Aborting test: distance check is not supported by " + << cd->getType() << ".\n"; + return; + } + + auto simpleFrame1 = SimpleFrame::createShared(Frame::World()); + + auto shape1 = std::make_shared(1.0); + simpleFrame1->setShape(shape1); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + + collision::RaycastOption option; + option.mEnableAllHits = false; + + collision::RaycastResult result; + EXPECT_FALSE(result.hasHit()); + + RayHit rayHit; + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + cd->raycast( + group1.get(), + Eigen::Vector3d(-2, 0, 0), + Eigen::Vector3d(2, 0, 0), + option, + &result); + EXPECT_TRUE(result.hasHit()); + EXPECT_EQ(result.mRayHits.size(), 1u); + rayHit = result.mRayHits[0]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-1, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0))); + EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0)); + cd->raycast( + group1.get(), + Eigen::Vector3d(2, 0, 0), + Eigen::Vector3d(-2, 0, 0), + option, + &result); + EXPECT_TRUE(result.hasHit()); + EXPECT_EQ(result.mRayHits.size(), 1u); + rayHit = result.mRayHits[0]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(1, 0, 0))); + EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25); + + result.clear(); + simpleFrame1->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + cd->raycast( + group1.get(), + Eigen::Vector3d(-2, 0, 0), + Eigen::Vector3d(2, 0, 0), + option, + &result); + EXPECT_TRUE(result.hasHit()); + EXPECT_EQ(result.mRayHits.size(), 1u); + rayHit = result.mRayHits[0]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(0, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0))); + EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.5); +} + +//============================================================================== +TEST(Distance, testBasicInterface) +{ + auto fcl = FCLCollisionDetector::create(); + testBasicInterface(fcl); + +#if HAVE_BULLET + auto bullet = BulletCollisionDetector::create(); + testBasicInterface(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testBasicInterface(dart); +} + +//============================================================================== +void testOptions(const std::shared_ptr& cd) +{ + if (cd->getType() != collision::BulletCollisionDetector::getStaticType()) + { + dtwarn << "Aborting test: distance check is not supported by " + << cd->getType() << ".\n"; + return; + } + + auto simpleFrame1 = SimpleFrame::createShared(Frame::World()); + auto shape1 = std::make_shared(1.0); + simpleFrame1->setShape(shape1); + + auto simpleFrame2 = SimpleFrame::createShared(Frame::World()); + auto shape2 = std::make_shared(1.0); + simpleFrame2->setShape(shape1); + + auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get()); + + collision::RaycastOption option; + option.mEnableAllHits = false; + + collision::RaycastResult result; + EXPECT_FALSE(result.hasHit()); + + RayHit rayHit; + + result.clear(); + option.mEnableAllHits = false; + option.mSortByClosest = false; + simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0)); + simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0)); + cd->raycast( + group.get(), + Eigen::Vector3d(-5, 0, 0), + Eigen::Vector3d(5, 0, 0), + option, + &result); + EXPECT_TRUE(result.hasHit()); + EXPECT_EQ(result.mRayHits.size(), 1u); + rayHit = result.mRayHits[0]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0))); + EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5); + + result.clear(); + option.mEnableAllHits = true; + option.mSortByClosest = true; + simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0)); + simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0)); + cd->raycast( + group.get(), + Eigen::Vector3d(-5, 0, 0), + Eigen::Vector3d(5, 0, 0), + option, + &result); + EXPECT_TRUE(result.hasHit()); + EXPECT_EQ(result.mRayHits.size(), 2u); + rayHit = result.mRayHits[0]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0))); + EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5); + rayHit = result.mRayHits[1]; + EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0))); + EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0))); + EXPECT_NEAR(rayHit.mFraction, 0.6, 1e-5); +} + +//============================================================================== +TEST(Distance, testOptions) +{ + auto fcl = FCLCollisionDetector::create(); + testOptions(fcl); + +#if HAVE_BULLET + auto bullet = BulletCollisionDetector::create(); + testOptions(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testOptions(dart); +}