hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
AABB.h
Go to the documentation of this file.
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
38#ifndef HPP_FCL_AABB_H
39#define HPP_FCL_AABB_H
40
41#include <hpp/fcl/data_types.h>
42
43namespace hpp {
44namespace fcl {
45
46struct CollisionRequest;
47
51
55 public:
60
63
65 AABB(const Vec3f& v) : min_(v), max_(v) {}
66
68 AABB(const Vec3f& a, const Vec3f& b)
69 : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
70
72 AABB(const AABB& core, const Vec3f& delta)
73 : min_(core.min_ - delta), max_(core.max_ + delta) {}
74
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 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
90 bool operator==(const AABB& other) const {
91 return min_ == other.min_ && max_ == other.max_;
92 }
93
94 bool operator!=(const AABB& other) const { return !(*this == other); }
95
99
101 inline bool contain(const Vec3f& p) const {
102 if (p[0] < min_[0] || p[0] > max_[0]) return false;
103 if (p[1] < min_[1] || p[1] > max_[1]) return false;
104 if (p[2] < min_[2] || p[2] > max_[2]) return false;
105
106 return true;
107 }
108
110 inline bool overlap(const AABB& other) const {
111 if (min_[0] > other.max_[0]) return false;
112 if (min_[1] > other.max_[1]) return false;
113 if (min_[2] > other.max_[2]) return false;
114
115 if (max_[0] < other.min_[0]) return false;
116 if (max_[1] < other.min_[1]) return false;
117 if (max_[2] < other.min_[2]) return false;
118
119 return true;
120 }
121
123 bool overlap(const AABB& other, const CollisionRequest& request,
124 FCL_REAL& sqrDistLowerBound) const;
125
127 FCL_REAL distance(const AABB& other) const;
128
131 FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
132
134 inline AABB& operator+=(const Vec3f& p) {
135 min_ = min_.cwiseMin(p);
136 max_ = max_.cwiseMax(p);
137 return *this;
138 }
139
141 inline AABB& operator+=(const AABB& other) {
142 min_ = min_.cwiseMin(other.min_);
143 max_ = max_.cwiseMax(other.max_);
144 return *this;
145 }
146
148 inline AABB operator+(const AABB& other) const {
149 AABB res(*this);
150 return res += other;
151 }
152
154 inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); }
155
157 inline Vec3f center() const { return (min_ + max_) * 0.5; }
158
160 inline FCL_REAL width() const { return max_[0] - min_[0]; }
161
163 inline FCL_REAL height() const { return max_[1] - min_[1]; }
164
166 inline FCL_REAL depth() const { return max_[2] - min_[2]; }
167
169 inline FCL_REAL volume() const { return width() * height() * depth(); }
170
172
174 inline bool contain(const AABB& other) const {
175 return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) &&
176 (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) &&
177 (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
178 }
179
181 inline bool overlap(const AABB& other, AABB& overlap_part) const {
182 if (!overlap(other)) {
183 return false;
184 }
185
186 overlap_part.min_ = min_.cwiseMax(other.min_);
187 overlap_part.max_ = max_.cwiseMin(other.max_);
188 return true;
189 }
190
192 inline bool axisOverlap(const AABB& other, int axis_id) const {
193 if (min_[axis_id] > other.max_[axis_id]) return false;
194 if (max_[axis_id] < other.min_[axis_id]) return false;
195
196 return true;
197 }
198
201 inline AABB& expand(const Vec3f& delta) {
202 min_ -= delta;
203 max_ += delta;
204 return *this;
205 }
206
209 inline AABB& expand(const FCL_REAL delta) {
210 min_.array() -= delta;
211 max_.array() += delta;
212 return *this;
213 }
214
216 inline AABB& expand(const AABB& core, FCL_REAL ratio) {
217 min_ = min_ * ratio - core.min_;
218 max_ = max_ * ratio - core.max_;
219 return *this;
220 }
221
222 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223};
224
226static inline AABB translate(const AABB& aabb, const Vec3f& t) {
227 AABB res(aabb);
228 res.min_ += t;
229 res.max_ += t;
230 return res;
231}
232
233static inline AABB rotate(const AABB& aabb, const Matrix3f& R) {
234 AABB res(R * aabb.min_);
235 Vec3f corner(aabb.min_);
236 const Eigen::DenseIndex bit[3] = {1, 2, 4};
237 for (Eigen::DenseIndex ic = 1; ic < 8;
238 ++ic) { // ic = 0 corresponds to aabb.min_. Skip it.
239 for (Eigen::DenseIndex i = 0; i < 3; ++i) {
240 corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
241 }
242 res += R * corner;
243 }
244 return res;
245}
246
249HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
250 const AABB& b2);
251
254HPP_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
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
bool overlap(const AABB &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two AABB are overlap.
AABB(const Vec3f &v)
Creating an AABB at position v with zero size.
Definition: AABB.h:65
bool contain(const AABB &other) const
Check whether the AABB contains another AABB.
Definition: AABB.h:174
bool operator!=(const AABB &other) const
Definition: AABB.h:94
FCL_REAL depth() const
Depth of the AABB.
Definition: AABB.h:166
FCL_REAL volume() const
Volume of the AABB.
Definition: AABB.h:169
AABB & update(const Vec3f &a, const Vec3f &b)
Definition: AABB.h:83
AABB operator+(const AABB &other) const
Return the merged AABB of current AABB and the other one.
Definition: AABB.h:148
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
AABB(const AABB &core, const Vec3f &delta)
Creating an AABB centered as core and is of half-dimension delta.
Definition: AABB.h:72
bool overlap(const AABB &other, AABB &overlap_part) const
Check whether two AABB are overlap and return the overlap part.
Definition: AABB.h:181
FCL_REAL width() const
Width of the AABB.
Definition: AABB.h:160
bool operator==(const AABB &other) const
Comparison operator.
Definition: AABB.h:90
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: AABB.h:110
AABB & operator+=(const Vec3f &p)
Merge the AABB and a point.
Definition: AABB.h:134
AABB(const Vec3f &a, const Vec3f &b)
Creating an AABB with two endpoints a and b.
Definition: AABB.h:68
AABB & operator+=(const AABB &other)
Merge the AABB and another AABB.
Definition: AABB.h:141
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
AABB & expand(const Vec3f &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: AABB.h:201
bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool axisOverlap(const AABB &other, int axis_id) const
Check whether two AABB are overlapped along specific axis.
Definition: AABB.h:192
AABB()
Creating an AABB with zero size (low bound +inf, upper bound -inf)
AABB & expand(const FCL_REAL delta)
expand the half size of the AABB by a scalar delta, and keep the center unchanged.
Definition: AABB.h:209
AABB & operator=(const AABB &other)=default
AABB(const Vec3f &a, const Vec3f &b, const Vec3f &c)
Creating an AABB contains three points.
Definition: AABB.h:76
AABB & expand(const AABB &core, FCL_REAL ratio)
expand the aabb by increase the thickness of the plate by a ratio
Definition: AABB.h:216
bool contain(const Vec3f &p) const
Check whether the AABB contains a point.
Definition: AABB.h:101
FCL_REAL height() const
Height of the AABB.
Definition: AABB.h:163
FCL_REAL distance(const AABB &other, Vec3f *P, Vec3f *Q) const
Distance between two AABBs; P and Q, should not be NULL, return the nearest points.
AABB(const AABB &other)=default
Vec3f center() const
Center of the AABB.
Definition: AABB.h:157
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: AABB.h:154
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
request to the collision algorithm
Definition: collision_data.h:235