GCC Code Coverage Report


Directory: ./
File: test/broadphase_dynamic_AABB_tree.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 34 34 100.0%
Branches: 58 112 51.8%

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 #define BOOST_TEST_MODULE COAL_BROADPHASE_DYNAMIC_AABB_TREE
40 #include <boost/test/included/unit_test.hpp>
41
42 // #include "coal/data_types.h"
43 #include "coal/shape/geometric_shapes.h"
44 #include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
45
46 #include <iostream>
47 #include <memory>
48
49 using namespace coal;
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 // Scalar& 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, Scalar& dist) {
66 8 return distance_callback(o1, o2, &data, dist);
67 }
68
69 8 bool distance_callback(CollisionObject* a, CollisionObject*,
70 void* callback_data, Scalar&) {
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
5/10
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 8 times.
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
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) {
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 CollisionGeometryPtr_t sphere0 = make_shared<Sphere>(0.1);
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 CollisionGeometryPtr_t sphere1 = make_shared<Sphere>(0.2);
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 CollisionObject object0(sphere0);
103
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 CollisionObject object1(sphere1);
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 const Vec3s position0(Scalar(0.1), Scalar(0.2), Scalar(0.3));
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 const Vec3s position1(Scalar(0.11), Scalar(0.21), Scalar(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 2 std::vector<CollisionObject*> objects;
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 objects.push_back(&object0);
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 objects.push_back(&object1);
121
122 2 std::vector<Vec3s> positions;
123
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 positions.push_back(position0);
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 positions.push_back(position1);
125
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 DynamicAABBTreeCollisionManager dynamic_tree;
127
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
6 for (size_t i = 0; i < objects.size(); ++i) {
128
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
4 objects[i]->setTranslation(positions[i]);
129
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 objects[i]->computeAABB();
130
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
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
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1 times.
18 for (int count = 0; count < 8; ++count) {
140
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
16 dynamic_tree.update();
141
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
16 dynamic_tree.distance(&callback);
142 }
143 2 }
144