xref: /aosp_15_r20/external/eigen/Eigen/src/Geometry/AlignedBox.h (revision bf2c37156dfe67e5dfebd6d394bad8b2ab5804d4)
1*bf2c3715SXin Li // This file is part of Eigen, a lightweight C++ template library
2*bf2c3715SXin Li // for linear algebra.
3*bf2c3715SXin Li //
4*bf2c3715SXin Li // Copyright (C) 2008 Gael Guennebaud <[email protected]>
5*bf2c3715SXin Li //
6*bf2c3715SXin Li // This Source Code Form is subject to the terms of the Mozilla
7*bf2c3715SXin Li // Public License v. 2.0. If a copy of the MPL was not distributed
8*bf2c3715SXin Li // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9*bf2c3715SXin Li 
10*bf2c3715SXin Li // Function void Eigen::AlignedBox::transform(const Transform& transform)
11*bf2c3715SXin Li // is provided under the following license agreement:
12*bf2c3715SXin Li //
13*bf2c3715SXin Li // Software License Agreement (BSD License)
14*bf2c3715SXin Li //
15*bf2c3715SXin Li // Copyright (c) 2011-2014, Willow Garage, Inc.
16*bf2c3715SXin Li // Copyright (c) 2014-2015, Open Source Robotics Foundation
17*bf2c3715SXin Li // All rights reserved.
18*bf2c3715SXin Li //
19*bf2c3715SXin Li // Redistribution and use in source and binary forms, with or without
20*bf2c3715SXin Li // modification, are permitted provided that the following conditions
21*bf2c3715SXin Li // are met:
22*bf2c3715SXin Li //
23*bf2c3715SXin Li //  * Redistributions of source code must retain the above copyright
24*bf2c3715SXin Li //    notice, this list of conditions and the following disclaimer.
25*bf2c3715SXin Li //  * Redistributions in binary form must reproduce the above
26*bf2c3715SXin Li //    copyright notice, this list of conditions and the following
27*bf2c3715SXin Li //    disclaimer in the documentation and/or other materials provided
28*bf2c3715SXin Li //    with the distribution.
29*bf2c3715SXin Li //  * Neither the name of Open Source Robotics Foundation nor the names of its
30*bf2c3715SXin Li //    contributors may be used to endorse or promote products derived
31*bf2c3715SXin Li //    from this software without specific prior written permission.
32*bf2c3715SXin Li //
33*bf2c3715SXin Li // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
34*bf2c3715SXin Li // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
35*bf2c3715SXin Li // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
36*bf2c3715SXin Li // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
37*bf2c3715SXin Li // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
38*bf2c3715SXin Li // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
39*bf2c3715SXin Li // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
40*bf2c3715SXin Li // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
41*bf2c3715SXin Li // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42*bf2c3715SXin Li // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
43*bf2c3715SXin Li // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
44*bf2c3715SXin Li // POSSIBILITY OF SUCH DAMAGE.
45*bf2c3715SXin Li 
46*bf2c3715SXin Li #ifndef EIGEN_ALIGNEDBOX_H
47*bf2c3715SXin Li #define EIGEN_ALIGNEDBOX_H
48*bf2c3715SXin Li 
49*bf2c3715SXin Li namespace Eigen {
50*bf2c3715SXin Li 
51*bf2c3715SXin Li /** \geometry_module \ingroup Geometry_Module
52*bf2c3715SXin Li   *
53*bf2c3715SXin Li   *
54*bf2c3715SXin Li   * \class AlignedBox
55*bf2c3715SXin Li   *
56*bf2c3715SXin Li   * \brief An axis aligned box
57*bf2c3715SXin Li   *
58*bf2c3715SXin Li   * \tparam _Scalar the type of the scalar coefficients
59*bf2c3715SXin Li   * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
60*bf2c3715SXin Li   *
61*bf2c3715SXin Li   * This class represents an axis aligned box as a pair of the minimal and maximal corners.
62*bf2c3715SXin Li   * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty().
63*bf2c3715SXin Li   * \sa alignedboxtypedefs
64*bf2c3715SXin Li   */
65*bf2c3715SXin Li template <typename _Scalar, int _AmbientDim>
66*bf2c3715SXin Li class AlignedBox
67*bf2c3715SXin Li {
68*bf2c3715SXin Li public:
69*bf2c3715SXin Li EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
70*bf2c3715SXin Li   enum { AmbientDimAtCompileTime = _AmbientDim };
71*bf2c3715SXin Li   typedef _Scalar                                   Scalar;
72*bf2c3715SXin Li   typedef NumTraits<Scalar>                         ScalarTraits;
73*bf2c3715SXin Li   typedef Eigen::Index                              Index; ///< \deprecated since Eigen 3.3
74*bf2c3715SXin Li   typedef typename ScalarTraits::Real               RealScalar;
75*bf2c3715SXin Li   typedef typename ScalarTraits::NonInteger         NonInteger;
76*bf2c3715SXin Li   typedef Matrix<Scalar,AmbientDimAtCompileTime,1>  VectorType;
77*bf2c3715SXin Li   typedef CwiseBinaryOp<internal::scalar_sum_op<Scalar>, const VectorType, const VectorType> VectorTypeSum;
78*bf2c3715SXin Li 
79*bf2c3715SXin Li   /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */
80*bf2c3715SXin Li   enum CornerType
81*bf2c3715SXin Li   {
82*bf2c3715SXin Li     /** 1D names @{ */
83*bf2c3715SXin Li     Min=0, Max=1,
84*bf2c3715SXin Li     /** @} */
85*bf2c3715SXin Li 
86*bf2c3715SXin Li     /** Identifier for 2D corner @{ */
87*bf2c3715SXin Li     BottomLeft=0, BottomRight=1,
88*bf2c3715SXin Li     TopLeft=2, TopRight=3,
89*bf2c3715SXin Li     /** @} */
90*bf2c3715SXin Li 
91*bf2c3715SXin Li     /** Identifier for 3D corner  @{ */
92*bf2c3715SXin Li     BottomLeftFloor=0, BottomRightFloor=1,
93*bf2c3715SXin Li     TopLeftFloor=2, TopRightFloor=3,
94*bf2c3715SXin Li     BottomLeftCeil=4, BottomRightCeil=5,
95*bf2c3715SXin Li     TopLeftCeil=6, TopRightCeil=7
96*bf2c3715SXin Li     /** @} */
97*bf2c3715SXin Li   };
98*bf2c3715SXin Li 
99*bf2c3715SXin Li 
100*bf2c3715SXin Li   /** Default constructor initializing a null box. */
AlignedBox()101*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox()
102*bf2c3715SXin Li   { if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime!=Dynamic)) setEmpty(); }
103*bf2c3715SXin Li 
104*bf2c3715SXin Li   /** Constructs a null box with \a _dim the dimension of the ambient space. */
AlignedBox(Index _dim)105*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim)
106*bf2c3715SXin Li   { setEmpty(); }
107*bf2c3715SXin Li 
108*bf2c3715SXin Li   /** Constructs a box with extremities \a _min and \a _max.
109*bf2c3715SXin Li    * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */
110*bf2c3715SXin Li   template<typename OtherVectorType1, typename OtherVectorType2>
AlignedBox(const OtherVectorType1 & _min,const OtherVectorType2 & _max)111*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {}
112*bf2c3715SXin Li 
113*bf2c3715SXin Li   /** Constructs a box containing a single point \a p. */
114*bf2c3715SXin Li   template<typename Derived>
AlignedBox(const MatrixBase<Derived> & p)115*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min)
116*bf2c3715SXin Li   { }
117*bf2c3715SXin Li 
~AlignedBox()118*bf2c3715SXin Li   EIGEN_DEVICE_FUNC ~AlignedBox() {}
119*bf2c3715SXin Li 
120*bf2c3715SXin Li   /** \returns the dimension in which the box holds */
dim()121*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); }
122*bf2c3715SXin Li 
123*bf2c3715SXin Li   /** \deprecated use isEmpty() */
isNull()124*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); }
125*bf2c3715SXin Li 
126*bf2c3715SXin Li   /** \deprecated use setEmpty() */
setNull()127*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); }
128*bf2c3715SXin Li 
129*bf2c3715SXin Li   /** \returns true if the box is empty.
130*bf2c3715SXin Li    * \sa setEmpty */
isEmpty()131*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); }
132*bf2c3715SXin Li 
133*bf2c3715SXin Li   /** Makes \c *this an empty box.
134*bf2c3715SXin Li    * \sa isEmpty */
setEmpty()135*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline void setEmpty()
136*bf2c3715SXin Li   {
137*bf2c3715SXin Li     m_min.setConstant( ScalarTraits::highest() );
138*bf2c3715SXin Li     m_max.setConstant( ScalarTraits::lowest() );
139*bf2c3715SXin Li   }
140*bf2c3715SXin Li 
141*bf2c3715SXin Li   /** \returns the minimal corner */
142*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; }
143*bf2c3715SXin Li   /** \returns a non const reference to the minimal corner */
144*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; }
145*bf2c3715SXin Li   /** \returns the maximal corner */
146*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; }
147*bf2c3715SXin Li   /** \returns a non const reference to the maximal corner */
148*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; }
149*bf2c3715SXin Li 
150*bf2c3715SXin Li   /** \returns the center of the box */
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum,RealScalar,quotient)151*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient)
152*bf2c3715SXin Li   center() const
153*bf2c3715SXin Li   { return (m_min+m_max)/RealScalar(2); }
154*bf2c3715SXin Li 
155*bf2c3715SXin Li   /** \returns the lengths of the sides of the bounding box.
156*bf2c3715SXin Li     * Note that this function does not get the same
157*bf2c3715SXin Li     * result for integral or floating scalar types: see
158*bf2c3715SXin Li     */
sizes()159*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline const CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> sizes() const
160*bf2c3715SXin Li   { return m_max - m_min; }
161*bf2c3715SXin Li 
162*bf2c3715SXin Li   /** \returns the volume of the bounding box */
volume()163*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline Scalar volume() const
164*bf2c3715SXin Li   { return sizes().prod(); }
165*bf2c3715SXin Li 
166*bf2c3715SXin Li   /** \returns an expression for the bounding box diagonal vector
167*bf2c3715SXin Li     * if the length of the diagonal is needed: diagonal().norm()
168*bf2c3715SXin Li     * will provide it.
169*bf2c3715SXin Li     */
diagonal()170*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> diagonal() const
171*bf2c3715SXin Li   { return sizes(); }
172*bf2c3715SXin Li 
173*bf2c3715SXin Li   /** \returns the vertex of the bounding box at the corner defined by
174*bf2c3715SXin Li     * the corner-id corner. It works only for a 1D, 2D or 3D bounding box.
175*bf2c3715SXin Li     * For 1D bounding boxes corners are named by 2 enum constants:
176*bf2c3715SXin Li     * BottomLeft and BottomRight.
177*bf2c3715SXin Li     * For 2D bounding boxes, corners are named by 4 enum constants:
178*bf2c3715SXin Li     * BottomLeft, BottomRight, TopLeft, TopRight.
179*bf2c3715SXin Li     * For 3D bounding boxes, the following names are added:
180*bf2c3715SXin Li     * BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil.
181*bf2c3715SXin Li     */
corner(CornerType corner)182*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const
183*bf2c3715SXin Li   {
184*bf2c3715SXin Li     EIGEN_STATIC_ASSERT(_AmbientDim <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE);
185*bf2c3715SXin Li 
186*bf2c3715SXin Li     VectorType res;
187*bf2c3715SXin Li 
188*bf2c3715SXin Li     Index mult = 1;
189*bf2c3715SXin Li     for(Index d=0; d<dim(); ++d)
190*bf2c3715SXin Li     {
191*bf2c3715SXin Li       if( mult & corner ) res[d] = m_max[d];
192*bf2c3715SXin Li       else                res[d] = m_min[d];
193*bf2c3715SXin Li       mult *= 2;
194*bf2c3715SXin Li     }
195*bf2c3715SXin Li     return res;
196*bf2c3715SXin Li   }
197*bf2c3715SXin Li 
198*bf2c3715SXin Li   /** \returns a random point inside the bounding box sampled with
199*bf2c3715SXin Li    * a uniform distribution */
sample()200*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline VectorType sample() const
201*bf2c3715SXin Li   {
202*bf2c3715SXin Li     VectorType r(dim());
203*bf2c3715SXin Li     for(Index d=0; d<dim(); ++d)
204*bf2c3715SXin Li     {
205*bf2c3715SXin Li       if(!ScalarTraits::IsInteger)
206*bf2c3715SXin Li       {
207*bf2c3715SXin Li         r[d] = m_min[d] + (m_max[d]-m_min[d])
208*bf2c3715SXin Li              * internal::random<Scalar>(Scalar(0), Scalar(1));
209*bf2c3715SXin Li       }
210*bf2c3715SXin Li       else
211*bf2c3715SXin Li         r[d] = internal::random(m_min[d], m_max[d]);
212*bf2c3715SXin Li     }
213*bf2c3715SXin Li     return r;
214*bf2c3715SXin Li   }
215*bf2c3715SXin Li 
216*bf2c3715SXin Li   /** \returns true if the point \a p is inside the box \c *this. */
217*bf2c3715SXin Li   template<typename Derived>
contains(const MatrixBase<Derived> & p)218*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const
219*bf2c3715SXin Li   {
220*bf2c3715SXin Li     typename internal::nested_eval<Derived,2>::type p_n(p.derived());
221*bf2c3715SXin Li     return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all();
222*bf2c3715SXin Li   }
223*bf2c3715SXin Li 
224*bf2c3715SXin Li   /** \returns true if the box \a b is entirely inside the box \c *this. */
contains(const AlignedBox & b)225*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const
226*bf2c3715SXin Li   { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); }
227*bf2c3715SXin Li 
228*bf2c3715SXin Li   /** \returns true if the box \a b is intersecting the box \c *this.
229*bf2c3715SXin Li    * \sa intersection, clamp */
intersects(const AlignedBox & b)230*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const
231*bf2c3715SXin Li   { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); }
232*bf2c3715SXin Li 
233*bf2c3715SXin Li   /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this.
234*bf2c3715SXin Li    * \sa extend(const AlignedBox&) */
235*bf2c3715SXin Li   template<typename Derived>
extend(const MatrixBase<Derived> & p)236*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p)
237*bf2c3715SXin Li   {
238*bf2c3715SXin Li     typename internal::nested_eval<Derived,2>::type p_n(p.derived());
239*bf2c3715SXin Li     m_min = m_min.cwiseMin(p_n);
240*bf2c3715SXin Li     m_max = m_max.cwiseMax(p_n);
241*bf2c3715SXin Li     return *this;
242*bf2c3715SXin Li   }
243*bf2c3715SXin Li 
244*bf2c3715SXin Li   /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this.
245*bf2c3715SXin Li    * \sa merged, extend(const MatrixBase&) */
extend(const AlignedBox & b)246*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b)
247*bf2c3715SXin Li   {
248*bf2c3715SXin Li     m_min = m_min.cwiseMin(b.m_min);
249*bf2c3715SXin Li     m_max = m_max.cwiseMax(b.m_max);
250*bf2c3715SXin Li     return *this;
251*bf2c3715SXin Li   }
252*bf2c3715SXin Li 
253*bf2c3715SXin Li   /** Clamps \c *this by the box \a b and returns a reference to \c *this.
254*bf2c3715SXin Li    * \note If the boxes don't intersect, the resulting box is empty.
255*bf2c3715SXin Li    * \sa intersection(), intersects() */
clamp(const AlignedBox & b)256*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b)
257*bf2c3715SXin Li   {
258*bf2c3715SXin Li     m_min = m_min.cwiseMax(b.m_min);
259*bf2c3715SXin Li     m_max = m_max.cwiseMin(b.m_max);
260*bf2c3715SXin Li     return *this;
261*bf2c3715SXin Li   }
262*bf2c3715SXin Li 
263*bf2c3715SXin Li   /** Returns an AlignedBox that is the intersection of \a b and \c *this
264*bf2c3715SXin Li    * \note If the boxes don't intersect, the resulting box is empty.
265*bf2c3715SXin Li    * \sa intersects(), clamp, contains()  */
intersection(const AlignedBox & b)266*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const
267*bf2c3715SXin Li   {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); }
268*bf2c3715SXin Li 
269*bf2c3715SXin Li   /** Returns an AlignedBox that is the union of \a b and \c *this.
270*bf2c3715SXin Li    * \note Merging with an empty box may result in a box bigger than \c *this.
271*bf2c3715SXin Li    * \sa extend(const AlignedBox&) */
merged(const AlignedBox & b)272*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const
273*bf2c3715SXin Li   { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); }
274*bf2c3715SXin Li 
275*bf2c3715SXin Li   /** Translate \c *this by the vector \a t and returns a reference to \c *this. */
276*bf2c3715SXin Li   template<typename Derived>
translate(const MatrixBase<Derived> & a_t)277*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t)
278*bf2c3715SXin Li   {
279*bf2c3715SXin Li     const typename internal::nested_eval<Derived,2>::type t(a_t.derived());
280*bf2c3715SXin Li     m_min += t;
281*bf2c3715SXin Li     m_max += t;
282*bf2c3715SXin Li     return *this;
283*bf2c3715SXin Li   }
284*bf2c3715SXin Li 
285*bf2c3715SXin Li   /** \returns a copy of \c *this translated by the vector \a t. */
286*bf2c3715SXin Li   template<typename Derived>
translated(const MatrixBase<Derived> & a_t)287*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const
288*bf2c3715SXin Li   {
289*bf2c3715SXin Li     AlignedBox result(m_min, m_max);
290*bf2c3715SXin Li     result.translate(a_t);
291*bf2c3715SXin Li     return result;
292*bf2c3715SXin Li   }
293*bf2c3715SXin Li 
294*bf2c3715SXin Li   /** \returns the squared distance between the point \a p and the box \c *this,
295*bf2c3715SXin Li     * and zero if \a p is inside the box.
296*bf2c3715SXin Li     * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&)
297*bf2c3715SXin Li     */
298*bf2c3715SXin Li   template<typename Derived>
299*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
300*bf2c3715SXin Li 
301*bf2c3715SXin Li   /** \returns the squared distance between the boxes \a b and \c *this,
302*bf2c3715SXin Li     * and zero if the boxes intersect.
303*bf2c3715SXin Li     * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&)
304*bf2c3715SXin Li     */
305*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
306*bf2c3715SXin Li 
307*bf2c3715SXin Li   /** \returns the distance between the point \a p and the box \c *this,
308*bf2c3715SXin Li     * and zero if \a p is inside the box.
309*bf2c3715SXin Li     * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&)
310*bf2c3715SXin Li     */
311*bf2c3715SXin Li   template<typename Derived>
exteriorDistance(const MatrixBase<Derived> & p)312*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const
313*bf2c3715SXin Li   { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); }
314*bf2c3715SXin Li 
315*bf2c3715SXin Li   /** \returns the distance between the boxes \a b and \c *this,
316*bf2c3715SXin Li     * and zero if the boxes intersect.
317*bf2c3715SXin Li     * \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&)
318*bf2c3715SXin Li     */
exteriorDistance(const AlignedBox & b)319*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const
320*bf2c3715SXin Li   { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); }
321*bf2c3715SXin Li 
322*bf2c3715SXin Li   /**
323*bf2c3715SXin Li    * Specialization of transform for pure translation.
324*bf2c3715SXin Li    */
325*bf2c3715SXin Li   template<int Mode, int Options>
transform(const typename Transform<Scalar,AmbientDimAtCompileTime,Mode,Options>::TranslationType & translation)326*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline void transform(
327*bf2c3715SXin Li       const typename Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>::TranslationType& translation)
328*bf2c3715SXin Li   {
329*bf2c3715SXin Li     this->translate(translation);
330*bf2c3715SXin Li   }
331*bf2c3715SXin Li 
332*bf2c3715SXin Li   /**
333*bf2c3715SXin Li    * Transforms this box by \a transform and recomputes it to
334*bf2c3715SXin Li    * still be an axis-aligned box.
335*bf2c3715SXin Li    *
336*bf2c3715SXin Li    * \note This method is provided under BSD license (see the top of this file).
337*bf2c3715SXin Li    */
338*bf2c3715SXin Li   template<int Mode, int Options>
transform(const Transform<Scalar,AmbientDimAtCompileTime,Mode,Options> & transform)339*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline void transform(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform)
340*bf2c3715SXin Li   {
341*bf2c3715SXin Li     // Only Affine and Isometry transforms are currently supported.
342*bf2c3715SXin Li     EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
343*bf2c3715SXin Li 
344*bf2c3715SXin Li     // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
345*bf2c3715SXin Li     // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
346*bf2c3715SXin Li     //
347*bf2c3715SXin Li     // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
348*bf2c3715SXin Li 
349*bf2c3715SXin Li     // two times rotated extent
350*bf2c3715SXin Li     const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
351*bf2c3715SXin Li     // two times new center
352*bf2c3715SXin Li     const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) +
353*bf2c3715SXin Li         Scalar(2) * transform.translation();
354*bf2c3715SXin Li 
355*bf2c3715SXin Li     this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
356*bf2c3715SXin Li     this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
357*bf2c3715SXin Li   }
358*bf2c3715SXin Li 
359*bf2c3715SXin Li   /**
360*bf2c3715SXin Li    * \returns a copy of \c *this transformed by \a transform and recomputed to
361*bf2c3715SXin Li    * still be an axis-aligned box.
362*bf2c3715SXin Li    */
363*bf2c3715SXin Li   template<int Mode, int Options>
transformed(const Transform<Scalar,AmbientDimAtCompileTime,Mode,Options> & transform)364*bf2c3715SXin Li   EIGEN_DEVICE_FUNC AlignedBox transformed(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) const
365*bf2c3715SXin Li   {
366*bf2c3715SXin Li     AlignedBox result(m_min, m_max);
367*bf2c3715SXin Li     result.transform(transform);
368*bf2c3715SXin Li     return result;
369*bf2c3715SXin Li   }
370*bf2c3715SXin Li 
371*bf2c3715SXin Li   /** \returns \c *this with scalar type casted to \a NewScalarType
372*bf2c3715SXin Li     *
373*bf2c3715SXin Li     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
374*bf2c3715SXin Li     * then this function smartly returns a const reference to \c *this.
375*bf2c3715SXin Li     */
376*bf2c3715SXin Li   template<typename NewScalarType>
377*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox,
cast()378*bf2c3715SXin Li            AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
379*bf2c3715SXin Li   {
380*bf2c3715SXin Li     return typename internal::cast_return_type<AlignedBox,
381*bf2c3715SXin Li                     AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
382*bf2c3715SXin Li   }
383*bf2c3715SXin Li 
384*bf2c3715SXin Li   /** Copy constructor with scalar type conversion */
385*bf2c3715SXin Li   template<typename OtherScalarType>
AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime> & other)386*bf2c3715SXin Li   EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
387*bf2c3715SXin Li   {
388*bf2c3715SXin Li     m_min = (other.min)().template cast<Scalar>();
389*bf2c3715SXin Li     m_max = (other.max)().template cast<Scalar>();
390*bf2c3715SXin Li   }
391*bf2c3715SXin Li 
392*bf2c3715SXin Li   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
393*bf2c3715SXin Li     * determined by \a prec.
394*bf2c3715SXin Li     *
395*bf2c3715SXin Li     * \sa MatrixBase::isApprox() */
396*bf2c3715SXin Li   EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const
397*bf2c3715SXin Li   { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
398*bf2c3715SXin Li 
399*bf2c3715SXin Li protected:
400*bf2c3715SXin Li 
401*bf2c3715SXin Li   VectorType m_min, m_max;
402*bf2c3715SXin Li };
403*bf2c3715SXin Li 
404*bf2c3715SXin Li 
405*bf2c3715SXin Li 
406*bf2c3715SXin Li template<typename Scalar,int AmbientDim>
407*bf2c3715SXin Li template<typename Derived>
squaredExteriorDistance(const MatrixBase<Derived> & a_p)408*bf2c3715SXin Li EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const
409*bf2c3715SXin Li {
410*bf2c3715SXin Li   typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived());
411*bf2c3715SXin Li   Scalar dist2(0);
412*bf2c3715SXin Li   Scalar aux;
413*bf2c3715SXin Li   for (Index k=0; k<dim(); ++k)
414*bf2c3715SXin Li   {
415*bf2c3715SXin Li     if( m_min[k] > p[k] )
416*bf2c3715SXin Li     {
417*bf2c3715SXin Li       aux = m_min[k] - p[k];
418*bf2c3715SXin Li       dist2 += aux*aux;
419*bf2c3715SXin Li     }
420*bf2c3715SXin Li     else if( p[k] > m_max[k] )
421*bf2c3715SXin Li     {
422*bf2c3715SXin Li       aux = p[k] - m_max[k];
423*bf2c3715SXin Li       dist2 += aux*aux;
424*bf2c3715SXin Li     }
425*bf2c3715SXin Li   }
426*bf2c3715SXin Li   return dist2;
427*bf2c3715SXin Li }
428*bf2c3715SXin Li 
429*bf2c3715SXin Li template<typename Scalar,int AmbientDim>
squaredExteriorDistance(const AlignedBox & b)430*bf2c3715SXin Li EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const
431*bf2c3715SXin Li {
432*bf2c3715SXin Li   Scalar dist2(0);
433*bf2c3715SXin Li   Scalar aux;
434*bf2c3715SXin Li   for (Index k=0; k<dim(); ++k)
435*bf2c3715SXin Li   {
436*bf2c3715SXin Li     if( m_min[k] > b.m_max[k] )
437*bf2c3715SXin Li     {
438*bf2c3715SXin Li       aux = m_min[k] - b.m_max[k];
439*bf2c3715SXin Li       dist2 += aux*aux;
440*bf2c3715SXin Li     }
441*bf2c3715SXin Li     else if( b.m_min[k] > m_max[k] )
442*bf2c3715SXin Li     {
443*bf2c3715SXin Li       aux = b.m_min[k] - m_max[k];
444*bf2c3715SXin Li       dist2 += aux*aux;
445*bf2c3715SXin Li     }
446*bf2c3715SXin Li   }
447*bf2c3715SXin Li   return dist2;
448*bf2c3715SXin Li }
449*bf2c3715SXin Li 
450*bf2c3715SXin Li /** \defgroup alignedboxtypedefs Global aligned box typedefs
451*bf2c3715SXin Li   *
452*bf2c3715SXin Li   * \ingroup Geometry_Module
453*bf2c3715SXin Li   *
454*bf2c3715SXin Li   * Eigen defines several typedef shortcuts for most common aligned box types.
455*bf2c3715SXin Li   *
456*bf2c3715SXin Li   * The general patterns are the following:
457*bf2c3715SXin Li   *
458*bf2c3715SXin Li   * \c AlignedBoxSizeType where \c Size can be \c 1, \c 2,\c 3,\c 4 for fixed size boxes or \c X for dynamic size,
459*bf2c3715SXin Li   * and where \c Type can be \c i for integer, \c f for float, \c d for double.
460*bf2c3715SXin Li   *
461*bf2c3715SXin Li   * For example, \c AlignedBox3d is a fixed-size 3x3 aligned box type of doubles, and \c AlignedBoxXf is a dynamic-size aligned box of floats.
462*bf2c3715SXin Li   *
463*bf2c3715SXin Li   * \sa class AlignedBox
464*bf2c3715SXin Li   */
465*bf2c3715SXin Li 
466*bf2c3715SXin Li #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)    \
467*bf2c3715SXin Li /** \ingroup alignedboxtypedefs */                                 \
468*bf2c3715SXin Li typedef AlignedBox<Type, Size>   AlignedBox##SizeSuffix##TypeSuffix;
469*bf2c3715SXin Li 
470*bf2c3715SXin Li #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
471*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
472*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
473*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
474*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
475*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
476*bf2c3715SXin Li 
477*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int,                  i)
478*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float,                f)
479*bf2c3715SXin Li EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double,               d)
480*bf2c3715SXin Li 
481*bf2c3715SXin Li #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
482*bf2c3715SXin Li #undef EIGEN_MAKE_TYPEDEFS
483*bf2c3715SXin Li 
484*bf2c3715SXin Li } // end namespace Eigen
485*bf2c3715SXin Li 
486*bf2c3715SXin Li #endif // EIGEN_ALIGNEDBOX_H
487