devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added AlignedBox::transform(AffineTransform).

Browse Source
This commit is contained in:
Martin Pecka
2020-09-28 18:06:23 +00:00
committed by Rasmus Munk Larsen
parent a967fadb21
commit 6425e875a1
2 changed files with 483 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
Eigen/src/Geometry/AlignedBox.h
View File

@@ -7,6 +7,42 @@
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// Function void Eigen::AlignedBox::transform(const Transform& transform)
// is provided under the following license agreement:
//
// Software License Agreement (BSD License)
//
// Copyright (c) 2011-2014, Willow Garage, Inc.
// Copyright (c) 2014-2015, Open Source Robotics Foundation
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of Open Source Robotics Foundation nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#ifndef EIGEN_ALIGNEDBOX_H
#define EIGEN_ALIGNEDBOX_H
@@ -246,6 +282,15 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
return *this;
}
/** \returns a copy of \c *this translated by the vector \a t. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const
{
AlignedBox result(m_min, m_max);
result.translate(a_t);
return result;
}
/** \returns the squared distance between the point \a p and the box \c *this,
* and zero if \a p is inside the box.
* \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&)
@@ -274,6 +319,55 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const
{ EIGEN_USING_STD_MATH(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); }
/**
* Specialization of transform for pure translation.
*/
template<int Mode, int Options>
EIGEN_DEVICE_FUNC inline void transform(
const typename Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>::TranslationType& translation)
{
this->translate(translation);
}
/**
* Transforms this box by \a transform and recomputes it to
* still be an axis-aligned box.
*
* \note This method is provided under BSD license (see the top of this file).
*/
template<int Mode, int Options>
EIGEN_DEVICE_FUNC inline void transform(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform)
{
// Projective transform is not (yet) supported
EIGEN_STATIC_ASSERT(Mode != Projective, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
// Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
// https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
//
// Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
// two times rotated extent
const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
// two times new center
const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) +
Scalar(2) * transform.translation();
this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
}
/**
* \returns a copy of \c *this transformed by \a transform and recomputed to
* still be an axis-aligned box.
*/
template<int Mode, int Options>
EIGEN_DEVICE_FUNC AlignedBox transformed(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) const
{
AlignedBox result(m_min, m_max);
result.transform(transform);
return result;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this