GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
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 |
|||
35 |
/** @author Damrong Guoy (Damrong.Guoy@tri.global) */ |
||
36 |
|||
37 |
/** Tests the dynamic axis-aligned bounding box tree.*/ |
||
38 |
|||
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" |
||
46 |
#include "hpp/fcl/shape/geometric_shapes.h" |
||
47 |
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" |
||
48 |
|||
49 |
using namespace hpp::fcl; |
||
50 |
|||
51 |
// Pack the data for callback function. |
||
52 |
struct CallBackData { |
||
53 |
bool expect_object0_then_object1; |
||
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 |
|||
64 |
struct DistanceCallBackDerived : DistanceCallBackBase { |
||
65 |
8 |
bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { |
|
66 |
8 |
return distance_callback(o1, o2, &data, dist); |
|
67 |
} |
||
68 |
|||
69 |
8 |
bool distance_callback(CollisionObject* a, CollisionObject*, |
|
70 |
void* callback_data, FCL_REAL&) { |
||
71 |
// Unpack the data. |
||
72 |
8 |
CallBackData* data = static_cast<CallBackData*>(callback_data); |
|
73 |
8 |
const std::vector<CollisionObject*>& objects = *(data->objects); |
|
74 |
8 |
const bool object0_first = a == objects[0]; |
|
75 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
8 |
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 |
8 |
data->expect_object0_then_object1 = !data->expect_object0_then_object1; |
|
81 |
// Return true to stop the tree traversal. |
||
82 |
8 |
return true; |
|
83 |
} |
||
84 |
|||
85 |
CallBackData data; |
||
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) { |
100 |
✓✗ | 4 |
CollisionGeometryPtr_t sphere0 = make_shared<Sphere>(0.1); |
101 |
✓✗ | 4 |
CollisionGeometryPtr_t sphere1 = make_shared<Sphere>(0.2); |
102 |
✓✗ | 4 |
CollisionObject object0(sphere0); |
103 |
✓✗ | 4 |
CollisionObject object1(sphere1); |
104 |
✓✗ | 2 |
const Eigen::Vector3d position0(0.1, 0.2, 0.3); |
105 |
✓✗ | 2 |
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 |
4 |
std::vector<CollisionObject*> objects; |
|
119 |
✓✗ | 2 |
objects.push_back(&object0); |
120 |
✓✗ | 2 |
objects.push_back(&object1); |
121 |
|||
122 |
4 |
std::vector<Eigen::Vector3d> positions; |
|
123 |
✓✗ | 2 |
positions.push_back(position0); |
124 |
✓✗ | 2 |
positions.push_back(position1); |
125 |
|||
126 |
✓✗ | 4 |
DynamicAABBTreeCollisionManager dynamic_tree; |
127 |
✓✓ | 6 |
for (size_t i = 0; i < objects.size(); ++i) { |
128 |
✓✗ | 4 |
objects[i]->setTranslation(positions[i]); |
129 |
✓✗ | 4 |
objects[i]->computeAABB(); |
130 |
✓✗ | 4 |
dynamic_tree.registerObject(objects[i]); |
131 |
} |
||
132 |
|||
133 |
2 |
DistanceCallBackDerived callback; |
|
134 |
2 |
callback.data.expect_object0_then_object1 = false; |
|
135 |
2 |
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 |
✓✓ | 18 |
for (int count = 0; count < 8; ++count) { |
140 |
✓✗ | 16 |
dynamic_tree.update(); |
141 |
✓✗ | 16 |
dynamic_tree.distance(&callback); |
142 |
} |
||
143 |
2 |
} |
Generated by: GCOVR (Version 4.2) |