test_fcl_box_box.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018. Toyota Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CNRS-LAAS and AIST nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <functional>
38 #include <map>
39 #include <memory>
40 #include <utility>
41 #include <vector>
42 
43 #include <gtest/gtest.h>
44 #include <Eigen/Dense>
45 
46 #include "fcl/math/constants.h"
49 
50 using std::map;
51 using std::pair;
52 using std::string;
53 using std::vector;
54 
55 // Simple specification for defining a box collision object. Specifies the
56 // dimensions and pose of the box in some frame F (X_FB). For an explanation
57 // of the notation X_FB, see:
58 // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
59 template <typename S> struct BoxSpecification {
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 };
64 
65 // Class for executing and evaluating various box-box tests.
66 // The class is initialized with two box specifications (size and pose).
67 // The test performs a collision test between the two boxes in 12 permutations:
68 // One axis is the order (box 1 vs box 2 and box 2 vs box 1).
69 // The other axis is the orientation box 2. Given that box 2 must be a cube, it
70 // can be oriented in six different configurations and still produced the same
71 // answer.
72 // The 12 permutations are the two orderings crossed with the six orientations.
73 template <typename S>
74 class BoxBoxTest {
75  public:
76  // Construct the test scenario with the given box specifications. Box 2
77  // must be a *cube* (all sides equal).
78  BoxBoxTest(const BoxSpecification<S>& box_spec_1,
79  const BoxSpecification<S>& box_spec_2)
80  : box_spec_1_(box_spec_1), box_spec_2_(box_spec_2) {
81  using fcl::AngleAxis;
82  using fcl::Transform3;
83  using fcl::Vector3;
84 
85  // Confirm box 2 is a cube
86  EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(1));
87  EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(2));
88 
89  const S pi = fcl::constants<S>::pi();
90 
91  // Initialize isomorphic rotations of box 2.
92  iso_poses_["top"] = Transform3<S>::Identity();
93  iso_poses_["bottom"] =
94  Transform3<S>{AngleAxis<S>(pi, Vector3<S>::UnitX())};
95  iso_poses_["back"] =
96  Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitX())};
97  iso_poses_["front"] =
98  Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::UnitX())};
99  iso_poses_["left"] =
100  Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitY())};
101  iso_poses_["right"] =
102  Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::UnitY())};
103  }
104 
105  // Runs the 12 tests for the two specified boxes.
106  //
107  // @param solver_type The solver type to use for computing collision.
108  // @param test_tolerance The tolerance to which the collision contact
109  // results will be compared to the results.
110  // @param expected_normal The expected normal for the (1, 2) query order.
111  // @param expected_depth The expected penetration depth.
112  // @param contact_pos_test A function to evaluate the reported contact
113  // position for validity; this should be written to
114  // account for the possibility of the contact
115  // position lying on some manifold (e.g., an edge, or
116  // face). This function should invoke googletest
117  // EXPECT_* methods.
118  // @param origin_name A string which is appended to error message to
119  // more easily parse failures and which test failed.
120  void
121  RunTests(fcl::GJKSolverType solver_type, S test_tolerance,
123  std::function<void(const fcl::Vector3<S> &, S, const std::string &)>
124  contact_pos_test,
125  const std::string& origin_name) {
126  fcl::Contact<S> expected_contact;
127  expected_contact.penetration_depth = expected_depth;
128 
129  for (const auto& reorient_pair : iso_poses_) {
130  const std::string& top_face = reorient_pair.first;
131  const fcl::Transform3<S>& pre_pose = reorient_pair.second;
132 
133  BoxSpecification<S> box_2_posed{
134  box_spec_2_.size,
135  box_spec_2_.X_FB * pre_pose
136  };
137 
138  // Collide (1, 2)
139  expected_contact.normal = expected_normal;
141  box_2_posed,
142  solver_type,
143  test_tolerance,
144  expected_contact,
145  contact_pos_test,
146  origin_name + " (1, 2) - " + top_face);
147 
148  // Collide (2, 1)
149  expected_contact.normal = -expected_normal;
150  RunSingleTest(box_2_posed,
151  box_spec_1_,
152  solver_type,
153  test_tolerance,
154  expected_contact,
155  contact_pos_test,
156  origin_name + " (2, 1) - " + top_face);
157  }
158  }
159 
160 private:
161 // Performs a collision test between two boxes and tests the *single* contact
162 // result against given expectations.
163 //
164 // @param box_spec_A A specification of the first box (treated as object
165 // 1 in the query).
166 // @param box_spec_B A specification of the second box (treated as object
167 // 2 in the query).
168 // @param solver_type The solver type to use for computing collision.
169 // @param test_tolerance The tolerance to which the collision contact results
170 // will be compared to the results.
171 // @param expected_contact The expected contact details (only penetration depth
172 // and normal are used).
173 // @param contact_pos_test A function to evaluate the reported contact position
174 // for validity; this should be written to account for
175 // the possibility of the contact position lying on
176 // some manifold (e.g., an edge, or face). This
177 // function should invoke googletest EXPECT_* methods.
178 // @param origin_name A string which is appended to error message to more
179 // easily parse failures and which test failed.
181  const BoxSpecification<S>& box_spec_A,
182  const BoxSpecification<S>& box_spec_B, fcl::GJKSolverType solver_type,
183  S test_tolerance, const fcl::Contact<S>& expected_contact,
184  std::function<void(const fcl::Vector3<S>&, S, const std::string&)>
185  contact_pos_test,
186  const std::string& origin_name) {
187  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
188  CollisionGeometryPtr_t box_geometry_A(new fcl::Box<S>(box_spec_A.size));
189  CollisionGeometryPtr_t box_geometry_B(new fcl::Box<S>(box_spec_B.size));
190 
191  fcl::CollisionObject<S> box_A(box_geometry_A, box_spec_A.X_FB);
192  fcl::CollisionObject<S> box_B(box_geometry_B, box_spec_B.X_FB);
193 
194  // Compute collision - single contact and enable contact.
195  fcl::CollisionRequest<S> collisionRequest(1, true);
196  collisionRequest.gjk_solver_type = solver_type;
197  fcl::CollisionResult<S> collisionResult;
198  fcl::collide(&box_A, &box_B, collisionRequest, collisionResult);
199  EXPECT_TRUE(collisionResult.isCollision()) << origin_name;
200  std::vector<fcl::Contact<S>> contacts;
201  collisionResult.getContacts(contacts);
202  GTEST_ASSERT_EQ(contacts.size(), 1u) << origin_name;
203 
204  const fcl::Contact<S>& contact = contacts[0];
205  EXPECT_NEAR(expected_contact.penetration_depth, contact.penetration_depth,
206  test_tolerance)
207  << origin_name;
208  EXPECT_TRUE(expected_contact.normal.isApprox(contact.normal,
209  test_tolerance))
210  << origin_name << ":\n\texpected: "
211  << expected_contact.normal.transpose()
212  << "\n\tcontact.normal: " << contact.normal.transpose();
213  contact_pos_test(contact.pos, test_tolerance, origin_name);
214  }
215 
218  map<string, fcl::Transform3<S>, std::less<string>,
219  Eigen::aligned_allocator<std::pair<const string, fcl::Transform3<S>>>>
221 };
222 
223 // This test exercises the case of face-something contact. In the language of
224 // boxBox2() (in box_box-inl.h) it is for codes 1-6.
225 //
226 // More particularly it is designed to exercise the case where no contact points
227 // need be culled. It assumes that boxBox2() is invoked with a maximum contact
228 // count of 4. The collision produces a manifold that is a four-sided polygon.
229 //
230 // The test looks like this:
231 //
232 // z
233 // │
234 // │
235 // ╱│╲
236 // ╱ │ ╲ Box1
237 // ──┲━━╱━━O━━╲━━┱─── x
238 // ┃ ╲ │ ╱ ┃
239 // ┃ ╲ │ ╱ ┃
240 // ┃ ╲│╱ ┃
241 // ┃ │ ┃ Box2
242 // ┃ │ ┃
243 // ┗━━━━━┿━━━━━┛
244 // │
245 //
246 // There are two boxes:
247 // Box 1: A cube with side length of 1, centered on the world origin (O) and
248 // rotated 45 degrees around the y-axis.
249 // Box 2: A cube with side length of 3, moved in the negative z-direction such
250 // that it's top face lies on the z = 0 plane.
251 //
252 // The penetration depth should be sqrt(2) / 2.
253 // The normal should be parallel with the z-axis. We test both the collision of
254 // 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z,
255 // respectively.
256 // The contact position, should lie on an edge parallel with the y-axis at x = 0
257 // and z = sqrt(2) / 4.
258 template <typename S>
260  S test_tolerance)
261 {
262  const S pi = fcl::constants<S>::pi();
263  const S size_1 = 1;
264  BoxSpecification<S> box_1{
265  fcl::Vector3<S>{size_1, size_1, size_1},
267 
268  const S size_2 = 3;
269  BoxSpecification<S> box_2{fcl::Vector3<S>{size_2, size_2, size_2},
271  fcl::Vector3<S>(0, 0, -size_2 / 2))}};
272 
274  S expected_depth = size_1 * sqrt(2) / 2;
275 
276  auto contact_pos_test = [size_1](const fcl::Vector3<S> &pos, S tolerance,
277  const std::string& origin_name) {
278  const double expected_pos_z = -size_1 * std::sqrt(2) / 4;
279  EXPECT_NEAR(expected_pos_z, pos(2), tolerance) << origin_name;
280  EXPECT_NEAR(0, pos(0), tolerance) << origin_name;
281  EXPECT_LE(pos(1), 0.5) << origin_name;
282  EXPECT_GE(pos(1), -0.5) << origin_name;
283  };
284 
285  BoxBoxTest<S> tests(box_1, box_2);
286  tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth,
287  contact_pos_test, "test_colliion_box_box_all_contacts");
288 }
289 
290 GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_ccd)
291 {
292  test_collision_box_box_all_contacts<double>(fcl::GJKSolverType::GST_LIBCCD,
293  1e-14);
294 }
295 
296 GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_indep)
297 {
298  test_collision_box_box_all_contacts<double>(fcl::GJKSolverType::GST_INDEP,
299  1e-12);
300 }
301 
302 // This test exercises the case of face-something contact. In the language of
303 // boxBox2() (in box_box-inl.h) it is for codes 1-6.
304 //
305 // In contrast with the previous test (test_collision_box_box_all_contacts),
306 // the contact manifold is an eight-sided polygon and contacts will need to be
307 // culled.
308 //
309 // The test looks like this:
310 //
311 // Top view Side view
312 //
313 // +y +z
314 // │ ┆
315 // ╱│╲ __━━━━━┓ Box1
316 // ┏━━━╱━━┿━━╲━━━┓ ┏━━━━━━━ ┆ ┃
317 // ┣━╱━━━━┿━━━━╲━┫ ┃ ┆ ┃
318 // ╱ │ ╲ ┃ ┆ ┃
319 // ___╱_┃______│______┃_╲__ +x ┄┄┸┰┄┄┄┄┄┄─┼┄─┄─┄┸┰┄┄┄┄ +y
320 // ╲ ┃ │ ┃ ╱ ┃ ┆ ┃
321 // ╲ │ ╱ ┌─┨ ┏━━━┿━━━┓ ┠──┐
322 // ┃ ╲ │ ╱ ┃ │ ┃ ┃ │ ┖━━┛ │
323 // ┗━━━●━━┿━━●━━━┛ │ ┗━━━┛ │ │
324 // ╲│╱ │ │ │ Box2
325 // │ │ │ │
326 // │ │ │ │
327 // └─────────┴─────────┘
328 //
329 //
330 // There are two boxes:
331 // Box 1: A cube with side length of 1, centered on the world origin and
332 // rotated θ = 22.5 degrees around the x-axis
333 // Box 2: A cube with side length of 1, rotated 45 degrees around the z-axis
334 // and centered on the point [0, 0, -0.75].
335 //
336 // The penetration depth should be √2/2 * cos(π/4 - θ) - 0.25 ≊ 0.4032814.
337 // The normal should be parallel with the z-axis. We test both the collision of
338 // 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z,
339 // respectively.
340 // The contact position, should lie alone the line segment illustrated
341 // in the top view (indicated by the two points '●') at z = -0.25 - depth / 2.
342 template <typename S>
344  S test_tolerance)
345 {
346  const S pi = fcl::constants<S>::pi();
347  const S size = 1;
348  const S tilt = pi / 8;
349  BoxSpecification<S> box_1{
350  fcl::Vector3<S>{size, size, size},
352 
353  BoxSpecification<S> box_2{fcl::Vector3<S>{size, size, size},
355  pi / 4, fcl::Vector3<S>::UnitZ())}};
356  box_2.X_FB.translation() << 0, 0, -0.75;
357 
359  const S expected_depth{sqrt(2) / 2 * cos(pi / 4 - tilt) - 0.25};
360  auto contact_pos_test = [expected_depth, tilt, pi](
361  const fcl::Vector3<S> &pos, S tolerance, const std::string& origin_name) {
362  // Edge is parallel to the x-axis at
363  // z = expected_z = -0.25 - depth / 2
364  // y = expected_y = -√2/2 * sin(π/4 - θ)
365  // x = lines in the range [-x_e, x_e] where
366  // x_e = √2/2 + expected_y
367  const S expected_z{-0.25 - expected_depth / 2};
368  const S expected_y{-sqrt(2) / 2 * sin(pi / 4 - tilt)};
369  const S expected_x{sqrt(2) / 2 + expected_y +
371  EXPECT_NEAR(expected_z, pos(2), tolerance) << origin_name;
372  EXPECT_NEAR(expected_y, pos(1), tolerance) << origin_name;
373  EXPECT_GE(pos(0), -expected_x);
374  EXPECT_LE(pos(0), expected_x);
375  };
376 
377  BoxBoxTest<S> tests(box_1, box_2);
378  tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth,
379  contact_pos_test, "test_collision_box_box_cull_contacts");
380 }
381 
382 GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_ccd)
383 {
384  test_collision_box_box_cull_contacts<double>(fcl::GJKSolverType::GST_LIBCCD,
385  1e-14);
386 }
387 
388 GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_indep)
389 {
390  test_collision_box_box_cull_contacts<double>(fcl::GJKSolverType::GST_INDEP,
391  1e-14);
392 }
393 
394 // This test exercises the case where the contact is between edges (rather than
395 // face-something as in previous tests).
396 //
397 // The test looks like this.
398 //
399 // ╱╲
400 // ╱ ╲ Box1
401 // +z ╱ 1 ╲
402 // ╱╲ ╲
403 // ┆ ╱ ╲ ╱
404 // ┆╱ ╲ ╱
405 // ┏━━━━━╲━━━━┓ ╲ ╱
406 // ┃ ┆ ╲ ┃ ╱
407 // ┃ ┆ ╲┃ ╱
408 // ┄┄┄┄┄┃┄┄┄┄┼┄┄┄┄┄┃╲╱┄┄┄┄┄┄┄┄ +x
409 // ┃ ┆ ┃
410 // ┃ ┆ ┃ Box2
411 // ┗━━━━━━━━━━┛
412 // ┆
413 //
414 // There are two boxes:
415 // Box 1: A cube with side length of 1. It is rotated 45° around the world's
416 // z-axis and then -45° around the world's y-axis. Given a target
417 // penetration depth of 0.1, it's center is finally moved along the
418 // vector <√2/2, √2/2> a distance of `1 * √2 - 0.1`.
419 // Box 2: A cube with side length of 1. It is aligned with and centered on
420 // the world frame.
421 //
422 template <typename S>
424  S test_tolerance) {
425  const S pi = fcl::constants<S>::pi();
426  const S size = 1;
427  BoxSpecification<S> box_1{
428  fcl::Vector3<S>{size, size, size},
431  const fcl::Vector3<S> dir{sqrt(2) / 2, 0, sqrt(2) / 2};
432  const S expected_depth = 0.1;
433  box_1.X_FB.translation() = dir * (size * sqrt(2) - expected_depth);
434 
435  BoxSpecification<S> box_2{fcl::Vector3<S>{size, size, size},
437 
438  auto contact_pos_test = [expected_depth, size, dir](
439  const fcl::Vector3<S> &pos, S tolerance, const std::string& origin_name) {
440  // The contact point should unambiguously be a single point.
441  const S dist = size * sqrt(2) / 2 - expected_depth / 2;
442  const fcl::Vector3<S> expected_pos{dir * dist};
443  EXPECT_TRUE(expected_pos.isApprox(pos, tolerance)) << origin_name
444  << "\n\texpected: " << expected_pos.transpose()
445  << "\n\tpos: " << pos.transpose();
446  };
447 
448  BoxBoxTest<S> tests(box_1, box_2);
449  tests.RunTests(solver_type, test_tolerance, -dir, expected_depth,
450  contact_pos_test, "test_collision_box_box_edge_contact");
451 }
452 
453 GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_ccd)
454 {
455  test_collision_box_box_edge_contact<double>(fcl::GJKSolverType::GST_LIBCCD,
456  1e-14);
457 }
458 
459 GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_indep)
460 {
461  test_collision_box_box_edge_contact<double>(fcl::GJKSolverType::GST_INDEP,
462  1e-14);
463 }
464 
465 //==============================================================================
466 int main(int argc, char* argv[])
467 {
468  ::testing::InitGoogleTest(&argc, argv);
469  return RUN_ALL_TESTS();
470 }
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::CollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
Enumeration indicating the GJK solver implementation to use.
Definition: collision_request.h:78
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
BoxBoxTest::box_spec_2_
const BoxSpecification< S > box_spec_2_
Definition: test_fcl_box_box.cpp:217
collision_object.h
BoxBoxTest::iso_poses_
map< string, fcl::Transform3< S >, std::less< string >, Eigen::aligned_allocator< std::pair< const string, fcl::Transform3< S > > > > iso_poses_
Definition: test_fcl_box_box.cpp:220
BoxSpecification
Definition: test_fcl_box_box.cpp:59
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
BoxBoxTest::box_spec_1_
const BoxSpecification< S > box_spec_1_
Definition: test_fcl_box_box.cpp:216
fcl::Contact
Contact information returned by collision.
Definition: contact.h:48
BoxBoxTest
Definition: test_fcl_box_box.cpp:74
fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_result-inl.h:76
fcl::Contact::normal
Vector3< S > normal
contact normal, pointing from o1 to o2
Definition: contact.h:71
expected_pos
Vector3< S > expected_pos
Definition: test_sphere_box.cpp:181
fcl::CollisionResult
collision result
Definition: collision_request.h:48
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:97
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
test_collision_box_box_all_contacts
void test_collision_box_box_all_contacts(fcl::GJKSolverType solver_type, S test_tolerance)
Definition: test_fcl_box_box.cpp:259
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
BoxSpecification::X_FB
fcl::Transform3< S > X_FB
Definition: test_fcl_box_box.cpp:62
BoxBoxTest::RunTests
void RunTests(fcl::GJKSolverType solver_type, S test_tolerance, const fcl::Vector3< S > &expected_normal, S expected_depth, std::function< void(const fcl::Vector3< S > &, S, const std::string &)> contact_pos_test, const std::string &origin_name)
Definition: test_fcl_box_box.cpp:121
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::Contact::pos
Vector3< S > pos
contact position, in world space
Definition: contact.h:74
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
expected_depth
S expected_depth
Definition: test_half_space_convex.cpp:152
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
expected_normal
Vector3< S > expected_normal
Definition: test_sphere_box.cpp:180
BoxBoxTest::RunSingleTest
void RunSingleTest(const BoxSpecification< S > &box_spec_A, const BoxSpecification< S > &box_spec_B, fcl::GJKSolverType solver_type, S test_tolerance, const fcl::Contact< S > &expected_contact, std::function< void(const fcl::Vector3< S > &, S, const std::string &)> contact_pos_test, const std::string &origin_name)
Definition: test_fcl_box_box.cpp:180
BoxSpecification::size
EIGEN_MAKE_ALIGNED_OPERATOR_NEW fcl::Vector3< S > size
Definition: test_fcl_box_box.cpp:61
fcl::CollisionResult::getContacts
void getContacts(std::vector< Contact< S >> &contacts_)
get all the contacts
Definition: collision_result-inl.h:107
GTEST_TEST
GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_ccd)
Definition: test_fcl_box_box.cpp:290
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
test_collision_box_box_cull_contacts
void test_collision_box_box_cull_contacts(fcl::GJKSolverType solver_type, S test_tolerance)
Definition: test_fcl_box_box.cpp:343
constants.h
fcl::Contact::penetration_depth
S penetration_depth
penetration depth
Definition: contact.h:77
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
tolerance
S tolerance()
Definition: test_fcl_geometric_shapes.cpp:87
test_collision_box_box_edge_contact
void test_collision_box_box_edge_contact(fcl::GJKSolverType solver_type, S test_tolerance)
Definition: test_fcl_box_box.cpp:423
main
int main(int argc, char *argv[])
Definition: test_fcl_box_box.cpp:466
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
BoxBoxTest::BoxBoxTest
BoxBoxTest(const BoxSpecification< S > &box_spec_1, const BoxSpecification< S > &box_spec_2)
Definition: test_fcl_box_box.cpp:78
EXPECT_EQ
#define EXPECT_EQ(a, b)
collision.h


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49