Debian Codegraph @

unittests/integration/test_Raycast.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 TEST(Raycast, RayHitDefaultConstructor)
46 {
47 RayHit rayHit;
48 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d::Zero().eval()));
49 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d::Zero().eval()));
50 EXPECT_DOUBLE_EQ(rayHit.mFraction, 0);
51 }
52
53 //==============================================================================
54 TEST(Raycast, RaycastResultDefaultConstructor)
55 {
56 RaycastResult result;
57 EXPECT_FALSE(result.hasHit());
58 EXPECT_TRUE(result.mRayHits.empty());
59 }
60
61 //==============================================================================
62 void testBasicInterface(const std::shared_ptr<CollisionDetector>& cd)
63 {
64 if (cd->getType() != "bullet")
65 {
66 dtwarn << "Aborting test: distance check is not supported by "
67 << cd->getType() << ".\n";
68 return;
69 }
70
71 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
72
73 auto shape1 = std::make_shared<SphereShape>(1.0);
74 simpleFrame1->setShape(shape1);
75
76 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
77
78 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
79
80 collision::RaycastOption option;
81 option.mEnableAllHits = false;
82
83 collision::RaycastResult result;
84 EXPECT_FALSE(result.hasHit());
85
86 RayHit rayHit;
87
88 result.clear();
89 simpleFrame1->setTranslation(Eigen::Vector3d(5.0, 0.0, 0.0));
90 cd->raycast(
91 group1.get(),
92 Eigen::Vector3d(-2, 0, 0),
93 Eigen::Vector3d(2, 0, 0),
94 option,
95 &result);
96 EXPECT_FALSE(result.hasHit());
97
98 result.clear();
99 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
100 cd->raycast(
101 group1.get(),
102 Eigen::Vector3d(-2, 0, 0),
103 Eigen::Vector3d(2, 0, 0),
104 option,
105 &result);
106 EXPECT_TRUE(result.hasHit());
107 EXPECT_EQ(result.mRayHits.size(), 1u);
108 rayHit = result.mRayHits[0];
109 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-1, 0, 0)));
110 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
111 EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25);
112
113 result.clear();
114 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
115 simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
116 cd->raycast(
117 group1.get(),
118 Eigen::Vector3d(2, 0, 0),
119 Eigen::Vector3d(-2, 0, 0),
120 option,
121 &result);
122 EXPECT_TRUE(result.hasHit());
123 EXPECT_EQ(result.mRayHits.size(), 1u);
124 rayHit = result.mRayHits[0];
125 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0)));
126 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(1, 0, 0)));
127 EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25);
128
129 result.clear();
130 simpleFrame1->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0));
131 cd->raycast(
132 group1.get(),
133 Eigen::Vector3d(-2, 0, 0),
134 Eigen::Vector3d(2, 0, 0),
135 option,
136 &result);
137 EXPECT_TRUE(result.hasHit());
138 EXPECT_EQ(result.mRayHits.size(), 1u);
139 rayHit = result.mRayHits[0];
140 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(0, 0, 0)));
141 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
142 EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.5);
143 }
144
145 //==============================================================================
146 TEST(Raycast, testBasicInterface)
147 {
148 auto fcl = FCLCollisionDetector::create();
149 testBasicInterface(fcl);
150
151 #if HAVE_BULLET
152 auto bullet = BulletCollisionDetector::create();
153 testBasicInterface(bullet);
154 #endif
155
156 auto dart = DARTCollisionDetector::create();
157 testBasicInterface(dart);
158 }
159
160 //==============================================================================
161 void testOptions(const std::shared_ptr<CollisionDetector>& cd)
162 {
163 if (cd->getType() != "bullet")
164 {
165 dtwarn << "Aborting test: distance check is not supported by "
166 << cd->getType() << ".\n";
167 return;
168 }
169
170 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
171 auto shape1 = std::make_shared<SphereShape>(1.0);
172 simpleFrame1->setShape(shape1);
173
174 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
175 auto shape2 = std::make_shared<SphereShape>(1.0);
176 simpleFrame2->setShape(shape1);
177
178 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
179
180 collision::RaycastOption option;
181 option.mEnableAllHits = false;
182
183 collision::RaycastResult result;
184 EXPECT_FALSE(result.hasHit());
185
186 RayHit rayHit;
187
188 result.clear();
189 option.mEnableAllHits = false;
190 option.mSortByClosest = false;
191 simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0));
192 simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0));
193 cd->raycast(
194 group.get(),
195 Eigen::Vector3d(-5, 0, 0),
196 Eigen::Vector3d(5, 0, 0),
197 option,
198 &result);
199 EXPECT_TRUE(result.hasHit());
200 EXPECT_EQ(result.mRayHits.size(), 1u);
201 rayHit = result.mRayHits[0];
202 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0)));
203 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
204 EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5);
205
206 result.clear();
207 option.mEnableAllHits = true;
208 option.mSortByClosest = true;
209 simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0));
210 simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0));
211 cd->raycast(
212 group.get(),
213 Eigen::Vector3d(-5, 0, 0),
214 Eigen::Vector3d(5, 0, 0),
215 option,
216 &result);
217 EXPECT_TRUE(result.hasHit());
218 EXPECT_EQ(result.mRayHits.size(), 2u);
219 rayHit = result.mRayHits[0];
220 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0)));
221 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
222 EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5);
223 rayHit = result.mRayHits[1];
224 EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0)));
225 EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
226 EXPECT_NEAR(rayHit.mFraction, 0.6, 1e-5);
227 }
228
229 //==============================================================================
230 TEST(Raycast, testOptions)
231 {
232 auto fcl = FCLCollisionDetector::create();
233 testOptions(fcl);
234
235 #if HAVE_BULLET
236 auto bullet = BulletCollisionDetector::create();
237 testOptions(bullet);
238 #endif
239
240 auto dart = DARTCollisionDetector::create();
241 testOptions(dart);
242 }