GCC Code Coverage Report

Directory: ./
File: include/coal/BV/AABB.h
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 77 82 93.9%
Branches: 92 132 69.7%
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 COAL_AABB_H
39 #define COAL_AABB_H
40
41 #include "coal/data_types.h"
42
43 namespace coal {
44
45 struct CollisionRequest;
46 class Plane;
47 class Halfspace;
48
49 /// @defgroup Bounding_Volume Bounding volumes
50 /// Classes of different types of bounding volume.
51 /// @{
52
53 /// @brief A class describing the AABB collision structure, which is a box in 3D
54 /// space determined by two diagonal points
55 class COAL_DLLAPI AABB {
56 public:
57 /// @brief The min point in the AABB
58 Vec3s min_;
59 /// @brief The max point in the AABB
60 Vec3s max_;
61
62 /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
63 AABB();
64
65 /// @brief Creating an AABB at position v with zero size
66 188569 AABB(const Vec3s& v) : min_(v), max_(v) {}
67
68 /// @brief Creating an AABB with two endpoints a and b
69 238829 AABB(const Vec3s& a, const Vec3s& b)
70
2/4
✓ Branch 2 taken 238829 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 238829 times.
✗ Branch 7 not taken.
 238829 : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
71
72 /// @brief Creating an AABB centered as core and is of half-dimension delta
73 258 AABB(const AABB& core, const Vec3s& delta)
74
2/4
✓ Branch 2 taken 258 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 258 times.
✗ Branch 7 not taken.
 258 : min_(core.min_ - delta), max_(core.max_ + delta) {}
75
76 /// @brief Creating an AABB contains three points
77 1 AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c)
78
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
79
80 218873 AABB(const AABB& other) = default;
81
82 703661 AABB& operator=(const AABB& other) = default;
83
84 AABB& update(const Vec3s& a, const Vec3s& b) {
85 min_ = a.cwiseMin(b);
86 max_ = a.cwiseMax(b);
87 return *this;
88 }
89
90 /// @brief Comparison operator
91 22774 bool operator==(const AABB& other) const {
92
4/4
✓ Branch 1 taken 20450 times.
✓ Branch 2 taken 2324 times.
✓ Branch 4 taken 20248 times.
✓ Branch 5 taken 202 times.
 22774 return min_ == other.min_ && max_ == other.max_;
93 }
94
95 ✗ bool operator!=(const AABB& other) const { return !(*this == other); }
96
97 /// @name Bounding volume API
98 /// Common API to BVs.
99 /// @{
100
101 /// @brief Check whether the AABB contains a point
102 21208 inline bool contain(const Vec3s& p) const {
103
5/6
✓ Branch 2 taken 20920 times.
✓ Branch 3 taken 288 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 20920 times.
✓ Branch 8 taken 288 times.
✓ Branch 9 taken 20920 times.
 21208 if (p[0] < min_[0] || p[0] > max_[0]) return false;
104
6/6
✓ Branch 2 taken 20496 times.
✓ Branch 3 taken 424 times.
✓ Branch 6 taken 96 times.
✓ Branch 7 taken 20400 times.
✓ Branch 8 taken 520 times.
✓ Branch 9 taken 20400 times.
 20920 if (p[1] < min_[1] || p[1] > max_[1]) return false;
105
6/6
✓ Branch 2 taken 20360 times.
✓ Branch 3 taken 40 times.
✓ Branch 6 taken 32 times.
✓ Branch 7 taken 20328 times.
✓ Branch 8 taken 72 times.
✓ Branch 9 taken 20328 times.
 20400 if (p[2] < min_[2] || p[2] > max_[2]) return false;
106
107 20328 return true;
108 }
109
110 /// @brief Check whether two AABB are overlap
111 3401842 inline bool overlap(const AABB& other) const {
112
2/2
✓ Branch 2 taken 656267 times.
✓ Branch 3 taken 2745575 times.
 3401842 if (min_[0] > other.max_[0]) return false;
113
2/2
✓ Branch 2 taken 1129499 times.
✓ Branch 3 taken 1616076 times.
 2745575 if (min_[1] > other.max_[1]) return false;
114
2/2
✓ Branch 2 taken 930726 times.
✓ Branch 3 taken 685350 times.
 1616076 if (min_[2] > other.max_[2]) return false;
115
116
2/2
✓ Branch 2 taken 384740 times.
✓ Branch 3 taken 300610 times.
 685350 if (max_[0] < other.min_[0]) return false;
117
2/2
✓ Branch 2 taken 117822 times.
✓ Branch 3 taken 182788 times.
 300610 if (max_[1] < other.min_[1]) return false;
118
2/2
✓ Branch 2 taken 14857 times.
✓ Branch 3 taken 167931 times.
 182788 if (max_[2] < other.min_[2]) return false;
119
120 167931 return true;
121 }
122
123 /// @brief Check whether AABB overlaps a plane
124 bool overlap(const Plane& p) const;
125
126 /// @brief Check whether AABB overlaps a halfspace
127 bool overlap(const Halfspace& hs) const;
128
129 /// @brief Check whether two AABB are overlap
130 bool overlap(const AABB& other, const CollisionRequest& request,
131 Scalar& sqrDistLowerBound) const;
132
133 /// @brief Distance between two AABBs
134 Scalar distance(const AABB& other) const;
135
136 /// @brief Distance between two AABBs; P and Q, should not be NULL, return the
137 /// nearest points
138 Scalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const;
139
140 /// @brief Merge the AABB and a point
141 7171703 inline AABB& operator+=(const Vec3s& p) {
142
1/2
✓ Branch 2 taken 7171703 times.
✗ Branch 3 not taken.
 7171703 min_ = min_.cwiseMin(p);
143
1/2
✓ Branch 2 taken 7171703 times.
✗ Branch 3 not taken.
 7171703 max_ = max_.cwiseMax(p);
144 7171703 return *this;
145 }
146
147 /// @brief Merge the AABB and another AABB
148 312221 inline AABB& operator+=(const AABB& other) {
149
1/2
✓ Branch 2 taken 312221 times.
✗ Branch 3 not taken.
 312221 min_ = min_.cwiseMin(other.min_);
150
1/2
✓ Branch 2 taken 312221 times.
✗ Branch 3 not taken.
 312221 max_ = max_.cwiseMax(other.max_);
151 312221 return *this;
152 }
153
154 /// @brief Return the merged AABB of current AABB and the other one
155 53078 inline AABB operator+(const AABB& other) const {
156
1/2
✓ Branch 1 taken 53078 times.
✗ Branch 2 not taken.
 53078 AABB res(*this);
157
2/4
✓ Branch 1 taken 53078 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53078 times.
✗ Branch 5 not taken.
 106156 return res += other;
158 }
159
160 /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
161
1/2
✓ Branch 2 taken 161422 times.
✗ Branch 3 not taken.
 161422 inline Scalar size() const { return (max_ - min_).squaredNorm(); }
162
163 /// @brief Center of the AABB
164
3/6
✓ Branch 1 taken 1796689 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1796689 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1796689 times.
✗ Branch 8 not taken.
 3593378 inline Vec3s center() const { return (min_ + max_) * 0.5; }
165
166 /// @brief Width of the AABB
167 389004 inline Scalar width() const { return max_[0] - min_[0]; }
168
169 /// @brief Height of the AABB
170 385873 inline Scalar height() const { return max_[1] - min_[1]; }
171
172 /// @brief Depth of the AABB
173 239114 inline Scalar depth() const { return max_[2] - min_[2]; }
174
175 /// @brief Volume of the AABB
176 2704 inline Scalar volume() const { return width() * height() * depth(); }
177
178 /// @}
179
180 /// @brief Check whether the AABB contains another AABB
181 24581 inline bool contain(const AABB& other) const {
182
2/2
✓ Branch 4 taken 23379 times.
✓ Branch 5 taken 435 times.
 48395 return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) &&
183
4/4
✓ Branch 2 taken 20714 times.
✓ Branch 3 taken 2665 times.
✓ Branch 6 taken 20357 times.
✓ Branch 7 taken 357 times.
 23379 (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) &&
184
6/6
✓ Branch 0 taken 23814 times.
✓ Branch 1 taken 767 times.
✓ Branch 4 taken 20156 times.
✓ Branch 5 taken 201 times.
✓ Branch 8 taken 20015 times.
✓ Branch 9 taken 141 times.
 48395 (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
185 }
186
187 /// @brief Check whether two AABB are overlap and return the overlap part
188 23328 inline bool overlap(const AABB& other, AABB& overlap_part) const {
189
2/2
✓ Branch 1 taken 333 times.
✓ Branch 2 taken 22995 times.
 23328 if (!overlap(other)) {
190 333 return false;
191 }
192
193
1/2
✓ Branch 2 taken 22995 times.
✗ Branch 3 not taken.
 22995 overlap_part.min_ = min_.cwiseMax(other.min_);
194
1/2
✓ Branch 2 taken 22995 times.
✗ Branch 3 not taken.
 22995 overlap_part.max_ = max_.cwiseMin(other.max_);
195 22995 return true;
196 }
197
198 /// @brief Check whether two AABB are overlapped along specific axis
199 6248 inline bool axisOverlap(const AABB& other, int axis_id) const {
200
2/2
✓ Branch 2 taken 2995 times.
✓ Branch 3 taken 3253 times.
 6248 if (min_[axis_id] > other.max_[axis_id]) return false;
201
2/2
✓ Branch 2 taken 3060 times.
✓ Branch 3 taken 193 times.
 3253 if (max_[axis_id] < other.min_[axis_id]) return false;
202
203 193 return true;
204 }
205
206 /// @brief expand the half size of the AABB by delta, and keep the center
207 /// unchanged.
208 11999 inline AABB& expand(const Vec3s& delta) {
209 11999 min_ -= delta;
210 11999 max_ += delta;
211 11999 return *this;
212 }
213
214 /// @brief expand the half size of the AABB by a scalar delta, and keep the
215 /// center unchanged.
216 ✗ inline AABB& expand(const Scalar delta) {
217 ✗ min_.array() -= delta;
218 ✗ max_.array() += delta;
219 ✗ return *this;
220 }
221
222 /// @brief expand the aabb by increase the thickness of the plate by a ratio
223 242 inline AABB& expand(const AABB& core, Scalar ratio) {
224
2/4
✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
 242 min_ = min_ * ratio - core.min_;
225
2/4
✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
 242 max_ = max_ * ratio - core.max_;
226 242 return *this;
227 }
228
229 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 };
231
232 /// @brief translate the center of AABB by t
233 40208 static inline AABB translate(const AABB& aabb, const Vec3s& t) {
234 40208 AABB res(aabb);
235 40208 res.min_ += t;
236 40208 res.max_ += t;
237 40208 return res;
238 }
239
240 31643 static inline AABB rotate(const AABB& aabb, const Matrix3s& R) {
241
3/6
✓ Branch 1 taken 31643 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31643 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31643 times.
✗ Branch 8 not taken.
 31643 AABB res(R * aabb.min_);
242
1/2
✓ Branch 1 taken 31643 times.
✗ Branch 2 not taken.
 31643 Vec3s corner(aabb.min_);
243 31643 const Eigen::DenseIndex bit[3] = {1, 2, 4};
244
2/2
✓ Branch 0 taken 221501 times.
✓ Branch 1 taken 31643 times.
 253144 for (Eigen::DenseIndex ic = 1; ic < 8;
245 ++ic) { // ic = 0 corresponds to aabb.min_. Skip it.
246
2/2
✓ Branch 0 taken 664503 times.
✓ Branch 1 taken 221501 times.
 886004 for (Eigen::DenseIndex i = 0; i < 3; ++i) {
247
5/8
✓ Branch 0 taken 379716 times.
✓ Branch 1 taken 284787 times.
✓ Branch 3 taken 379716 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 284787 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 664503 times.
✗ Branch 10 not taken.
 664503 corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
248 }
249
3/6
✓ Branch 1 taken 221501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 221501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 221501 times.
✗ Branch 8 not taken.
 221501 res += R * corner;
250 }
251 63286 return res;
252 }
253
254 /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
255 /// and b2 is in identity.
256 COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
257 const AABB& b2);
258
259 /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
260 /// and b2 is in identity.
261 COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
262 const AABB& b2, const CollisionRequest& request,
263 Scalar& sqrDistLowerBound);
264 } // namespace coal
265
266 #endif
267