Eigen  5.0.1-dev+master
 
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// SPDX-License-Identifier: MPL-2.0
10
11// Function void Eigen::AlignedBox::transform(const Transform& transform)
12// is provided under the following license agreement:
13//
14// Software License Agreement (BSD License)
15//
16// Copyright (c) 2011-2014, Willow Garage, Inc.
17// Copyright (c) 2014-2015, Open Source Robotics Foundation
18// All rights reserved.
19//
20// Redistribution and use in source and binary forms, with or without
21// modification, are permitted provided that the following conditions
22// are met:
23//
24// * Redistributions of source code must retain the above copyright
25// notice, this list of conditions and the following disclaimer.
26// * Redistributions in binary form must reproduce the above
27// copyright notice, this list of conditions and the following
28// disclaimer in the documentation and/or other materials provided
29// with the distribution.
30// * Neither the name of Open Source Robotics Foundation nor the names of its
31// contributors may be used to endorse or promote products derived
32// from this software without specific prior written permission.
33//
34// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
37// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
38// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
39// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
40// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
41// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
42// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45// POSSIBILITY OF SUCH DAMAGE.
46
47#ifndef EIGEN_ALIGNEDBOX_H
48#define EIGEN_ALIGNEDBOX_H
49
50// IWYU pragma: private
51#include "./InternalHeaderCheck.h"
52
53namespace Eigen {
54
69template <typename Scalar_, int AmbientDim_>
71 public:
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, AmbientDim_)
73 enum { AmbientDimAtCompileTime = AmbientDim_ };
74 using Scalar = Scalar_;
75 using ScalarTraits = NumTraits<Scalar>;
76 using Index = Eigen::Index;
77 using RealScalar = typename ScalarTraits::Real;
78 using NonInteger = typename ScalarTraits::NonInteger;
80 using VectorTypeSum = CwiseBinaryOp<internal::scalar_sum_op<Scalar>, const VectorType, const VectorType>;
81
107
109 EIGEN_DEVICE_FUNC inline AlignedBox() {
110 if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime != Dynamic)) setEmpty();
111 }
112
114 EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); }
115
119 template <typename OtherVectorType1, typename OtherVectorType2>
120 EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max)
121 : m_min(_min), m_max(_max) {}
122
124 template <typename Derived>
125 EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min) {}
126
128 EIGEN_DEVICE_FUNC inline Index dim() const {
129 return AmbientDimAtCompileTime == Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime);
130 }
131
133 EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); }
134
136 EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); }
137
140 EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); }
141
144 EIGEN_DEVICE_FUNC inline void setEmpty() {
145 m_min.setConstant(ScalarTraits::highest());
146 m_max.setConstant(ScalarTraits::lowest());
147 }
148
150 EIGEN_DEVICE_FUNC inline const VectorType&(min)() const { return m_min; }
152 EIGEN_DEVICE_FUNC inline VectorType&(min)() { return m_min; }
154 EIGEN_DEVICE_FUNC inline const VectorType&(max)() const { return m_max; }
156 EIGEN_DEVICE_FUNC inline VectorType&(max)() { return m_max; }
157
159 EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar,
160 internal::scalar_quotient_op) center() const {
161 return (m_min + m_max) / RealScalar(2);
162 }
163
168 EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_difference_op<Scalar, Scalar>, const VectorType,
169 const VectorType>
170 sizes() const {
171 return m_max - m_min;
172 }
173
175 EIGEN_DEVICE_FUNC inline Scalar volume() const { return isEmpty() ? Scalar(0) : sizes().prod(); }
176
181 EIGEN_DEVICE_FUNC inline CwiseBinaryOp<internal::scalar_difference_op<Scalar, Scalar>, const VectorType,
182 const VectorType>
183 diagonal() const {
184 return sizes();
185 }
186
196 EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const {
197 EIGEN_STATIC_ASSERT(AmbientDim_ <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE);
198
199 VectorType res(dim());
200
201 Index mult = 1;
202 for (Index d = 0; d < dim(); ++d) {
203 if (mult & corner)
204 res[d] = m_max[d];
205 else
206 res[d] = m_min[d];
207 mult *= 2;
208 }
209 return res;
210 }
211
214 EIGEN_DEVICE_FUNC inline VectorType sample() const {
215 VectorType r(dim());
216 for (Index d = 0; d < dim(); ++d) {
217 EIGEN_IF_CONSTEXPR (!ScalarTraits::IsInteger) {
218 r[d] = m_min[d] + (m_max[d] - m_min[d]) * internal::random<Scalar>(Scalar(0), Scalar(1));
219 } else
220 r[d] = internal::random(m_min[d], m_max[d]);
221 }
222 return r;
223 }
224
226 template <typename Derived>
227 EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const {
228 typename internal::nested_eval<Derived, 2>::type p_n(p.derived());
229 return (m_min.array() <= p_n.array()).all() && (p_n.array() <= m_max.array()).all();
230 }
231
233 EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const {
234 return (m_min.array() <= (b.min)().array()).all() && ((b.max)().array() <= m_max.array()).all();
235 }
236
239 EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const {
240 return (m_min.array() <= (b.max)().array()).all() && ((b.min)().array() <= m_max.array()).all();
241 }
242
245 template <typename Derived>
246 EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p) {
247 typename internal::nested_eval<Derived, 2>::type p_n(p.derived());
248 m_min = m_min.cwiseMin(p_n);
249 m_max = m_max.cwiseMax(p_n);
250 return *this;
251 }
252
255 EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b) {
256 m_min = m_min.cwiseMin(b.m_min);
257 m_max = m_max.cwiseMax(b.m_max);
258 return *this;
259 }
260
264 EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b) {
265 m_min = m_min.cwiseMax(b.m_min);
266 m_max = m_max.cwiseMin(b.m_max);
267 return *this;
268 }
269
273 EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const {
274 return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max));
275 }
276
280 EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const {
281 return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max));
282 }
283
285 template <typename Derived>
286 EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t) {
287 const typename internal::nested_eval<Derived, 2>::type t(a_t.derived());
288 m_min += t;
289 m_max += t;
290 return *this;
291 }
292
294 template <typename Derived>
295 EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const {
296 AlignedBox result(m_min, m_max);
297 result.translate(a_t);
298 return result;
299 }
300
305 template <typename Derived>
306 EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
307
312 EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
313
318 template <typename Derived>
319 EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const {
320 return numext::sqrt(NonInteger(squaredExteriorDistance(p)));
321 }
322
327 EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const {
328 return numext::sqrt(NonInteger(squaredExteriorDistance(b)));
329 }
330
334 EIGEN_DEVICE_FUNC inline void transform(const Translation<Scalar, AmbientDimAtCompileTime>& translation) {
335 this->translate(translation.vector());
336 }
337
344 template <int Mode, int Options>
346 // Only Affine and Isometry transforms are currently supported.
347 EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry,
348 THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
349
350 // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
351 // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
352 //
353 // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
354
355 // two times rotated extent
356 const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
357 // two times new center
358 const VectorType rotated_center_2 =
359 transform.linear() * (this->m_max + this->m_min) + Scalar(2) * transform.translation();
360
361 this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
362 this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
363 }
364
369 template <int Mode, int Options>
370 EIGEN_DEVICE_FUNC AlignedBox
372 AlignedBox result(m_min, m_max);
373 result.transform(transform);
374 return result;
375 }
376
382 template <typename NewScalarType>
383 EIGEN_DEVICE_FUNC inline
384 typename internal::cast_return_type<AlignedBox, AlignedBox<NewScalarType, AmbientDimAtCompileTime> >::type
385 cast() const {
386 return typename internal::cast_return_type<AlignedBox, AlignedBox<NewScalarType, AmbientDimAtCompileTime> >::type(
387 *this);
388 }
389
391 template <typename OtherScalarType>
392 EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType, AmbientDimAtCompileTime>& other) {
393 m_min = (other.min)().template cast<Scalar>();
394 m_max = (other.max)().template cast<Scalar>();
395 }
396
401 EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other,
402 const RealScalar& prec = ScalarTraits::dummy_precision()) const {
403 return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec);
404 }
405
406 protected:
407 VectorType m_min, m_max;
408};
409
410template <typename Scalar, int AmbientDim>
411template <typename Derived>
413 const MatrixBase<Derived>& a_p) const {
414 typename internal::nested_eval<Derived, 2 * AmbientDim>::type p(a_p.derived());
415 Scalar dist2(0);
416 Scalar aux;
417 for (Index k = 0; k < dim(); ++k) {
418 if (m_min[k] > p[k]) {
419 aux = m_min[k] - p[k];
420 dist2 += aux * aux;
421 } else if (p[k] > m_max[k]) {
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 Scalar dist2(0);
432 Scalar aux;
433 for (Index k = 0; k < dim(); ++k) {
434 if (m_min[k] > b.m_max[k]) {
435 aux = m_min[k] - b.m_max[k];
436 dist2 += aux * aux;
437 } else if (b.m_min[k] > m_max[k]) {
438 aux = b.m_min[k] - m_max[k];
439 dist2 += aux * aux;
440 }
441 }
442 return dist2;
443}
444
461
462#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
463 \
464 using AlignedBox##SizeSuffix##TypeSuffix = AlignedBox<Type, Size>;
465
466#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
467 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
468 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
469 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
470 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
471 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
472
473EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
474EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
475EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
476
477#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
478#undef EIGEN_MAKE_TYPEDEFS
479
480} // end namespace Eigen
481
482#endif // EIGEN_ALIGNEDBOX_H
An axis aligned box.
Definition AlignedBox.h:70
bool isApprox(const AlignedBox &other, const RealScalar &prec=ScalarTraits::dummy_precision()) const
Definition AlignedBox.h:401
AlignedBox intersection(const AlignedBox &b) const
Definition AlignedBox.h:273
const VectorType & max() const
Definition AlignedBox.h:154
AlignedBox(const OtherVectorType1 &_min, const OtherVectorType2 &_max)
Definition AlignedBox.h:120
void transform(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform)
Definition AlignedBox.h:345
bool isNull() const
Definition AlignedBox.h:133
VectorType sample() const
Definition AlignedBox.h:214
AlignedBox(const MatrixBase< Derived > &p)
Definition AlignedBox.h:125
bool contains(const AlignedBox &b) const
Definition AlignedBox.h:233
AlignedBox transformed(const Transform< Scalar, AmbientDimAtCompileTime, Mode, Options > &transform) const
Definition AlignedBox.h:371
bool intersects(const AlignedBox &b) const
Definition AlignedBox.h:239
AlignedBox merged(const AlignedBox &b) const
Definition AlignedBox.h:280
VectorType corner(CornerType corner) const
Definition AlignedBox.h:196
AlignedBox(Index _dim)
Definition AlignedBox.h:114
internal::cast_return_type< AlignedBox, AlignedBox< NewScalarType, AmbientDimAtCompileTime > >::type cast() const
Definition AlignedBox.h:385
AlignedBox & extend(const MatrixBase< Derived > &p)
Definition AlignedBox.h:246
AlignedBox translated(const MatrixBase< Derived > &a_t) const
Definition AlignedBox.h:295
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, internal::scalar_quotient_op) center() const
Definition AlignedBox.h:159
bool contains(const MatrixBase< Derived > &p) const
Definition AlignedBox.h:227
Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition AlignedBox.h:412
Eigen::Index Index
Definition AlignedBox.h:76
void transform(const Translation< Scalar, AmbientDimAtCompileTime > &translation)
Definition AlignedBox.h:334
Index dim() const
Definition AlignedBox.h:128
CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > diagonal() const
Definition AlignedBox.h:183
AlignedBox()
Definition AlignedBox.h:109
AlignedBox & clamp(const AlignedBox &b)
Definition AlignedBox.h:264
bool isEmpty() const
Definition AlignedBox.h:140
Scalar squaredExteriorDistance(const AlignedBox &b) const
Definition AlignedBox.h:430
NonInteger exteriorDistance(const AlignedBox &b) const
Definition AlignedBox.h:327
const CwiseBinaryOp< internal::scalar_difference_op< Scalar, Scalar >, const VectorType, const VectorType > sizes() const
Definition AlignedBox.h:170
NonInteger exteriorDistance(const MatrixBase< Derived > &p) const
Definition AlignedBox.h:319
CornerType
Definition AlignedBox.h:83
@ Max
Definition AlignedBox.h:86
@ TopLeft
Definition AlignedBox.h:92
@ TopRight
Definition AlignedBox.h:93
@ TopLeftFloor
Definition AlignedBox.h:99
@ BottomRight
Definition AlignedBox.h:91
@ Min
Definition AlignedBox.h:85
@ TopLeftCeil
Definition AlignedBox.h:103
@ BottomLeft
Definition AlignedBox.h:90
@ BottomRightFloor
Definition AlignedBox.h:98
@ TopRightFloor
Definition AlignedBox.h:100
@ BottomLeftCeil
Definition AlignedBox.h:101
@ BottomRightCeil
Definition AlignedBox.h:102
@ TopRightCeil
Definition AlignedBox.h:104
@ BottomLeftFloor
Definition AlignedBox.h:97
AlignedBox(const AlignedBox< OtherScalarType, AmbientDimAtCompileTime > &other)
Definition AlignedBox.h:392
Scalar volume() const
Definition AlignedBox.h:175
void setEmpty()
Definition AlignedBox.h:144
void setNull()
Definition AlignedBox.h:136
AlignedBox & extend(const AlignedBox &b)
Definition AlignedBox.h:255
AlignedBox & translate(const MatrixBase< Derived > &a_t)
Definition AlignedBox.h:286
const VectorType & min() const
Definition AlignedBox.h:150
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition CwiseBinaryOp.h:91
Base class for all dense matrices, vectors, and expressions.
Definition MatrixBase.h:53
The matrix class, also used for vectors and row-vectors.
Definition Matrix.h:188
Represents an homogeneous transformation in a N dimensional space.
Definition Transform.h:222
Represents a translation transformation.
Definition Translation.h:34
@ Affine
Definition Constants.h:475
@ AffineCompact
Definition Constants.h:477
@ Isometry
Definition Constants.h:472
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition NumTraits.h:267