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