test_broadphase_dynamic_AABB_tree.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020. 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 
39 #include <iostream>
40 #include <memory>
41 
42 #include <gtest/gtest.h>
43 
44 #include "fcl/common/types.h"
47 
49 
50 // Tests repeatability of a dynamic tree of two spheres when we call update()
51 // and distance() again and again without changing the poses of the objects.
52 // We only use the distance() method to invoke a hierarchy traversal.
53 // The distance-callback function in this test does not compute the signed
54 // distance between the two objects; it only checks their order.
55 //
56 // Currently every call to update() switches the order of the two objects.
57 // TODO(DamrongGuoy): Remove the above comment when we solve the
58 // repeatability problem as mentioned in:
59 // https://github.com/flexible-collision-library/fcl/issues/368
60 //
61 GTEST_TEST(DynamicAABBTreeCollisionManager, update) {
62  auto sphere0 = std::make_shared<fcl::Sphered>(0.1);
63  auto sphere1 = std::make_shared<fcl::Sphered>(0.2);
64  fcl::CollisionObjectd object0(sphere0);
65  fcl::CollisionObjectd object1(sphere1);
66  const Vector3d position0(0.1, 0.2, 0.3);
67  const Vector3d position1(0.11, 0.21, 0.31);
68 
69  // We will use `objects` to check the order of the two collision objects in
70  // our callback function.
71  //
72  // We use std::vector that contains *pointers* to CollisionObjectd,
73  // instead of std::vector that contains CollisionObjectd's.
74  // Previously we used std::vector<fcl::CollisionObjectd>, and it failed the
75  // Eigen alignment assertion on Win32. We also tried, without success, the
76  // custom allocator:
77  // std::vector<fcl::CollisionObjectd,
78  // Eigen::aligned_allocator<fcl::CollisionObjectd>>,
79  // but some platforms failed to build.
80  std::vector<fcl::CollisionObjectd*> objects = {&object0, &object1};
81  std::vector<const Vector3d*> positions = {&position0, &position1};
82 
84  for (int i = 0; i < static_cast<int>(objects.size()); ++i) {
85  objects[i]->setTranslation(*positions[i]);
86  objects[i]->computeAABB();
87  dynamic_tree.registerObject(objects[i]);
88  }
89 
90  // Pack the data for callback function.
91  struct CallBackData {
92  bool expect_object0_then_object1;
93  std::vector<fcl::CollisionObjectd*>* objects;
94  } data;
95  data.expect_object0_then_object1 = false;
96  data.objects = &objects;
97 
98  // This callback function tests the order of the two collision objects from
99  // the dynamic tree against the `data`. We assume that the first two
100  // parameters are always objects[0] and objects[1] in two possible orders,
101  // so we can safely ignore the second parameter. We do not use the last
102  // double& parameter, which specifies the distance beyond which the
103  // pair of objects will be skipped.
104  auto distance_callback = [](fcl::CollisionObjectd* a, fcl::CollisionObjectd*,
105  void* callback_data, double&) -> bool {
106  // Unpack the data.
107  auto data = static_cast<CallBackData*>(callback_data);
108  const std::vector<fcl::CollisionObjectd*>& objects = *(data->objects);
109  const bool object0_first = a == objects[0];
110  EXPECT_EQ(data->expect_object0_then_object1, object0_first);
111  // TODO(DamrongGuoy): Remove the statement below when we solve the
112  // repeatability problem as mentioned in:
113  // https://github.com/flexible-collision-library/fcl/issues/368
114  // Expect to switch the order next time.
115  data->expect_object0_then_object1 = !data->expect_object0_then_object1;
116  // Return true to stop the tree traversal.
117  return true;
118  };
119  // We repeat update() and distance() many times. Each time, in the
120  // callback function, we check the order of the two objects.
121  for (int count = 0; count < 8; ++count) {
122  dynamic_tree.update();
123  dynamic_tree.distance(&data, distance_callback);
124  }
125 }
126 
127 //==============================================================================
128 int main(int argc, char* argv[]) {
129  ::testing::InitGoogleTest(&argc, argv);
130  return RUN_ALL_TESTS();
131 }
main
int main(int argc, char *argv[])
Definition: test_broadphase_dynamic_AABB_tree.cpp:128
fcl::DynamicAABBTreeCollisionManager
Definition: broadphase_dynamic_AABB_tree.h:54
types.h
gtest.h
sphere.h
fcl::DynamicAABBTreeCollisionManager::update
void update()
update the condition of manager
Definition: broadphase_dynamic_AABB_tree-inl.h:826
fcl::Vector3d
Vector3< double > Vector3d
Definition: types.h:111
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:2287
GTEST_TEST
GTEST_TEST(DynamicAABBTreeCollisionManager, update)
Definition: test_broadphase_dynamic_AABB_tree.cpp:61
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
Definition: gtest.cc:5008
broadphase_dynamic_AABB_tree.h
fcl::DynamicAABBTreeCollisionManager::distance
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: broadphase_dynamic_AABB_tree-inl.h:922
fcl::DynamicAABBTreeCollisionManager::registerObject
void registerObject(CollisionObject< S > *obj)
add one object to the manager
Definition: broadphase_dynamic_AABB_tree-inl.h:781
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:04