GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: test/broadphase_dynamic_AABB_tree.cpp Lines: 34 34 100.0 %
Date: 2024-02-09 12:57:42 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
#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
}