Debian Codegraph @

unittests/integration/test_Distance.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 <gtest/gtest.h>
34
35 #include "dart/collision/fcl/fcl.hpp"
36 #include "dart/dart.hpp"
37 #if HAVE_BULLET
38 #include "dart/collision/bullet/bullet.hpp"
39 #endif
40 #include "TestHelpers.hpp"
41
42 using namespace dart;
43
44 //==============================================================================
45 void testBasicInterface(
46 const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
47 {
48 if (cd->getType() != collision::FCLCollisionDetector::getStaticType())
49 {
50 dtwarn << "Aborting test: distance check is not supported by "
51 << cd->getType() << ".\n";
52 return;
53 }
54
55 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
56 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
57
58 ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
59 ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
60 simpleFrame1->setShape(shape1);
61 simpleFrame2->setShape(shape2);
62
63 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
64 auto group2 = cd->createCollisionGroup(simpleFrame2.get());
65 auto group12 = cd->createCollisionGroup(group1.get(), group2.get());
66
67 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
68 EXPECT_EQ(group2->getNumShapeFrames(), 1u);
69 EXPECT_EQ(group12->getNumShapeFrames(), 2u);
70
71 collision::DistanceOption option;
72 option.enableNearestPoints = true;
73 EXPECT_TRUE(option.enableNearestPoints == true);
74 EXPECT_TRUE(option.distanceLowerBound == 0.0);
75 EXPECT_TRUE(option.distanceFilter == nullptr);
76
77 collision::DistanceResult result;
78 EXPECT_TRUE(result.found() == false);
79
80 result.clear();
81 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
82 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
83 group1->distance(group2.get(), option, &result);
84 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
85 EXPECT_TRUE(
86 result.shapeFrame1 == simpleFrame1.get()
87 || result.shapeFrame1 == simpleFrame2.get());
88 EXPECT_TRUE(
89 result.shapeFrame2 == simpleFrame1.get()
90 || result.shapeFrame2 == simpleFrame2.get());
91 EXPECT_TRUE(
92 result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
93 EXPECT_TRUE(
94 result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
95 EXPECT_TRUE(result.found() == true);
96
97 result.clear();
98 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
99 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
100 group12->distance(option, &result);
101 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
102 EXPECT_TRUE(
103 result.shapeFrame1 == simpleFrame1.get()
104 || result.shapeFrame1 == simpleFrame2.get());
105 EXPECT_TRUE(
106 result.shapeFrame2 == simpleFrame1.get()
107 || result.shapeFrame2 == simpleFrame2.get());
108 EXPECT_TRUE(
109 result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
110 EXPECT_TRUE(
111 result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
112 EXPECT_TRUE(result.found() == true);
113
114 result.clear();
115 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
116 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
117 cd->distance(group1.get(), group2.get(), option, &result);
118 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
119 EXPECT_TRUE(
120 result.shapeFrame1 == simpleFrame1.get()
121 || result.shapeFrame1 == simpleFrame2.get());
122 EXPECT_TRUE(
123 result.shapeFrame2 == simpleFrame1.get()
124 || result.shapeFrame2 == simpleFrame2.get());
125 EXPECT_TRUE(
126 result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
127 EXPECT_TRUE(
128 result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
129 EXPECT_TRUE(result.found() == true);
130 }
131
132 //==============================================================================
133 TEST(Distance, testBasicInterface)
134 {
135 auto fcl = FCLCollisionDetector::create();
136 testBasicInterface(fcl);
137
138 #if HAVE_BULLET
139 auto bullet = BulletCollisionDetector::create();
140 testBasicInterface(bullet);
141 #endif
142
143 auto dart = DARTCollisionDetector::create();
144 testBasicInterface(dart);
145 }
146
147 //==============================================================================
148 void testOptions(
149 const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
150 {
151 if (cd->getType() != collision::FCLCollisionDetector::getStaticType())
152 {
153 dtwarn << "Aborting test: distance check is not supported by "
154 << cd->getType() << ".\n";
155 return;
156 }
157
158 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
159 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
160
161 ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
162 ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
163 simpleFrame1->setShape(shape1);
164 simpleFrame2->setShape(shape2);
165
166 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
167 auto group2 = cd->createCollisionGroup(simpleFrame2.get());
168 auto group12 = cd->createCollisionGroup(group1.get(), group2.get());
169
170 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
171 EXPECT_EQ(group2->getNumShapeFrames(), 1u);
172 EXPECT_EQ(group12->getNumShapeFrames(), 2u);
173
174 collision::DistanceOption option;
175 collision::DistanceResult result;
176
177 EXPECT_TRUE(option.distanceFilter == nullptr);
178 EXPECT_TRUE(option.enableNearestPoints == false);
179
180 EXPECT_TRUE(result.found() == false);
181
182 result.clear();
183 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
184 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
185 group12->distance(option, &result);
186 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
187 EXPECT_TRUE(
188 result.shapeFrame1 == simpleFrame1.get()
189 || result.shapeFrame1 == simpleFrame2.get());
190 EXPECT_TRUE(
191 result.shapeFrame2 == simpleFrame1.get()
192 || result.shapeFrame2 == simpleFrame2.get());
193 EXPECT_EQ(result.nearestPoint1, Eigen::Vector3d::Zero());
194 EXPECT_EQ(result.nearestPoint2, Eigen::Vector3d::Zero());
195 EXPECT_TRUE(result.found() == true);
196
197 option.enableNearestPoints = true;
198 result.clear();
199 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
200 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
201 group12->distance(option, &result);
202 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
203 EXPECT_TRUE(
204 result.shapeFrame1 == simpleFrame1.get()
205 || result.shapeFrame1 == simpleFrame2.get());
206 EXPECT_TRUE(
207 result.shapeFrame2 == simpleFrame1.get()
208 || result.shapeFrame2 == simpleFrame2.get());
209 EXPECT_TRUE(
210 result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
211 EXPECT_TRUE(
212 result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol));
213 EXPECT_TRUE(result.found() == true);
214
215 option.enableNearestPoints = true;
216 result.clear();
217 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
218 simpleFrame2->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0));
219 group12->distance(option, &result);
220 EXPECT_DOUBLE_EQ(result.minDistance, 0.25);
221 EXPECT_TRUE(
222 result.shapeFrame1 == simpleFrame1.get()
223 || result.shapeFrame1 == simpleFrame2.get());
224 EXPECT_TRUE(
225 result.shapeFrame2 == simpleFrame1.get()
226 || result.shapeFrame2 == simpleFrame2.get());
227 EXPECT_TRUE(
228 result.nearestPoint1.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)
229 || result.nearestPoint1.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol));
230 EXPECT_TRUE(
231 result.nearestPoint2.isApprox(Eigen::Vector3d(0.5, 0.0, 0.0), tol)
232 || result.nearestPoint2.isApprox(Eigen::Vector3d(0.75, 0.0, 0.0), tol));
233 EXPECT_TRUE(result.found() == true);
234 }
235
236 //==============================================================================
237 TEST(Distance, Options)
238 {
239 auto fcl = FCLCollisionDetector::create();
240 testOptions(fcl);
241
242 #if HAVE_BULLET
243 auto bullet = BulletCollisionDetector::create();
244 testOptions(bullet);
245 #endif
246
247 auto dart = DARTCollisionDetector::create();
248 testOptions(dart);
249 }
250
251 //==============================================================================
252 void testSphereSphere(
253 const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
254 {
255 if (cd->getType() != collision::FCLCollisionDetector::getStaticType())
256 {
257 dtwarn << "Aborting test: distance check is not supported by "
258 << cd->getType() << ".\n";
259 return;
260 }
261
262 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
263 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
264
265 ShapePtr shape1(new EllipsoidShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
266 ShapePtr shape2(new EllipsoidShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
267 simpleFrame1->setShape(shape1);
268 simpleFrame2->setShape(shape2);
269
270 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
271 auto group2 = cd->createCollisionGroup(simpleFrame2.get());
272 auto group12 = cd->createCollisionGroup(group1.get(), group2.get());
273
274 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
275 EXPECT_EQ(group2->getNumShapeFrames(), 1u);
276 EXPECT_EQ(group12->getNumShapeFrames(), 2u);
277
278 collision::DistanceOption option;
279 collision::DistanceResult result;
280
281 result.clear();
282 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
283 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
284 group12->distance(option, &result);
285 EXPECT_DOUBLE_EQ(result.minDistance, 0.0);
286 EXPECT_TRUE(
287 result.shapeFrame1 == simpleFrame1.get()
288 || result.shapeFrame1 == simpleFrame2.get());
289 EXPECT_TRUE(
290 result.shapeFrame2 == simpleFrame1.get()
291 || result.shapeFrame2 == simpleFrame2.get());
292 EXPECT_TRUE(result.nearestPoint1.isApprox(Eigen::Vector3d::Zero(), tol));
293 EXPECT_TRUE(result.nearestPoint2.isApprox(Eigen::Vector3d::Zero(), tol));
294 }
295
296 //==============================================================================
297 TEST(Distance, SphereSphere)
298 {
299 auto fcl = FCLCollisionDetector::create();
300 testSphereSphere(fcl);
301
302 #if HAVE_BULLET
303 auto bullet = BulletCollisionDetector::create();
304 testSphereSphere(bullet);
305 #endif
306
307 auto dart = DARTCollisionDetector::create();
308 testSphereSphere(dart);
309 }