GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/BV/BV_node.h Lines: 20 21 95.2 %
Date: 2024-02-09 12:57:42 Branches: 5 10 50.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-2015, 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_BV_NODE_H
39
#define HPP_FCL_BV_NODE_H
40
41
#include <hpp/fcl/data_types.h>
42
43
#include <hpp/fcl/BV/BV.h>
44
#include <iostream>
45
46
namespace hpp {
47
namespace fcl {
48
49
/// @defgroup Construction_Of_BVH Construction of BVHModel
50
/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy)
51
/// @{
52
53
/// @brief BVNodeBase encodes the tree structure for BVH
54
struct HPP_FCL_DLLAPI BVNodeBase {
55
  /// @brief An index for first child node or primitive
56
  /// If the value is positive, it is the index of the first child bv node
57
  /// If the value is negative, it is -(primitive index + 1)
58
  /// Zero is not used.
59
  int first_child;
60
61
  /// @brief The start id the primitive belonging to the current node. The index
62
  /// is referred to the primitive_indices in BVHModel and from that we can
63
  /// obtain the primitive's index in original data indirectly.
64
  unsigned int first_primitive;
65
66
  /// @brief The number of primitives belonging to the current node
67
  unsigned int num_primitives;
68
69
  /// @brief Default constructor
70
2048031
  BVNodeBase()
71
2048031
      : first_child(0),
72
        first_primitive(
73
2048031
            (std::numeric_limits<unsigned int>::max)())  // value we should help
74
                                                         // to raise an issue
75
        ,
76
2048031
        num_primitives(0) {}
77
78
  /// @brief Equality operator
79
39662
  bool operator==(const BVNodeBase& other) const {
80
79324
    return first_child == other.first_child &&
81

79324
           first_primitive == other.first_primitive &&
82
79324
           num_primitives == other.num_primitives;
83
  }
84
85
  /// @brief Difference operator
86
  bool operator!=(const BVNodeBase& other) const { return !(*this == other); }
87
88
  /// @brief Whether current node is a leaf node (i.e. contains a primitive
89
  /// index
90
162289610
  inline bool isLeaf() const { return first_child < 0; }
91
92
  /// @brief Return the primitive index. The index is referred to the original
93
  /// data (i.e. vertices or tri_indices) in BVHModel
94
59009497
  inline int primitiveId() const { return -(first_child + 1); }
95
96
  /// @brief Return the index of the first child. The index is referred to the
97
  /// bounding volume array (i.e. bvs) in BVHModel
98
22756747
  inline int leftChild() const { return first_child; }
99
100
  /// @brief Return the index of the second child. The index is referred to the
101
  /// bounding volume array (i.e. bvs) in BVHModel
102
22756498
  inline int rightChild() const { return first_child + 1; }
103
};
104
105
/// @brief A class describing a bounding volume node. It includes the tree
106
/// structure providing in BVNodeBase and also the geometry data provided in BV
107
/// template parameter.
108
template <typename BV>
109
struct HPP_FCL_DLLAPI BVNode : public BVNodeBase {
110
  typedef BVNodeBase Base;
111
112
  /// @brief bounding volume storing the geometry
113
  BV bv;
114
115
  /// @brief Equality operator
116
39662
  bool operator==(const BVNode& other) const {
117

39662
    return Base::operator==(other) && bv == other.bv;
118
  }
119
120
  /// @brief Difference operator
121
39662
  bool operator!=(const BVNode& other) const { return !(*this == other); }
122
123
  /// @brief Check whether two BVNode collide
124
  bool overlap(const BVNode& other) const { return bv.overlap(other.bv); }
125
  /// @brief Check whether two BVNode collide
126
37740092
  bool overlap(const BVNode& other, const CollisionRequest& request,
127
               FCL_REAL& sqrDistLowerBound) const {
128
37740092
    return bv.overlap(other.bv, request, sqrDistLowerBound);
129
  }
130
131
  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
132
  /// the underlying BV supports distance, return the nearest points.
133
5556
  FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL,
134
                    Vec3f* P2 = NULL) const {
135
5556
    return bv.distance(other.bv, P1, P2);
136
  }
137
138
  /// @brief Access to the center of the BV
139
  Vec3f getCenter() const { return bv.center(); }
140
141
  /// @brief Access to the orientation of the BV
142
  const Matrix3f& getOrientation() const {
143
    static const Matrix3f id3 = Matrix3f::Identity();
144
    return id3;
145
  }
146
147
  /// \cond
148
12414
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149
  /// \endcond
150
};
151
152
template <>
153
inline const Matrix3f& BVNode<OBB>::getOrientation() const {
154
  return bv.axes;
155
}
156
157
template <>
158
inline const Matrix3f& BVNode<RSS>::getOrientation() const {
159
  return bv.axes;
160
}
161
162
template <>
163
inline const Matrix3f& BVNode<OBBRSS>::getOrientation() const {
164
  return bv.obb.axes;
165
}
166
167
}  // namespace fcl
168
169
}  // namespace hpp
170
171
#endif