Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
Eigen  3.4.0
 
Loading...
Searching...
No Matches
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
49namespace Eigen {
50
65template <typename _Scalar, int _AmbientDim>
67{
68public:
69EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
70 enum { AmbientDimAtCompileTime = _AmbientDim };
71 typedef _Scalar Scalar;
74 typedef typename ScalarTraits::Real RealScalar;
75 typedef typename ScalarTraits::NonInteger NonInteger;
78
81 {
83 Min=0, Max=1,
97 };
98
99
101 EIGEN_DEVICE_FUNC inline AlignedBox()
102 { if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime!=Dynamic)) setEmpty(); }
103
105 EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim)
106 { setEmpty(); }
107
110 template<typename OtherVectorType1, typename OtherVectorType2>
111 EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {}
112
114 template<typename Derived>
115 EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min)
116 { }
117
118 EIGEN_DEVICE_FUNC ~AlignedBox() {}
119
121 EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); }
122
124 EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); }
125
127 EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); }
128
131 EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); }
132
135 EIGEN_DEVICE_FUNC inline void setEmpty()
136 {
137 m_min.setConstant( ScalarTraits::highest() );
138 m_max.setConstant( ScalarTraits::lowest() );
139 }
140
142 EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; }
144 EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; }
146 EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; }
148 EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; }
149
151 EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient)
152 center() const
153 { return (m_min+m_max)/RealScalar(2); }
154
160 { return m_max - m_min; }
161
163 EIGEN_DEVICE_FUNC inline Scalar volume() const
164 { return sizes().prod(); }
165
171 { return sizes(); }
172
182 EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const
183 {
184 EIGEN_STATIC_ASSERT(_AmbientDim <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE);
185
186 VectorType res;
187
188 Index mult = 1;
189 for(Index d=0; d<dim(); ++d)
190 {
191 if( mult & corner ) res[d] = m_max[d];
192 else res[d] = m_min[d];
193 mult *= 2;
194 }
195 return res;
196 }
197
200 EIGEN_DEVICE_FUNC inline VectorType sample() const
201 {
202 VectorType r(dim());
203 for(Index d=0; d<dim(); ++d)
204 {
205 if(!ScalarTraits::IsInteger)
206 {
207 r[d] = m_min[d] + (m_max[d]-m_min[d])
208 * internal::random<Scalar>(Scalar(0), Scalar(1));
209 }
210 else
211 r[d] = internal::random(m_min[d], m_max[d]);
212 }
213 return r;
214 }
215
217 template<typename Derived>
218 EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const
219 {
220 typename internal::nested_eval<Derived,2>::type p_n(p.derived());
221 return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all();
222 }
223
225 EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const
226 { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); }
227
230 EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const
231 { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); }
232
235 template<typename Derived>
236 EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p)
237 {
238 typename internal::nested_eval<Derived,2>::type p_n(p.derived());
239 m_min = m_min.cwiseMin(p_n);
240 m_max = m_max.cwiseMax(p_n);
241 return *this;
242 }
243
246 EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b)
247 {
248 m_min = m_min.cwiseMin(b.m_min);
249 m_max = m_max.cwiseMax(b.m_max);
250 return *this;
251 }
252
256 EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b)
257 {
258 m_min = m_min.cwiseMax(b.m_min);
259 m_max = m_max.cwiseMin(b.m_max);
260 return *this;
261 }
262
266 EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const
267 {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); }
268
272 EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const
273 { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); }
274
276 template<typename Derived>
277 EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t)
278 {
279 const typename internal::nested_eval<Derived,2>::type t(a_t.derived());
280 m_min += t;
281 m_max += t;
282 return *this;
283 }
284
286 template<typename Derived>
287 EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const
288 {
289 AlignedBox result(m_min, m_max);
290 result.translate(a_t);
291 return result;
292 }
293
298 template<typename Derived>
299 EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
300
305 EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
306
311 template<typename Derived>
312 EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const
313 { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); }
314
319 EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const
320 { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); }
321
325 template<int Mode, int Options>
326 EIGEN_DEVICE_FUNC inline void transform(
328 {
329 this->translate(translation);
330 }
331
338 template<int Mode, int Options>
340 {
341 // Only Affine and Isometry transforms are currently supported.
342 EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
343
344 // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
345 // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
346 //
347 // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
348
349 // two times rotated extent
350 const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
351 // two times new center
352 const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) +
353 Scalar(2) * transform.translation();
354
355 this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
356 this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
357 }
358
363 template<int Mode, int Options>
365 {
366 AlignedBox result(m_min, m_max);
367 result.transform(transform);
368 return result;
369 }
370
376 template<typename NewScalarType>
377 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox,
379 {
380 return typename internal::cast_return_type<AlignedBox,
382 }
383
385 template<typename OtherScalarType>
386 EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
387 {
388 m_min = (other.min)().template cast<Scalar>();
389 m_max = (other.max)().template cast<Scalar>();
390 }
391
396 EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const
397 { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
398
399protected:
400
401 VectorType m_min, m_max;
402};
403
404
405
406template<typename Scalar,int AmbientDim>
407template<typename Derived>
409{
410 typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived());
411 Scalar dist2(0);
412 Scalar aux;
413 for (Index k=0; k<dim(); ++k)
414 {
415 if( m_min[k] > p[k] )
416 {
417 aux = m_min[k] - p[k];
418 dist2 += aux*aux;
419 }
420 else if( p[k] > m_max[k] )
421 {
422 aux = p[k] - m_max[k];
423 dist2 += aux*aux;
424 }
425 }
426 return dist2;
427}
428
429template<typename Scalar,int AmbientDim>
430EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const
431{
432 Scalar dist2(0);
433 Scalar aux;
434 for (Index k=0; k<dim(); ++k)
435 {
436 if( m_min[k] > b.m_max[k] )
437 {
438 aux = m_min[k] - b.m_max[k];
439 dist2 += aux*aux;
440 }
441 else if( b.m_min[k] > m_max[k] )
442 {
443 aux = b.m_min[k] - m_max[k];
444 dist2 += aux*aux;
445 }
446 }
447 return dist2;
448}
449
466#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
467 \
468typedef AlignedBox<Type, Size> AlignedBox##SizeSuffix##TypeSuffix;
469
470#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
471EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
472EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
473EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
474EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
475EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
476
477EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
478EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
479EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
480
481#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
482#undef EIGEN_MAKE_TYPEDEFS
483
484} // end namespace Eigen
485
486#endif // EIGEN_ALIGNEDBOX_H
An axis aligned box.
Definition: AlignedBox.h:67
bool contains(const AlignedBox &b) const
Definition: AlignedBox.h:225
AlignedBox(const AlignedBox< OtherScalarType, AmbientDimAtCompileTime > &other)
Definition: AlignedBox.h:386
VectorType corner(CornerType corner) const
Definition: AlignedBox.h:182
AlignedBox & extend(const MatrixBase< Derived > &p)
Definition: AlignedBox.h:236
bool intersects(const AlignedBox &b) const
Definition: AlignedBox.h:230
VectorType &() max()
Definition: AlignedBox.h:148
const CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > sizes() const
Definition: AlignedBox.h:159
void setEmpty()
Definition: AlignedBox.h:135
void setNull()
Definition: AlignedBox.h:127
VectorType &() min()
Definition: AlignedBox.h:144
bool isEmpty() const
Definition: AlignedBox.h:131
bool isNull() const
Definition: AlignedBox.h:124
NonInteger exteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:312
internal::cast_return_type< AlignedBox, AlignedBox< NewScalarType, AmbientDimAtCompileTime > >::type cast() const
Definition: AlignedBox.h:378
AlignedBox translated(const MatrixBase< Derived > &a_t) const
Definition: AlignedBox.h:287
AlignedBox()
Definition: AlignedBox.h:101
Index dim() const
Definition: AlignedBox.h:121
AlignedBox merged(const AlignedBox &b) const
Definition: AlignedBox.h:272
Eigen::Index Index
Definition: AlignedBox.h:73
AlignedBox(const MatrixBase< Derived > &p)
Definition: AlignedBox.h:115
const VectorType &() max() const
Definition: AlignedBox.h:146
AlignedBox & clamp(const AlignedBox &b)
Definition: AlignedBox.h:256
Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:408
CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > diagonal() const
Definition: AlignedBox.h:170
bool contains(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:218
AlignedBox & translate(const MatrixBase< Derived > &a_t)
Definition: AlignedBox.h:277
void transform(const typename Transform< Scalar, AmbientDimAtCompileTime, Mode, Options >::TranslationType &translation)
Definition: AlignedBox.h:326
void transform(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform)
Definition: AlignedBox.h:339
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient) center() const
Definition: AlignedBox.h:151
AlignedBox(const OtherVectorType1 &_min, const OtherVectorType2 &_max)
Definition: AlignedBox.h:111
AlignedBox transformed(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform) const
Definition: AlignedBox.h:364
AlignedBox intersection(const AlignedBox &b) const
Definition: AlignedBox.h:266
bool isApprox(const AlignedBox &other, const RealScalar &prec=ScalarTraits::dummy_precision()) const
Definition: AlignedBox.h:396
AlignedBox & extend(const AlignedBox &b)
Definition: AlignedBox.h:246
const VectorType &() min() const
Definition: AlignedBox.h:142
AlignedBox(Index _dim)
Definition: AlignedBox.h:105
VectorType sample() const
Definition: AlignedBox.h:200
Scalar volume() const
Definition: AlignedBox.h:163
CornerType
Definition: AlignedBox.h:81
@ TopLeftFloor
Definition: AlignedBox.h:93
@ BottomLeft
Definition: AlignedBox.h:87
@ BottomRightFloor
Definition: AlignedBox.h:92
@ Min
Definition: AlignedBox.h:83
@ BottomLeftCeil
Definition: AlignedBox.h:94
@ BottomLeftFloor
Definition: AlignedBox.h:92
@ TopLeft
Definition: AlignedBox.h:88
@ Max
Definition: AlignedBox.h:83
@ TopRightCeil
Definition: AlignedBox.h:95
@ TopRightFloor
Definition: AlignedBox.h:93
@ BottomRight
Definition: AlignedBox.h:87
@ BottomRightCeil
Definition: AlignedBox.h:94
@ TopLeftCeil
Definition: AlignedBox.h:95
@ TopRight
Definition: AlignedBox.h:88
NonInteger exteriorDistance(const AlignedBox &b) const
Definition: AlignedBox.h:319
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition: CwiseBinaryOp.h:84
Derived & derived()
Definition: EigenBase.h:46
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:50
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:180
Derived & setConstant(Index size, const Scalar &val)
Definition: CwiseNullaryOp.h:361
Represents an homogeneous transformation in a N dimensional space.
Definition: Transform.h:205
Represents a translation transformation.
Definition: Translation.h:31
static const Eigen::internal::all_t all
Definition: IndexedViewHelper.h:171
@ Affine
Definition: Constants.h:460
@ AffineCompact
Definition: Constants.h:462
@ Isometry
Definition: Constants.h:457
Namespace containing all symbols from the Eigen library.
Definition: Core:141
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
const int Dynamic
Definition: Constants.h:22
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:233