Eigen  3.4.90 (git rev 67eeba6e720c5745abc77ae6c92ce0a44aa7b7ae)
AlignedBox.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 // Function void Eigen::AlignedBox::transform(const Transform& transform)
11 // is provided under the following license agreement:
12 //
13 // Software License Agreement (BSD License)
14 //
15 // Copyright (c) 2011-2014, Willow Garage, Inc.
16 // Copyright (c) 2014-2015, Open Source Robotics Foundation
17 // All rights reserved.
18 //
19 // Redistribution and use in source and binary forms, with or without
20 // modification, are permitted provided that the following conditions
21 // are met:
22 //
23 // * Redistributions of source code must retain the above copyright
24 // notice, this list of conditions and the following disclaimer.
25 // * Redistributions in binary form must reproduce the above
26 // copyright notice, this list of conditions and the following
27 // disclaimer in the documentation and/or other materials provided
28 // with the distribution.
29 // * Neither the name of Open Source Robotics Foundation nor the names of its
30 // contributors may be used to endorse or promote products derived
31 // from this software without specific prior written permission.
32 //
33 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
34 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
35 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
36 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
37 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
38 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
39 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
40 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
41 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
43 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
44 // POSSIBILITY OF SUCH DAMAGE.
45 
46 #ifndef EIGEN_ALIGNEDBOX_H
47 #define EIGEN_ALIGNEDBOX_H
48 
49 #include "./InternalHeaderCheck.h"
50 
51 namespace Eigen {
52 
67 template <typename Scalar_, int AmbientDim_>
69 {
70 public:
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
72  enum { AmbientDimAtCompileTime = AmbientDim_ };
73  typedef Scalar_ Scalar;
75  typedef Eigen::Index Index;
76  typedef typename ScalarTraits::Real RealScalar;
77  typedef typename ScalarTraits::NonInteger NonInteger;
80 
83  {
85  Min=0, Max=1,
99  };
100 
101 
103  EIGEN_DEVICE_FUNC inline AlignedBox()
104  { if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime!=Dynamic)) setEmpty(); }
105 
107  EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim)
108  { setEmpty(); }
109 
112  template<typename OtherVectorType1, typename OtherVectorType2>
113  EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {}
114 
116  template<typename Derived>
117  EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min)
118  { }
119 
120  EIGEN_DEVICE_FUNC ~AlignedBox() {}
121 
123  EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); }
124 
126  EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); }
127 
129  EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); }
130 
133  EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); }
134 
137  EIGEN_DEVICE_FUNC inline void setEmpty()
138  {
139  m_min.setConstant( ScalarTraits::highest() );
140  m_max.setConstant( ScalarTraits::lowest() );
141  }
142 
144  EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; }
146  EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; }
148  EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; }
150  EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; }
151 
153  EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient)
154  center() const
155  { return (m_min+m_max)/RealScalar(2); }
156 
161  EIGEN_DEVICE_FUNC inline const CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> sizes() const
162  { return m_max - m_min; }
163 
165  EIGEN_DEVICE_FUNC inline Scalar volume() const
166  { return sizes().prod(); }
167 
173  { return sizes(); }
174 
184  EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const
185  {
186  EIGEN_STATIC_ASSERT(AmbientDim_ <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE);
187 
188  VectorType res;
189 
190  Index mult = 1;
191  for(Index d=0; d<dim(); ++d)
192  {
193  if( mult & corner ) res[d] = m_max[d];
194  else res[d] = m_min[d];
195  mult *= 2;
196  }
197  return res;
198  }
199 
202  EIGEN_DEVICE_FUNC inline VectorType sample() const
203  {
204  VectorType r(dim());
205  for(Index d=0; d<dim(); ++d)
206  {
207  if(!ScalarTraits::IsInteger)
208  {
209  r[d] = m_min[d] + (m_max[d]-m_min[d])
210  * internal::random<Scalar>(Scalar(0), Scalar(1));
211  }
212  else
213  r[d] = internal::random(m_min[d], m_max[d]);
214  }
215  return r;
216  }
217 
219  template<typename Derived>
220  EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const
221  {
222  typename internal::nested_eval<Derived,2>::type p_n(p.derived());
223  return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all();
224  }
225 
227  EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const
228  { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); }
229 
232  EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const
233  { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); }
234 
237  template<typename Derived>
238  EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p)
239  {
240  typename internal::nested_eval<Derived,2>::type p_n(p.derived());
241  m_min = m_min.cwiseMin(p_n);
242  m_max = m_max.cwiseMax(p_n);
243  return *this;
244  }
245 
248  EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b)
249  {
250  m_min = m_min.cwiseMin(b.m_min);
251  m_max = m_max.cwiseMax(b.m_max);
252  return *this;
253  }
254 
258  EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b)
259  {
260  m_min = m_min.cwiseMax(b.m_min);
261  m_max = m_max.cwiseMin(b.m_max);
262  return *this;
263  }
264 
268  EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const
269  {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); }
270 
274  EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const
275  { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); }
276 
278  template<typename Derived>
279  EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t)
280  {
281  const typename internal::nested_eval<Derived,2>::type t(a_t.derived());
282  m_min += t;
283  m_max += t;
284  return *this;
285  }
286 
288  template<typename Derived>
289  EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const
290  {
291  AlignedBox result(m_min, m_max);
292  result.translate(a_t);
293  return result;
294  }
295 
300  template<typename Derived>
301  EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
302 
307  EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
308 
313  template<typename Derived>
314  EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const
315  { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); }
316 
321  EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const
322  { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); }
323 
327  template<int Mode, int Options>
328  EIGEN_DEVICE_FUNC inline void transform(
330  {
331  this->translate(translation);
332  }
333 
340  template<int Mode, int Options>
342  {
343  // Only Affine and Isometry transforms are currently supported.
344  EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
345 
346  // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
347  // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
348  //
349  // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
350 
351  // two times rotated extent
352  const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
353  // two times new center
354  const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) +
355  Scalar(2) * transform.translation();
356 
357  this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
358  this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
359  }
360 
365  template<int Mode, int Options>
367  {
368  AlignedBox result(m_min, m_max);
369  result.transform(transform);
370  return result;
371  }
372 
378  template<typename NewScalarType>
379  EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox,
381  {
382  return typename internal::cast_return_type<AlignedBox,
384  }
385 
387  template<typename OtherScalarType>
388  EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
389  {
390  m_min = (other.min)().template cast<Scalar>();
391  m_max = (other.max)().template cast<Scalar>();
392  }
393 
398  EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const
399  { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
400 
401 protected:
402 
403  VectorType m_min, m_max;
404 };
405 
406 
407 
408 template<typename Scalar,int AmbientDim>
409 template<typename Derived>
410 EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const
411 {
412  typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived());
413  Scalar dist2(0);
414  Scalar aux;
415  for (Index k=0; k<dim(); ++k)
416  {
417  if( m_min[k] > p[k] )
418  {
419  aux = m_min[k] - p[k];
420  dist2 += aux*aux;
421  }
422  else if( p[k] > m_max[k] )
423  {
424  aux = p[k] - m_max[k];
425  dist2 += aux*aux;
426  }
427  }
428  return dist2;
429 }
430 
431 template<typename Scalar,int AmbientDim>
432 EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const
433 {
434  Scalar dist2(0);
435  Scalar aux;
436  for (Index k=0; k<dim(); ++k)
437  {
438  if( m_min[k] > b.m_max[k] )
439  {
440  aux = m_min[k] - b.m_max[k];
441  dist2 += aux*aux;
442  }
443  else if( b.m_min[k] > m_max[k] )
444  {
445  aux = b.m_min[k] - m_max[k];
446  dist2 += aux*aux;
447  }
448  }
449  return dist2;
450 }
451 
468 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
469  \
470 typedef AlignedBox<Type, Size> AlignedBox##SizeSuffix##TypeSuffix;
471 
472 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
473 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
474 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
475 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
476 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
477 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
478 
479 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
480 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
481 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
482 
483 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
484 #undef EIGEN_MAKE_TYPEDEFS
485 
486 } // end namespace Eigen
487 
488 #endif // EIGEN_ALIGNEDBOX_H
An axis aligned box.
Definition: AlignedBox.h:69
bool isApprox(const AlignedBox &other, const RealScalar &prec=ScalarTraits::dummy_precision()) const
Definition: AlignedBox.h:398
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient) center() const
Definition: AlignedBox.h:153
AlignedBox intersection(const AlignedBox &b) const
Definition: AlignedBox.h:268
AlignedBox(const OtherVectorType1 &_min, const OtherVectorType2 &_max)
Definition: AlignedBox.h:113
void transform(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform)
Definition: AlignedBox.h:341
bool isNull() const
Definition: AlignedBox.h:126
VectorType sample() const
Definition: AlignedBox.h:202
const VectorType &() max() const
Definition: AlignedBox.h:148
AlignedBox(const MatrixBase< Derived > &p)
Definition: AlignedBox.h:117
AlignedBox & extend(const MatrixBase< Derived > &p)
Definition: AlignedBox.h:238
bool contains(const AlignedBox &b) const
Definition: AlignedBox.h:227
AlignedBox transformed(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform) const
Definition: AlignedBox.h:366
bool intersects(const AlignedBox &b) const
Definition: AlignedBox.h:232
AlignedBox merged(const AlignedBox &b) const
Definition: AlignedBox.h:274
VectorType corner(CornerType corner) const
Definition: AlignedBox.h:184
Eigen::Index Index
Definition: AlignedBox.h:75
AlignedBox(Index _dim)
Definition: AlignedBox.h:107
AlignedBox translated(const MatrixBase< Derived > &a_t) const
Definition: AlignedBox.h:289
bool contains(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:220
AlignedBox & clamp(const AlignedBox &b)
Definition: AlignedBox.h:258
Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:410
VectorType &() max()
Definition: AlignedBox.h:150
VectorType &() min()
Definition: AlignedBox.h:146
Index dim() const
Definition: AlignedBox.h:123
AlignedBox & extend(const AlignedBox &b)
Definition: AlignedBox.h:248
AlignedBox()
Definition: AlignedBox.h:103
internal::cast_return_type< AlignedBox, AlignedBox< NewScalarType, AmbientDimAtCompileTime > >::type cast() const
Definition: AlignedBox.h:380
bool isEmpty() const
Definition: AlignedBox.h:133
NonInteger exteriorDistance(const AlignedBox &b) const
Definition: AlignedBox.h:321
void transform(const typename Transform< Scalar, AmbientDimAtCompileTime, Mode, Options >::TranslationType &translation)
Definition: AlignedBox.h:328
const VectorType &() min() const
Definition: AlignedBox.h:144
const CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > sizes() const
Definition: AlignedBox.h:161
NonInteger exteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:314
CornerType
Definition: AlignedBox.h:83
@ Max
Definition: AlignedBox.h:85
@ TopLeft
Definition: AlignedBox.h:90
@ TopRight
Definition: AlignedBox.h:90
@ TopLeftFloor
Definition: AlignedBox.h:95
@ BottomRight
Definition: AlignedBox.h:89
@ Min
Definition: AlignedBox.h:85
@ TopLeftCeil
Definition: AlignedBox.h:97
@ BottomLeft
Definition: AlignedBox.h:89
@ BottomRightFloor
Definition: AlignedBox.h:94
@ TopRightFloor
Definition: AlignedBox.h:95
@ BottomLeftCeil
Definition: AlignedBox.h:96
@ BottomRightCeil
Definition: AlignedBox.h:96
@ TopRightCeil
Definition: AlignedBox.h:97
@ BottomLeftFloor
Definition: AlignedBox.h:94
AlignedBox(const AlignedBox< OtherScalarType, AmbientDimAtCompileTime > &other)
Definition: AlignedBox.h:388
Scalar volume() const
Definition: AlignedBox.h:165
AlignedBox & translate(const MatrixBase< Derived > &a_t)
Definition: AlignedBox.h:279
void setEmpty()
Definition: AlignedBox.h:137
void setNull()
Definition: AlignedBox.h:129
CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > diagonal() const
Definition: AlignedBox.h:172
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition: CwiseBinaryOp.h:86
Derived & derived()
Definition: EigenBase.h:48
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:52
Derived & setConstant(Index size, const Scalar &val)
Definition: CwiseNullaryOp.h:363
Represents an homogeneous transformation in a N dimensional space.
Definition: Transform.h:207
Represents a translation transformation.
Definition: Translation.h:33
static const Eigen::internal::all_t all
Definition: IndexedViewHelper.h:189
@ Affine
Definition: Constants.h:462
@ AffineCompact
Definition: Constants.h:464
@ Isometry
Definition: Constants.h:459
Namespace containing all symbols from the Eigen library.
Definition: Core:139
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:59
const int Dynamic
Definition: Constants.h:24
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)