GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h Lines: 0 99 0.0 %
Date: 2024-02-09 12:57:42 Branches: 0 230 0.0 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
5
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
6
 *  All rights reserved.
7
 *
8
 *  Redistribution and use in source and binary forms, with or without
9
 *  modification, are permitted provided that the following conditions
10
 *  are met:
11
 *
12
 *   * Redistributions of source code must retain the above copyright
13
 *     notice, this list of conditions and the following disclaimer.
14
 *   * Redistributions in binary form must reproduce the above
15
 *     copyright notice, this list of conditions and the following
16
 *     disclaimer in the documentation and/or other materials provided
17
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
 *     contributors may be used to endorse or promote products derived
20
 *     from this software without specific prior written permission.
21
 *
22
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
 *  POSSIBILITY OF SUCH DAMAGE.
34
 */
35
36
/** @author Jia Pan */
37
38
#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
39
#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
40
41
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
42
43
#include <limits>
44
45
#if HPP_FCL_HAVE_OCTOMAP
46
#include "hpp/fcl/octree.h"
47
#endif
48
49
#include "hpp/fcl/BV/BV.h"
50
#include "hpp/fcl/shape/geometric_shapes_utility.h"
51
52
namespace hpp {
53
namespace fcl {
54
namespace detail {
55
56
namespace dynamic_AABB_tree {
57
58
#if HPP_FCL_HAVE_OCTOMAP
59
60
//==============================================================================
61
template <typename Derived>
62
bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
63
                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
64
                       const AABB& root2_bv,
65
                       const Eigen::MatrixBase<Derived>& translation2,
66
                       CollisionCallBackBase* callback) {
67
  if (!root2) {
68
    if (root1->isLeaf()) {
69
      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
70
71
      if (!obj1->collisionGeometry()->isFree()) {
72
        const AABB& root2_bv_t = translate(root2_bv, translation2);
73
        if (root1->bv.overlap(root2_bv_t)) {
74
          Box* box = new Box();
75
          Transform3f box_tf;
76
          Transform3f tf2 = Transform3f::Identity();
77
          tf2.translation() = translation2;
78
          constructBox(root2_bv, tf2, *box, box_tf);
79
80
          box->cost_density =
81
              tree2->getOccupancyThres();  // thresholds are 0, 1, so uncertain
82
83
          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
84
          return (*callback)(obj1, &obj2);
85
        }
86
      }
87
    } else {
88
      if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
89
                            translation2, callback))
90
        return true;
91
      if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
92
                            translation2, callback))
93
        return true;
94
    }
95
96
    return false;
97
  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
98
    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
99
100
    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
101
      const AABB& root2_bv_t = translate(root2_bv, translation2);
102
      if (root1->bv.overlap(root2_bv_t)) {
103
        Box* box = new Box();
104
        Transform3f box_tf;
105
        Transform3f tf2 = Transform3f::Identity();
106
        tf2.translation() = translation2;
107
        constructBox(root2_bv, tf2, *box, box_tf);
108
109
        box->cost_density = root2->getOccupancy();
110
        box->threshold_occupied = tree2->getOccupancyThres();
111
112
        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
113
        return (*callback)(obj1, &obj2);
114
      } else
115
        return false;
116
    } else
117
      return false;
118
  }
119
120
  const AABB& root2_bv_t = translate(root2_bv, translation2);
121
  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
122
123
  if (!tree2->nodeHasChildren(root2) ||
124
      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
125
    if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
126
                          translation2, callback))
127
      return true;
128
    if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
129
                          translation2, callback))
130
      return true;
131
  } else {
132
    for (unsigned int i = 0; i < 8; ++i) {
133
      if (tree2->nodeChildExists(root2, i)) {
134
        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
135
        AABB child_bv;
136
        computeChildBV(root2_bv, i, child_bv);
137
138
        if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
139
                              callback))
140
          return true;
141
      } else {
142
        AABB child_bv;
143
        computeChildBV(root2_bv, i, child_bv);
144
        if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
145
                              callback))
146
          return true;
147
      }
148
    }
149
  }
150
  return false;
151
}
152
153
//==============================================================================
154
template <typename Derived>
155
bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
156
                      const OcTree* tree2, const OcTree::OcTreeNode* root2,
157
                      const AABB& root2_bv,
158
                      const Eigen::MatrixBase<Derived>& translation2,
159
                      DistanceCallBackBase* callback, FCL_REAL& min_dist) {
160
  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
161
    if (tree2->isNodeOccupied(root2)) {
162
      Box* box = new Box();
163
      Transform3f box_tf;
164
      Transform3f tf2 = Transform3f::Identity();
165
      tf2.translation() = translation2;
166
      constructBox(root2_bv, tf2, *box, box_tf);
167
      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
168
      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
169
                         min_dist);
170
    } else
171
      return false;
172
  }
173
174
  if (!tree2->isNodeOccupied(root2)) return false;
175
176
  if (!tree2->nodeHasChildren(root2) ||
177
      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
178
    const AABB& aabb2 = translate(root2_bv, translation2);
179
    FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
180
    FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
181
182
    if (d2 < d1) {
183
      if (d2 < min_dist) {
184
        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
185
                             translation2, callback, min_dist))
186
          return true;
187
      }
188
189
      if (d1 < min_dist) {
190
        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
191
                             translation2, callback, min_dist))
192
          return true;
193
      }
194
    } else {
195
      if (d1 < min_dist) {
196
        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
197
                             translation2, callback, min_dist))
198
          return true;
199
      }
200
201
      if (d2 < min_dist) {
202
        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
203
                             translation2, callback, min_dist))
204
          return true;
205
      }
206
    }
207
  } else {
208
    for (unsigned int i = 0; i < 8; ++i) {
209
      if (tree2->nodeChildExists(root2, i)) {
210
        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
211
        AABB child_bv;
212
        computeChildBV(root2_bv, i, child_bv);
213
        const AABB& aabb2 = translate(child_bv, translation2);
214
215
        FCL_REAL d = root1->bv.distance(aabb2);
216
217
        if (d < min_dist) {
218
          if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
219
                               callback, min_dist))
220
            return true;
221
        }
222
      }
223
    }
224
  }
225
226
  return false;
227
}
228
229
#endif
230
231
}  // namespace dynamic_AABB_tree
232
}  // namespace detail
233
}  // namespace fcl
234
}  // namespace hpp
235
236
#endif