Debian Codegraph @

unittests/regression/test_Issue1234.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <TestHelpers.hpp>
34 #include <dart/collision/bullet/BulletCollisionDetector.hpp>
35 #include <dart/collision/ode/OdeCollisionDetector.hpp>
36 #include <dart/constraint/ConstraintSolver.hpp>
37 #include <dart/dynamics/BoxShape.hpp>
38 #include <dart/dynamics/FreeJoint.hpp>
39 #include <dart/dynamics/MeshShape.hpp>
40 #include <dart/dynamics/PlaneShape.hpp>
41 #include <dart/dynamics/Skeleton.hpp>
42 #include <dart/dynamics/WeldJoint.hpp>
43 #include <dart/simulation/World.hpp>
44 #include <dart/utils/DartResourceRetriever.hpp>
45 #include <gtest/gtest.h>
46
47 //==============================================================================
48 bool runIssue1234Subtest(
49 const dart::collision::CollisionDetectorPtr detector,
50 const dart::dynamics::ShapePtr& against,
51 const Eigen::Vector3d normal,
52 const double offset,
53 const double angle,
54 const Eigen::Vector3d axis)
55 {
56 auto group = detector->createCollisionGroup();
57
58 const Eigen::Matrix3d R(Eigen::AngleAxisd(angle, axis));
59
60 auto plane = dart::dynamics::Skeleton::create("plane");
61 plane->createJointAndBodyNodePair<FreeJoint>()
62 .second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
63 std::make_shared<dart::dynamics::PlaneShape>(R * normal, offset));
64 group->subscribeTo(plane);
65
66 auto mesh = dart::dynamics::Skeleton::create("against");
67 mesh->createJointAndBodyNodePair<FreeJoint>()
68 .second->createShapeNodeWith<dart::dynamics::CollisionAspect>(against)
69 ->setRelativeTranslation(R * normal * offset);
70 group->subscribeTo(mesh);
71
72 const bool collision = group->collide();
73 EXPECT_TRUE(collision) << "\nFailed config:"
74 << "\nNormal: " << (R * normal).transpose()
75 << "\nOffset: " << offset
76 << "\nTilt: " << dart::math::toDegree(angle)
77 << " degrees"
78 << "\nAgainst: " << against->getType() << std::endl;
79
80 return collision;
81 }
82
83 //==============================================================================
84 void runIssue1234Test(
85 std::function<dart::collision::CollisionDetectorPtr()> detectorFactory)
86 {
87 const std::string meshUri = "dart://sample/obj/BoxSmall.obj";
88 const auto aiscene = dart::dynamics::MeshShape::loadMesh(
89 meshUri, dart::utils::DartResourceRetriever::create());
90 ASSERT_TRUE(aiscene);
91 const auto mesh = std::make_shared<dart::dynamics::MeshShape>(
92 100.0 * Eigen::Vector3d::Ones(), aiscene, meshUri);
93
94 const auto bb = mesh->getBoundingBox();
95 for (int i = 0; i < 3; ++i)
96 {
97 EXPECT_NEAR(bb.getMin()[i], -2.0, 1e-3);
98 EXPECT_NEAR(bb.getMax()[i], 2.0, 1e-3);
99 }
100
101 const auto box
102 = std::make_shared<dart::dynamics::BoxShape>(bb.getMax() - bb.getMin());
103
104 const auto sphere = std::make_shared<dart::dynamics::SphereShape>(
105 bb.getMax()[0] - bb.getMin()[0]);
106
107 std::size_t numTests = 0;
108 std::size_t numPasses = 0;
109
110 for (const dart::dynamics::ShapePtr& shape :
111 {dart::dynamics::ShapePtr{box}, dart::dynamics::ShapePtr{mesh}})
112 {
113 for (const double offset : {-0.1, 0.0, 0.1})
114 {
115 for (const auto& normal :
116 {Eigen::Vector3d(0.0, 0.0, 1.0),
117 Eigen::Vector3d(0.0, 0.0, -1.0),
118 Eigen::Vector3d(0.0, 1.0, 0.0),
119 Eigen::Vector3d(0.0, -1.0, 0.0),
120 Eigen::Vector3d(1.0, 0.0, 0.0),
121 Eigen::Vector3d(-1.0, 0.0, 0.0)})
122 {
123 if (offset < 0.0)
124 {
125 for (const auto& angle : {0.0, dart::math::toRadian(10.0)})
126 {
127 const Eigen::Vector3d axis = std::abs(normal[1]) > 1e-3
128 ? Eigen::Vector3d::UnitY()
129 : Eigen::Vector3d::UnitX();
130
131 ++numTests;
132 if (runIssue1234Subtest(
133 detectorFactory(), shape, normal, offset, angle, axis))
134 {
135 ++numPasses;
136 }
137 }
138 }
139 else
140 {
141 ++numTests;
142 if (runIssue1234Subtest(
143 detectorFactory(),
144 shape,
145 normal,
146 offset,
147 0.0,
148 Eigen::Vector3d::UnitZ()))
149 {
150 ++numPasses;
151 }
152 }
153 }
154 }
155 }
156
157 EXPECT_EQ(numTests, numPasses);
158 std::cout << "Failures: " << numTests - numPasses << std::endl;
159 }
160
161 //==============================================================================
162 TEST(DISABLED_Issue1234, Bullet)
163 {
164 runIssue1234Test(
165 [] { return dart::collision::BulletCollisionDetector::create(); });
166 }
167
168 //==============================================================================
169 TEST(DISABLED_Issue1234, ODE)
170 {
171 runIssue1234Test(
172 [] { return dart::collision::OdeCollisionDetector::create(); });
173 }