GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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_AABB_H |
||
39 |
#define HPP_FCL_AABB_H |
||
40 |
|||
41 |
#include <hpp/fcl/data_types.h> |
||
42 |
|||
43 |
namespace hpp { |
||
44 |
namespace fcl { |
||
45 |
|||
46 |
struct CollisionRequest; |
||
47 |
|||
48 |
/// @defgroup Bounding_Volume Bounding volumes |
||
49 |
/// Classes of different types of bounding volume. |
||
50 |
/// @{ |
||
51 |
|||
52 |
/// @brief A class describing the AABB collision structure, which is a box in 3D |
||
53 |
/// space determined by two diagonal points |
||
54 |
616944 |
class HPP_FCL_DLLAPI AABB { |
|
55 |
public: |
||
56 |
/// @brief The min point in the AABB |
||
57 |
Vec3f min_; |
||
58 |
/// @brief The max point in the AABB |
||
59 |
Vec3f max_; |
||
60 |
|||
61 |
/// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) |
||
62 |
AABB(); |
||
63 |
|||
64 |
/// @brief Creating an AABB at position v with zero size |
||
65 |
185400 |
AABB(const Vec3f& v) : min_(v), max_(v) {} |
|
66 |
|||
67 |
/// @brief Creating an AABB with two endpoints a and b |
||
68 |
216218 |
AABB(const Vec3f& a, const Vec3f& b) |
|
69 |
✓✗✓✗ |
216218 |
: min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} |
70 |
|||
71 |
/// @brief Creating an AABB centered as core and is of half-dimension delta |
||
72 |
258 |
AABB(const AABB& core, const Vec3f& delta) |
|
73 |
✓✗✓✗ |
258 |
: min_(core.min_ - delta), max_(core.max_ + delta) {} |
74 |
|||
75 |
/// @brief Creating an AABB contains three points |
||
76 |
AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) |
||
77 |
: min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} |
||
78 |
|||
79 |
214097 |
AABB(const AABB& other) = default; |
|
80 |
|||
81 |
AABB& operator=(const AABB& other) = default; |
||
82 |
|||
83 |
AABB& update(const Vec3f& a, const Vec3f& b) { |
||
84 |
min_ = a.cwiseMin(b); |
||
85 |
max_ = a.cwiseMax(b); |
||
86 |
return *this; |
||
87 |
} |
||
88 |
|||
89 |
/// @brief Comparison operator |
||
90 |
22650 |
bool operator==(const AABB& other) const { |
|
91 |
✓✓✓✓ |
22650 |
return min_ == other.min_ && max_ == other.max_; |
92 |
} |
||
93 |
|||
94 |
bool operator!=(const AABB& other) const { return !(*this == other); } |
||
95 |
|||
96 |
/// @name Bounding volume API |
||
97 |
/// Common API to BVs. |
||
98 |
/// @{ |
||
99 |
|||
100 |
/// @brief Check whether the AABB contains a point |
||
101 |
21208 |
inline bool contain(const Vec3f& p) const { |
|
102 |
✓✓✗✓ ✓✓ |
21208 |
if (p[0] < min_[0] || p[0] > max_[0]) return false; |
103 |
✓✓✓✓ ✓✓ |
20920 |
if (p[1] < min_[1] || p[1] > max_[1]) return false; |
104 |
✓✓✓✓ ✓✓ |
20400 |
if (p[2] < min_[2] || p[2] > max_[2]) return false; |
105 |
|||
106 |
20328 |
return true; |
|
107 |
} |
||
108 |
|||
109 |
/// @brief Check whether two AABB are overlap |
||
110 |
3392643 |
inline bool overlap(const AABB& other) const { |
|
111 |
✓✓ | 3392643 |
if (min_[0] > other.max_[0]) return false; |
112 |
✓✓ | 2726517 |
if (min_[1] > other.max_[1]) return false; |
113 |
✓✓ | 1624909 |
if (min_[2] > other.max_[2]) return false; |
114 |
|||
115 |
✓✓ | 659029 |
if (max_[0] < other.min_[0]) return false; |
116 |
✓✓ | 268331 |
if (max_[1] < other.min_[1]) return false; |
117 |
✓✓ | 161883 |
if (max_[2] < other.min_[2]) return false; |
118 |
|||
119 |
144774 |
return true; |
|
120 |
} |
||
121 |
|||
122 |
/// @brief Check whether two AABB are overlap |
||
123 |
bool overlap(const AABB& other, const CollisionRequest& request, |
||
124 |
FCL_REAL& sqrDistLowerBound) const; |
||
125 |
|||
126 |
/// @brief Distance between two AABBs |
||
127 |
FCL_REAL distance(const AABB& other) const; |
||
128 |
|||
129 |
/// @brief Distance between two AABBs; P and Q, should not be NULL, return the |
||
130 |
/// nearest points |
||
131 |
FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; |
||
132 |
|||
133 |
/// @brief Merge the AABB and a point |
||
134 |
7142972 |
inline AABB& operator+=(const Vec3f& p) { |
|
135 |
✓✗ | 7142972 |
min_ = min_.cwiseMin(p); |
136 |
✓✗ | 7142972 |
max_ = max_.cwiseMax(p); |
137 |
7142972 |
return *this; |
|
138 |
} |
||
139 |
|||
140 |
/// @brief Merge the AABB and another AABB |
||
141 |
312185 |
inline AABB& operator+=(const AABB& other) { |
|
142 |
✓✗ | 312185 |
min_ = min_.cwiseMin(other.min_); |
143 |
✓✗ | 312185 |
max_ = max_.cwiseMax(other.max_); |
144 |
312185 |
return *this; |
|
145 |
} |
||
146 |
|||
147 |
/// @brief Return the merged AABB of current AABB and the other one |
||
148 |
53042 |
inline AABB operator+(const AABB& other) const { |
|
149 |
✓✗ | 53042 |
AABB res(*this); |
150 |
✓✗✓✗ |
106084 |
return res += other; |
151 |
} |
||
152 |
|||
153 |
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs) |
||
154 |
✓✗ | 129602 |
inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); } |
155 |
|||
156 |
/// @brief Center of the AABB |
||
157 |
✓✗✓✗ ✓✗ |
3337998 |
inline Vec3f center() const { return (min_ + max_) * 0.5; } |
158 |
|||
159 |
/// @brief Width of the AABB |
||
160 |
389004 |
inline FCL_REAL width() const { return max_[0] - min_[0]; } |
|
161 |
|||
162 |
/// @brief Height of the AABB |
||
163 |
385873 |
inline FCL_REAL height() const { return max_[1] - min_[1]; } |
|
164 |
|||
165 |
/// @brief Depth of the AABB |
||
166 |
239114 |
inline FCL_REAL depth() const { return max_[2] - min_[2]; } |
|
167 |
|||
168 |
/// @brief Volume of the AABB |
||
169 |
2704 |
inline FCL_REAL volume() const { return width() * height() * depth(); } |
|
170 |
|||
171 |
/// @} |
||
172 |
|||
173 |
/// @brief Check whether the AABB contains another AABB |
||
174 |
24519 |
inline bool contain(const AABB& other) const { |
|
175 |
✓✓ | 48297 |
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && |
176 |
✓✓✓✓ |
23328 |
(other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && |
177 |
✓✓✓✓ ✓✓ |
48297 |
(other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); |
178 |
} |
||
179 |
|||
180 |
/// @brief Check whether two AABB are overlap and return the overlap part |
||
181 |
23470 |
inline bool overlap(const AABB& other, AABB& overlap_part) const { |
|
182 |
✓✓ | 23470 |
if (!overlap(other)) { |
183 |
492 |
return false; |
|
184 |
} |
||
185 |
|||
186 |
✓✗ | 22978 |
overlap_part.min_ = min_.cwiseMax(other.min_); |
187 |
✓✗ | 22978 |
overlap_part.max_ = max_.cwiseMin(other.max_); |
188 |
22978 |
return true; |
|
189 |
} |
||
190 |
|||
191 |
/// @brief Check whether two AABB are overlapped along specific axis |
||
192 |
6476 |
inline bool axisOverlap(const AABB& other, int axis_id) const { |
|
193 |
✓✓ | 6476 |
if (min_[axis_id] > other.max_[axis_id]) return false; |
194 |
✓✓ | 3291 |
if (max_[axis_id] < other.min_[axis_id]) return false; |
195 |
|||
196 |
140 |
return true; |
|
197 |
} |
||
198 |
|||
199 |
/// @brief expand the half size of the AABB by delta, and keep the center |
||
200 |
/// unchanged. |
||
201 |
12008 |
inline AABB& expand(const Vec3f& delta) { |
|
202 |
12008 |
min_ -= delta; |
|
203 |
12008 |
max_ += delta; |
|
204 |
12008 |
return *this; |
|
205 |
} |
||
206 |
|||
207 |
/// @brief expand the half size of the AABB by a scalar delta, and keep the |
||
208 |
/// center unchanged. |
||
209 |
inline AABB& expand(const FCL_REAL delta) { |
||
210 |
min_.array() -= delta; |
||
211 |
max_.array() += delta; |
||
212 |
return *this; |
||
213 |
} |
||
214 |
|||
215 |
/// @brief expand the aabb by increase the thickness of the plate by a ratio |
||
216 |
221 |
inline AABB& expand(const AABB& core, FCL_REAL ratio) { |
|
217 |
✓✗✓✗ |
221 |
min_ = min_ * ratio - core.min_; |
218 |
✓✗✓✗ |
221 |
max_ = max_ * ratio - core.max_; |
219 |
221 |
return *this; |
|
220 |
} |
||
221 |
|||
222 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
223 |
}; |
||
224 |
|||
225 |
/// @brief translate the center of AABB by t |
||
226 |
37039 |
static inline AABB translate(const AABB& aabb, const Vec3f& t) { |
|
227 |
37039 |
AABB res(aabb); |
|
228 |
37039 |
res.min_ += t; |
|
229 |
37039 |
res.max_ += t; |
|
230 |
37039 |
return res; |
|
231 |
} |
||
232 |
|||
233 |
28474 |
static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { |
|
234 |
✓✗✓✗ ✓✗ |
28474 |
AABB res(R * aabb.min_); |
235 |
✓✗ | 28474 |
Vec3f corner(aabb.min_); |
236 |
28474 |
const Eigen::DenseIndex bit[3] = {1, 2, 4}; |
|
237 |
✓✓ | 227792 |
for (Eigen::DenseIndex ic = 1; ic < 8; |
238 |
++ic) { // ic = 0 corresponds to aabb.min_. Skip it. |
||
239 |
✓✓ | 797272 |
for (Eigen::DenseIndex i = 0; i < 3; ++i) { |
240 |
✓✓✓✗ ✓✗✓✗ |
597954 |
corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; |
241 |
} |
||
242 |
✓✗✓✗ ✓✗ |
199318 |
res += R * corner; |
243 |
} |
||
244 |
56948 |
return res; |
|
245 |
} |
||
246 |
|||
247 |
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) |
||
248 |
/// and b2 is in identity. |
||
249 |
HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, |
||
250 |
const AABB& b2); |
||
251 |
|||
252 |
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) |
||
253 |
/// and b2 is in identity. |
||
254 |
HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, |
||
255 |
const AABB& b2, const CollisionRequest& request, |
||
256 |
FCL_REAL& sqrDistLowerBound); |
||
257 |
} // namespace fcl |
||
258 |
|||
259 |
} // namespace hpp |
||
260 |
|||
261 |
#endif |
Generated by: GCOVR (Version 4.2) |