From ca2dcd690286a34559e9cdb8ad1e1c9b14c6c5d6 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 11 Feb 2019 10:33:52 +0800 Subject: [PATCH] Unreliable plane-mesh collision detection (#1235) --- unittests/regression/CMakeLists.txt | 10 ++ unittests/regression/test_Issue1234.cpp | 177 ++++++++++++++++++++++++ 2 files changed, 187 insertions(+) create mode 100644 unittests/regression/test_Issue1234.cpp diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index c6b445a1bd16c..e8dfe07ce53f5 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -19,3 +19,13 @@ if(TARGET dart-collision-bullet) target_link_libraries(test_Issue1184 dart-collision-bullet) endif() + +if(TARGET dart-collision-bullet AND TARGET dart-collision-ode) + + dart_add_test("regression" test_Issue1234) + target_link_libraries(test_Issue1234 + dart-collision-bullet + dart-collision-ode + dart-utils) + +endif() diff --git a/unittests/regression/test_Issue1234.cpp b/unittests/regression/test_Issue1234.cpp new file mode 100644 index 0000000000000..147d91a3068c0 --- /dev/null +++ b/unittests/regression/test_Issue1234.cpp @@ -0,0 +1,177 @@ +/* + * 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 + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +//============================================================================== +bool runIssue1234Subtest( + const dart::collision::CollisionDetectorPtr detector, + const dart::dynamics::ShapePtr& against, + const Eigen::Vector3d normal, + const double offset, + const double angle, + const Eigen::Vector3d axis) +{ + auto group = detector->createCollisionGroup(); + + const Eigen::Matrix3d R(Eigen::AngleAxisd(angle, axis)); + + auto plane = dart::dynamics::Skeleton::create("plane"); + plane->createJointAndBodyNodePair().second + ->createShapeNodeWith( + std::make_shared(R*normal, offset)); + group->subscribeTo(plane); + + auto mesh = dart::dynamics::Skeleton::create("against"); + mesh->createJointAndBodyNodePair().second + ->createShapeNodeWith(against) + ->setRelativeTranslation(R*normal*offset); + group->subscribeTo(mesh); + + const bool collision = group->collide(); + EXPECT_TRUE(collision) + << "\nFailed config:" + << "\nNormal: " << (R*normal).transpose() + << "\nOffset: " << offset + << "\nTilt: " << angle*180.0/M_PI << " degrees" + << "\nAgainst: " << against->getType() << std::endl; + + + + return collision; +} + +//============================================================================== +void runIssue1234Test( + std::function detectorFactory) +{ + const std::string meshUri = "dart://sample/obj/BoxSmall.obj"; + const auto aiscene = dart::dynamics::MeshShape::loadMesh( + meshUri, dart::utils::DartResourceRetriever::create()); + ASSERT_TRUE(aiscene); + const auto mesh = std::make_shared( + 100.0 * Eigen::Vector3d::Ones(), aiscene, meshUri); + + const auto bb = mesh->getBoundingBox(); + for (int i = 0; i < 3; ++i) + { + EXPECT_NEAR(bb.getMin()[i], -2.0, 1e-3); + EXPECT_NEAR(bb.getMax()[i], 2.0, 1e-3); + } + + const auto box = std::make_shared( + bb.getMax() - bb.getMin()); + + const auto sphere = std::make_shared( + bb.getMax()[0] - bb.getMin()[0]); + + std::size_t numTests = 0; + std::size_t numPasses = 0; + + for(const dart::dynamics::ShapePtr& shape : { + dart::dynamics::ShapePtr{box}, + dart::dynamics::ShapePtr{mesh} + }) + { + for(const double offset : {-0.1, 0.0, 0.1}) + { + for(const auto& normal : { + Eigen::Vector3d( 0.0, 0.0, 1.0), + Eigen::Vector3d( 0.0, 0.0, -1.0), + Eigen::Vector3d( 0.0, 1.0, 0.0), + Eigen::Vector3d( 0.0, -1.0, 0.0), + Eigen::Vector3d( 1.0, 0.0, 0.0), + Eigen::Vector3d(-1.0, 0.0, 0.0) + }) + { + if(offset < 0.0) + { + for(const auto& angle : {0.0, 10.0*M_PI/180.0}) + { + const Eigen::Vector3d axis = std::abs(normal[1]) > 1e-3? + Eigen::Vector3d::UnitY() : Eigen::Vector3d::UnitX(); + + ++numTests; + if(runIssue1234Subtest( + detectorFactory(), shape, normal, offset, angle, axis)) + { + ++numPasses; + } + } + } + else + { + ++numTests; + if(runIssue1234Subtest( + detectorFactory(), shape, normal, offset, + 0.0, Eigen::Vector3d::UnitZ())) + { + ++numPasses; + } + } + } + } + } + + EXPECT_EQ(numTests, numPasses); + std::cout << "Failures: " << numTests - numPasses << std::endl; +} + +//============================================================================== +TEST(DISABLED_Issue1234, Bullet) +{ + runIssue1234Test( + []{ return dart::collision::BulletCollisionDetector::create(); }); +} + +//============================================================================== +TEST(DISABLED_Issue1234, ODE) +{ + runIssue1234Test( + []{ return dart::collision::OdeCollisionDetector::create(); }); +}