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 #define BOOST_TEST_MODULE BROADPHASE_DYNAMIC_AABB_TREE
43 #include <boost/test/included/unit_test.hpp>
44 
45 // #include "hpp/fcl/data_types.h"
48 
49 using namespace hpp::fcl;
50 
51 // Pack the data for callback function.
52 struct CallBackData {
54  std::vector<CollisionObject*>* objects;
55 };
56 
57 // This callback function tests the order of the two collision objects from
58 // the dynamic tree against the `data`. We assume that the first two
59 // parameters are always objects[0] and objects[1] in two possible orders,
60 // so we can safely ignore the second parameter. We do not use the last
61 // FCL_REAL& parameter, which specifies the distance beyond which the
62 // pair of objects will be skipped.
63 
66  return distance_callback(o1, o2, &data, dist);
67  }
68 
70  void* callback_data, FCL_REAL&) {
71  // Unpack the data.
72  CallBackData* data = static_cast<CallBackData*>(callback_data);
73  const std::vector<CollisionObject*>& objects = *(data->objects);
74  const bool object0_first = a == objects[0];
75  BOOST_CHECK_EQUAL(data->expect_object0_then_object1, object0_first);
76  // TODO(DamrongGuoy): Remove the statement below when we solve the
77  // repeatability problem as mentioned in:
78  // https://github.com/flexible-collision-library/fcl/issues/368
79  // Expect to switch the order next time.
80  data->expect_object0_then_object1 = !data->expect_object0_then_object1;
81  // Return true to stop the tree traversal.
82  return true;
83  }
84 
86 };
87 
88 // Tests repeatability of a dynamic tree of two spheres when we call update()
89 // and distance() again and again without changing the poses of the objects.
90 // We only use the distance() method to invoke a hierarchy traversal.
91 // The distance-callback function in this test does not compute the signed
92 // distance between the two objects; it only checks their order.
93 //
94 // Currently every call to update() switches the order of the two objects.
95 // TODO(DamrongGuoy): Remove the above comment when we solve the
96 // repeatability problem as mentioned in:
97 // https://github.com/flexible-collision-library/fcl/issues/368
98 //
99 BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) {
100  CollisionGeometryPtr_t sphere0 = make_shared<Sphere>(0.1);
101  CollisionGeometryPtr_t sphere1 = make_shared<Sphere>(0.2);
102  CollisionObject object0(sphere0);
103  CollisionObject object1(sphere1);
104  const Eigen::Vector3d position0(0.1, 0.2, 0.3);
105  const Eigen::Vector3d position1(0.11, 0.21, 0.31);
106 
107  // We will use `objects` to check the order of the two collision objects in
108  // our callback function.
109  //
110  // We use std::vector that contains *pointers* to CollisionObject,
111  // instead of std::vector that contains CollisionObject's.
112  // Previously we used std::vector<CollisionObject>, and it failed the
113  // Eigen alignment assertion on Win32. We also tried, without success, the
114  // custom allocator:
115  // std::vector<CollisionObject,
116  // Eigen::aligned_allocator<CollisionObject>>,
117  // but some platforms failed to build.
118  std::vector<CollisionObject*> objects;
119  objects.push_back(&object0);
120  objects.push_back(&object1);
121 
122  std::vector<Eigen::Vector3d> positions;
123  positions.push_back(position0);
124  positions.push_back(position1);
125 
126  DynamicAABBTreeCollisionManager dynamic_tree;
127  for (size_t i = 0; i < objects.size(); ++i) {
128  objects[i]->setTranslation(positions[i]);
129  objects[i]->computeAABB();
130  dynamic_tree.registerObject(objects[i]);
131  }
132 
134  callback.data.expect_object0_then_object1 = false;
135  callback.data.objects = &objects;
136 
137  // We repeat update() and distance() many times. Each time, in the
138  // callback function, we check the order of the two objects.
139  for (int count = 0; count < 8; ++count) {
140  dynamic_tree.update();
141  dynamic_tree.distance(&callback);
142  }
143 }
collision_manager.callback
callback
Definition: collision_manager.py:27
CallBackData
Definition: test/broadphase_dynamic_AABB_tree.cpp:52
hpp::fcl::DynamicAABBTreeCollisionManager::update
virtual void update()
update the condition of manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:512
hpp::fcl::DynamicAABBTreeCollisionManager::distance
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:593
data
data
hpp::fcl::DynamicAABBTreeCollisionManager::registerObject
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:478
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
CallBackData::expect_object0_then_object1
bool expect_object0_then_object1
Definition: test/broadphase_dynamic_AABB_tree.cpp:53
a
list a
hpp::fcl::DynamicAABBTreeCollisionManager
Definition: broadphase_dynamic_AABB_tree.h:54
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceCallBackBase
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
CallBackData::objects
std::vector< CollisionObject * > * objects
Definition: test/broadphase_dynamic_AABB_tree.cpp:54
DistanceCallBackDerived::distance_callback
bool distance_callback(CollisionObject *a, CollisionObject *, void *callback_data, FCL_REAL &)
Definition: test/broadphase_dynamic_AABB_tree.cpp:69
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class)
Definition: test/broadphase_dynamic_AABB_tree.cpp:99
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
DistanceCallBackDerived::data
CallBackData data
Definition: test/broadphase_dynamic_AABB_tree.cpp:85
hpp::fcl
Definition: broadphase_bruteforce.h:45
broadphase_dynamic_AABB_tree.h
geometric_shapes.h
DistanceCallBackDerived
Definition: test/broadphase_dynamic_AABB_tree.cpp:64
DistanceCallBackDerived::distance
bool distance(CollisionObject *o1, CollisionObject *o2, FCL_REAL &dist)
Distance evaluation between two objects in collision. This callback will cause the broadphase evaluat...
Definition: test/broadphase_dynamic_AABB_tree.cpp:65


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:12