AABB-inl.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-2016, 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 FCL_BV_AABB_INL_H
39 #define FCL_BV_AABB_INL_H
40 
41 #include "fcl/math/bv/AABB.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT AABB<double>;
49 
50 //==============================================================================
51 template <typename S>
53  : min_(Vector3<S>::Constant(std::numeric_limits<S>::max())),
54  max_(Vector3<S>::Constant(-std::numeric_limits<S>::max()))
55 {
56  // Do nothing
57 }
58 
59 //==============================================================================
60 template <typename S>
61 AABB<S>::AABB(const Vector3<S>& v) : min_(v), max_(v)
62 {
63  // Do nothing
64 }
65 
66 //==============================================================================
67 template <typename S>
68 AABB<S>::AABB(const Vector3<S>& a, const Vector3<S>& b)
69  : min_(a.cwiseMin(b)),
70  max_(a.cwiseMax(b))
71 {
72  // Do nothing
73 }
74 
75 //==============================================================================
76 template <typename S>
77 AABB<S>::AABB(const AABB<S>& core, const Vector3<S>& delta)
78  : min_(core.min_ - delta),
79  max_(core.max_ + delta)
80 {
81  // Do nothing
82 }
83 
84 //==============================================================================
85 template <typename S>
87  const Vector3<S>& a,
88  const Vector3<S>& b,
89  const Vector3<S>& c)
90  : min_(a.cwiseMin(b).cwiseMin(c)),
91  max_(a.cwiseMax(b).cwiseMax(c))
92 {
93  // Do nothing
94 }
95 
96 //==============================================================================
97 template <typename S>
98 bool AABB<S>::overlap(const AABB<S>& other) const
99 {
100  if ((min_.array() > other.max_.array()).any())
101  return false;
102 
103  if ((max_.array() < other.min_.array()).any())
104  return false;
105 
106  return true;
107 }
108 
109 //==============================================================================
110 template <typename S>
111 bool AABB<S>::contain(const AABB<S>& other) const
112 {
113  if ((min_.array() > other.min_.array()).any())
114  return false;
115 
116  if ((max_.array() < other.max_.array()).any())
117  return false;
118 
119  return true;
120 }
121 
122 //==============================================================================
123 template <typename S>
124 bool AABB<S>::axisOverlap(const AABB<S>& other, int axis_id) const
125 {
126  if(min_[axis_id] > other.max_[axis_id]) return false;
127 
128  if(max_[axis_id] < other.min_[axis_id]) return false;
129 
130  return true;
131 }
132 
133 //==============================================================================
134 template <typename S>
135 bool AABB<S>::overlap(const AABB<S>& other, AABB<S>& overlap_part) const
136 {
137  if(!overlap(other))
138  {
139  return false;
140  }
141 
142  overlap_part.min_ = min_.cwiseMax(other.min_);
143  overlap_part.max_ = max_.cwiseMin(other.max_);
144  return true;
145 }
146 
147 //==============================================================================
148 template <typename S>
149 bool AABB<S>::contain(const Vector3<S>& p) const
150 {
151  if ((min_.array() > p.array()).any())
152  return false;
153 
154  if ((max_.array() < p.array()).any())
155  return false;
156 
157  return true;
158 }
159 
160 //==============================================================================
161 template <typename S>
163 {
164  min_ = min_.cwiseMin(p);
165  max_ = max_.cwiseMax(p);
166  return *this;
167 }
168 
169 //==============================================================================
170 template <typename S>
172 {
173  min_ = min_.cwiseMin(other.min_);
174  max_ = max_.cwiseMax(other.max_);
175  return *this;
176 }
177 
178 //==============================================================================
179 template <typename S>
181 {
182  AABB res(*this);
183  return res += other;
184 }
185 
186 //==============================================================================
187 template <typename S>
189 {
190  return max_[0] - min_[0];
191 }
192 
193 //==============================================================================
194 template <typename S>
196 {
197  return max_[1] - min_[1];
198 }
199 
200 //==============================================================================
201 template <typename S>
203 {
204  return max_[2] - min_[2];
205 }
206 
207 //==============================================================================
208 template <typename S>
210 {
211  return width() * height() * depth();
212 }
213 
214 //==============================================================================
215 template <typename S>
217 {
218  return (max_ - min_).squaredNorm();
219 }
220 
221 //==============================================================================
222 template <typename S>
224 {
225  return (max_ - min_).norm() / 2;
226 }
227 
228 //==============================================================================
229 template <typename S>
231 {
232  return (min_ + max_) * 0.5;
233 }
234 
235 //==============================================================================
236 template <typename S>
237 S AABB<S>::distance(const AABB<S>& other, Vector3<S>* P, Vector3<S>* Q) const
238 {
239  S result = 0;
240  for(std::size_t i = 0; i < 3; ++i)
241  {
242  const S& amin = min_[i];
243  const S& amax = max_[i];
244  const S& bmin = other.min_[i];
245  const S& bmax = other.max_[i];
246 
247  if(amin > bmax)
248  {
249  S delta = bmax - amin;
250  result += delta * delta;
251  if(P && Q)
252  {
253  (*P)[i] = amin;
254  (*Q)[i] = bmax;
255  }
256  }
257  else if(bmin > amax)
258  {
259  S delta = amax - bmin;
260  result += delta * delta;
261  if(P && Q)
262  {
263  (*P)[i] = amax;
264  (*Q)[i] = bmin;
265  }
266  }
267  else
268  {
269  if(P && Q)
270  {
271  if(bmin >= amin)
272  {
273  S t = 0.5 * (amax + bmin);
274  (*P)[i] = t;
275  (*Q)[i] = t;
276  }
277  else
278  {
279  S t = 0.5 * (amin + bmax);
280  (*P)[i] = t;
281  (*Q)[i] = t;
282  }
283  }
284  }
285  }
286 
287  return std::sqrt(result);
288 }
289 
290 //==============================================================================
291 template <typename S>
292 S AABB<S>::distance(const AABB<S>& other) const
293 {
294  S result = 0;
295  for(std::size_t i = 0; i < 3; ++i)
296  {
297  const S& amin = min_[i];
298  const S& amax = max_[i];
299  const S& bmin = other.min_[i];
300  const S& bmax = other.max_[i];
301 
302  if(amin > bmax)
303  {
304  S delta = bmax - amin;
305  result += delta * delta;
306  }
307  else if(bmin > amax)
308  {
309  S delta = amax - bmin;
310  result += delta * delta;
311  }
312  }
313 
314  return std::sqrt(result);
315 }
316 
317 //==============================================================================
318 template <typename S>
319 bool AABB<S>::equal(const AABB<S>& other) const
320 {
321  return min_.isApprox(other.min_, std::numeric_limits<S>::epsilon() * 100)
322  && max_.isApprox(other.max_, std::numeric_limits<S>::epsilon() * 100);
323 }
324 
325 //==============================================================================
326 template <typename S>
328 {
329  min_ -= delta;
330  max_ += delta;
331  return *this;
332 }
333 
334 //==============================================================================
335 template <typename S>
336 AABB<S>& AABB<S>::expand(const AABB<S>& core, S ratio)
337 {
338  min_ = min_ * ratio - core.min_;
339  max_ = max_ * ratio - core.max_;
340  return *this;
341 }
342 
343 //==============================================================================
344 template <typename S, typename Derived>
346  const AABB<S>& aabb, const Eigen::MatrixBase<Derived>& t)
347 {
348  AABB<S> res(aabb);
349  res.min_ += t;
350  res.max_ += t;
351  return res;
352 }
353 
354 } // namespace fcl
355 
356 #endif
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:49
fcl::detail::overlap
bool overlap(S a1, S a2, S b1, S b2)
returns 1 if the intervals overlap, and 0 otherwise
Definition: interval_tree-inl.h:498
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
fcl::AABB< OBBRSS< Shape::S > ::S >::S
OBBRSS< Shape::S > ::S S
Definition: AABB.h:53
std
Definition: gtest-tuple.h:104
fcl::AABB< double >
template class FCL_EXPORT AABB< double >
AABB.h
fcl::AABB::AABB
AABB()
Creating an AABB with zero size (low bound +inf, upper bound -inf)
Definition: AABB-inl.h:52
aabb
SaPCollisionManager< S >::SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:184
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:02