Directory: | ./ |
---|---|
File: | include/coal/BV/AABB.h |
Date: | 2025-04-01 09:23:31 |
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 | 218878 | AABB(const AABB& other) = default; | |
81 | |||
82 | 703675 | 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 | 3401840 | inline bool overlap(const AABB& other) const { | |
112 |
2/2✓ Branch 2 taken 656267 times.
✓ Branch 3 taken 2745573 times.
|
3401840 | if (min_[0] > other.max_[0]) return false; |
113 |
2/2✓ Branch 2 taken 1129499 times.
✓ Branch 3 taken 1616074 times.
|
2745573 | if (min_[1] > other.max_[1]) return false; |
114 |
2/2✓ Branch 2 taken 930726 times.
✓ Branch 3 taken 685348 times.
|
1616074 | if (min_[2] > other.max_[2]) return false; |
115 | |||
116 |
2/2✓ Branch 2 taken 384740 times.
✓ Branch 3 taken 300608 times.
|
685348 | if (max_[0] < other.min_[0]) return false; |
117 |
2/2✓ Branch 2 taken 117822 times.
✓ Branch 3 taken 182786 times.
|
300608 | if (max_[1] < other.min_[1]) return false; |
118 |
2/2✓ Branch 2 taken 14857 times.
✓ Branch 3 taken 167929 times.
|
182786 | if (max_[2] < other.min_[2]) return false; |
119 | |||
120 | 167929 | 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 | 24579 | inline bool contain(const AABB& other) const { | |
182 |
2/2✓ Branch 4 taken 23377 times.
✓ Branch 5 taken 435 times.
|
48391 | return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && |
183 |
4/4✓ Branch 2 taken 20712 times.
✓ Branch 3 taken 2665 times.
✓ Branch 6 taken 20355 times.
✓ Branch 7 taken 357 times.
|
23377 | (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && |
184 |
6/6✓ Branch 0 taken 23812 times.
✓ Branch 1 taken 767 times.
✓ Branch 4 taken 20154 times.
✓ Branch 5 taken 201 times.
✓ Branch 8 taken 20013 times.
✓ Branch 9 taken 141 times.
|
48391 | (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 | 23326 | inline bool overlap(const AABB& other, AABB& overlap_part) const { | |
189 |
2/2✓ Branch 1 taken 333 times.
✓ Branch 2 taken 22993 times.
|
23326 | if (!overlap(other)) { |
190 | 333 | return false; | |
191 | } | ||
192 | |||
193 |
1/2✓ Branch 2 taken 22993 times.
✗ Branch 3 not taken.
|
22993 | overlap_part.min_ = min_.cwiseMax(other.min_); |
194 |
1/2✓ Branch 2 taken 22993 times.
✗ Branch 3 not taken.
|
22993 | overlap_part.max_ = max_.cwiseMin(other.max_); |
195 | 22993 | 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 |