hpp-fcl: include/hpp/fcl/BV/AABB.h Source File

hpp-fcl 1.8.0 HPP fork of FCL -- The Flexible Collision Library

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
43 namespace hpp
44{
45 namespace fcl
46{
47
48struct CollisionRequest;
49
53
55 class HPP_FCL_DLLAPI AABB
56{
57public:
58
60 Vec3f min_;
62 Vec3f max_;
63
65 AABB();
66
68 AABB(const Vec3f& v) : min_(v), max_(v)
69 {
70 }
71
73 AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)),
74 max_(a.cwiseMax(b))
75 {
76 }
77
79 AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta),
80 max_(core.max_ + delta)
81 {
82 }
83
85 AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)),
86 max_(a.cwiseMax(b).cwiseMax(c))
87 {
88 }
89
90 AABB & update(const Vec3f& a, const Vec3f& b)
91 {
92 min_ = a.cwiseMin(b); max_ = a.cwiseMax(b);
93 return *this;
94 }
95
97 bool operator==(const AABB & other) const
98 {
99 return min_ == other.min_ && max_ == other.max_;
100 }
101
105
107 inline bool contain(const Vec3f& p) const
108 {
109 if(p[0] < min_[0] || p[0] > max_[0]) return false;
110 if(p[1] < min_[1] || p[1] > max_[1]) return false;
111 if(p[2] < min_[2] || p[2] > max_[2]) return false;
112
113 return true;
114 }
115
117 inline bool overlap(const AABB& other) const
118 {
119 if(min_[0] > other.max_[0]) return false;
120 if(min_[1] > other.max_[1]) return false;
121 if(min_[2] > other.max_[2]) return false;
122
123 if(max_[0] < other.min_[0]) return false;
124 if(max_[1] < other.min_[1]) return false;
125 if(max_[2] < other.min_[2]) return false;
126
127 return true;
128 }
129
131 bool overlap(const AABB& other, const CollisionRequest& request,
132 FCL_REAL& sqrDistLowerBound) const;
133
135 FCL_REAL distance(const AABB& other) const;
136
138 FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
139
141 inline AABB& operator += (const Vec3f& p)
142 {
143 min_ = min_.cwiseMin(p);
144 max_ = max_.cwiseMax(p);
145 return *this;
146 }
147
149 inline AABB& operator += (const AABB& other)
150 {
151 min_ = min_.cwiseMin(other.min_);
152 max_ = max_.cwiseMax(other.max_);
153 return *this;
154 }
155
157 inline AABB operator + (const AABB& other) const
158 {
159 AABB res(*this);
160 return res += other;
161 }
162
164 inline FCL_REAL size() const
165 {
166 return (max_ - min_).squaredNorm();
167 }
168
170 inline Vec3f center() const
171 {
172 return (min_ + max_) * 0.5;
173 }
174
176 inline FCL_REAL width() const
177 {
178 return max_[0] - min_[0];
179 }
180
182 inline FCL_REAL height() const
183 {
184 return max_[1] - min_[1];
185 }
186
188 inline FCL_REAL depth() const
189 {
190 return max_[2] - min_[2];
191 }
192
194 inline FCL_REAL volume() const
195 {
196 return width() * height() * depth();
197 }
198
200
202 inline bool contain(const AABB& other) const
203 {
204 return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
205 }
206
208 inline bool overlap(const AABB& other, AABB& overlap_part) const
209 {
210 if(!overlap(other))
211 {
212 return false;
213 }
214
215 overlap_part.min_ = min_.cwiseMax(other.min_);
216 overlap_part.max_ = max_.cwiseMin(other.max_);
217 return true;
218 }
219
221 inline AABB& expand(const Vec3f& delta)
222 {
223 min_ -= delta;
224 max_ += delta;
225 return *this;
226 }
227
229 inline AABB& expand(const AABB& core, FCL_REAL ratio)
230 {
231 min_ = min_ * ratio - core.min_;
232 max_ = max_ * ratio - core.max_;
233 return *this;
234 }
235
236 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
237};
238
240static inline AABB translate(const AABB& aabb, const Vec3f& t)
241{
242 AABB res(aabb);
243 res.min_ += t;
244 res.max_ += t;
245 return res;
246}
247
248static inline AABB rotate(const AABB& aabb, const Matrix3f& R)
249{
250 AABB res (R * aabb.min_);
251 Vec3f corner (aabb.min_);
252 const Eigen::DenseIndex bit[3] = { 1, 2, 4 };
253 for (Eigen::DenseIndex ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it.
254 for (Eigen::DenseIndex i = 0; i < 3; ++i) {
255 corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
256 }
257 res += R * corner;
258 }
259 return res;
260}
261
263HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
264 const AABB& b2);
265
267HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
268 const AABB& b2, const CollisionRequest& request,
269 FCL_REAL& sqrDistLowerBound);
270}
271
272} // namespace hpp
273
274#endif
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:56
data_types.h
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: AABB.h:62
hpp::fcl::AABB::overlap
bool overlap(const AABB &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two AABB are overlap.
hpp::fcl::AABB::AABB
AABB(const Vec3f &v)
Creating an AABB at position v with zero size.
Definition: AABB.h:68
hpp::fcl::AABB::contain
bool contain(const AABB &other) const
Check whether the AABB contains another AABB.
Definition: AABB.h:202
hpp::fcl::AABB::depth
FCL_REAL depth() const
Depth of the AABB.
Definition: AABB.h:188
hpp::fcl::AABB::volume
FCL_REAL volume() const
Volume of the AABB.
Definition: AABB.h:194
hpp::fcl::AABB::update
AABB & update(const Vec3f &a, const Vec3f &b)
Definition: AABB.h:90
hpp::fcl::AABB::distance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
hpp::fcl::AABB::AABB
AABB(const AABB &core, const Vec3f &delta)
Creating an AABB centered as core and is of half-dimension delta.
Definition: AABB.h:79
hpp::fcl::AABB::overlap
bool overlap(const AABB &other, AABB &overlap_part) const
Check whether two AABB are overlap and return the overlap part.
Definition: AABB.h:208
hpp::fcl::AABB::width
FCL_REAL width() const
Width of the AABB.
Definition: AABB.h:176
hpp::fcl::AABB::operator==
bool operator==(const AABB &other) const
Comparison operator.
Definition: AABB.h:97
hpp::fcl::AABB::overlap
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: AABB.h:117
hpp::fcl::AABB::AABB
AABB(const Vec3f &a, const Vec3f &b)
Creating an AABB with two endpoints a and b.
Definition: AABB.h:73
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: AABB.h:60
hpp::fcl::AABB::expand
AABB & expand(const Vec3f &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: AABB.h:221
hpp::fcl::overlap
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.
hpp::fcl::AABB::AABB
AABB()
Creating an AABB with zero size (low bound +inf, upper bound -inf)
hpp::fcl::AABB::AABB
AABB(const Vec3f &a, const Vec3f &b, const Vec3f &c)
Creating an AABB contains three points.
Definition: AABB.h:85
hpp::fcl::AABB::expand
AABB & expand(const AABB &core, FCL_REAL ratio)
expand the aabb by increase the thickness of the plate by a ratio
Definition: AABB.h:229
hpp::fcl::AABB::contain
bool contain(const Vec3f &p) const
Check whether the AABB contains a point.
Definition: AABB.h:107
hpp::fcl::AABB::height
FCL_REAL height() const
Height of the AABB.
Definition: AABB.h:182
hpp::fcl::AABB::distance
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.
hpp::fcl::AABB::center
Vec3f center() const
Center of the AABB.
Definition: AABB.h:170
hpp::fcl::AABB::size
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: AABB.h:164
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:210