diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py index 7fe3a99dd23..70e21549bc0 100755 --- a/upstream_utils/eigen.py +++ b/upstream_utils/eigen.py @@ -54,6 +54,7 @@ def eigen_inclusions(dp, f): "Cholesky", "Core", "Eigenvalues", + "Geometry", "Householder", "IterativeLinearSolvers", "Jacobi", diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index bf844a12aa8..6ddc916a69e 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -24,4 +24,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( : PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} { + ResetPose(m_odometryImpl.GetPose()); +} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 7c75e71ffee..5db1df74c06 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const wpi::array& visionMeasurementStdDevs) : PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) { + ResetPose(m_odometryImpl.GetPose()); +} diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 506bb69d945..c5981e7bfb8 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -45,6 +45,9 @@ class WPILIB_DLLEXPORT PoseEstimator { /** * Constructs a PoseEstimator. * + * @warning The initial pose estimate will always be the default pose, + * regardless of the odometry's current pose. + * * @param kinematics A correctly-configured kinematics object for your * drivetrain. * @param odometry A correctly-configured odometry object for your drivetrain. @@ -60,7 +63,7 @@ class WPILIB_DLLEXPORT PoseEstimator { Odometry& odometry, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) { + : m_odometry(odometry) { for (size_t i = 0; i < 3; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; } diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 8aeec4e5b3b..7205771e50b 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -85,7 +85,9 @@ class SwerveDrivePoseEstimator : PoseEstimator, wpi::array>( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} + m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} { + this->ResetPose(initialPose); + } private: SwerveDriveOdometry m_odometryImpl; diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 52eec9de2b2..24a7283cc3f 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -187,7 +187,7 @@ class LinearFilter { */ template static LinearFilter FiniteDifference( - const wpi::array stencil, units::second_t period) { + const wpi::array& stencil, units::second_t period) { // See // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points // diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues index 51438ef365b..e95f5be1865 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues @@ -14,7 +14,7 @@ #include "Jacobi" #include "Householder" #include "LU" -// #include "Geometry" +#include "Geometry" #include "src/Core/util/DisableStupidWarnings.h" diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry new file mode 100644 index 00000000000..019c98b6ef0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry @@ -0,0 +1,61 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_GEOMETRY_MODULE_H +#define EIGEN_GEOMETRY_MODULE_H + +#include "Core" + +#include "SVD" +#include "LU" +#include + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Geometry_Module Geometry module + * + * This module provides support for: + * - fixed-size homogeneous transformations + * - translation, scaling, 2D and 3D rotations + * - \link Quaternion quaternions \endlink + * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) + * - orthogonal vector generation (\ref MatrixBase::unitOrthogonal) + * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes + * \endlink + * - \link AlignedBox axis aligned bounding boxes \endlink + * - \link umeyama least-square transformation fitting \endlink + * + * \code + * #include + * \endcode + */ + +// IWYU pragma: begin_exports +#include "src/Geometry/OrthoMethods.h" +#include "src/Geometry/EulerAngles.h" +#include "src/Geometry/Homogeneous.h" +#include "src/Geometry/RotationBase.h" +#include "src/Geometry/Rotation2D.h" +#include "src/Geometry/Quaternion.h" +#include "src/Geometry/AngleAxis.h" +#include "src/Geometry/Transform.h" +#include "src/Geometry/Translation.h" +#include "src/Geometry/Scaling.h" +#include "src/Geometry/Hyperplane.h" +#include "src/Geometry/ParametrizedLine.h" +#include "src/Geometry/AlignedBox.h" +#include "src/Geometry/Umeyama.h" + +// Use the SSE optimized version whenever possible. +#if (defined EIGEN_VECTORIZE_SSE) || (defined EIGEN_VECTORIZE_NEON) +#include "src/Geometry/arch/Geometry_SIMD.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_GEOMETRY_MODULE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AlignedBox.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AlignedBox.h new file mode 100644 index 00000000000..e97a8f29531 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AlignedBox.h @@ -0,0 +1,485 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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 + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * + * \class AlignedBox + * + * \brief An axis aligned box + * + * \tparam Scalar_ the type of the scalar coefficients + * \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic. + * + * This class represents an axis aligned box as a pair of the minimal and maximal corners. + * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using + * isEmpty(). \sa alignedboxtypedefs + */ +template +class AlignedBox { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, AmbientDim_) + enum { AmbientDimAtCompileTime = AmbientDim_ }; + typedef Scalar_ Scalar; + typedef NumTraits ScalarTraits; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef typename ScalarTraits::Real RealScalar; + typedef typename ScalarTraits::NonInteger NonInteger; + typedef Matrix VectorType; + typedef CwiseBinaryOp, const VectorType, const VectorType> VectorTypeSum; + + /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */ + enum CornerType { + /** 1D names @{ */ + Min = 0, + Max = 1, + /** @} */ + + /** Identifier for 2D corner @{ */ + BottomLeft = 0, + BottomRight = 1, + TopLeft = 2, + TopRight = 3, + /** @} */ + + /** Identifier for 3D corner @{ */ + BottomLeftFloor = 0, + BottomRightFloor = 1, + TopLeftFloor = 2, + TopRightFloor = 3, + BottomLeftCeil = 4, + BottomRightCeil = 5, + TopLeftCeil = 6, + TopRightCeil = 7 + /** @} */ + }; + + /** Default constructor initializing a null box. */ + EIGEN_DEVICE_FUNC inline AlignedBox() { + if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime != Dynamic)) setEmpty(); + } + + /** Constructs a null box with \a _dim the dimension of the ambient space. */ + EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); } + + /** Constructs a box with extremities \a _min and \a _max. + * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. + */ + template + EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) + : m_min(_min), m_max(_max) {} + + /** Constructs a box containing a single point \a p. */ + template + EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase& p) : m_min(p), m_max(m_min) {} + + EIGEN_DEVICE_FUNC ~AlignedBox() {} + + /** \returns the dimension in which the box holds */ + EIGEN_DEVICE_FUNC inline Index dim() const { + return AmbientDimAtCompileTime == Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); + } + + /** \deprecated use isEmpty() */ + EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } + + /** \deprecated use setEmpty() */ + EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } + + /** \returns true if the box is empty. + * \sa setEmpty */ + EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } + + /** Makes \c *this an empty box. + * \sa isEmpty */ + EIGEN_DEVICE_FUNC inline void setEmpty() { + m_min.setConstant(ScalarTraits::highest()); + m_max.setConstant(ScalarTraits::lowest()); + } + + /** \returns the minimal corner */ + EIGEN_DEVICE_FUNC inline const VectorType&(min)() const { return m_min; } + /** \returns a non const reference to the minimal corner */ + EIGEN_DEVICE_FUNC inline VectorType&(min)() { return m_min; } + /** \returns the maximal corner */ + EIGEN_DEVICE_FUNC inline const VectorType&(max)() const { return m_max; } + /** \returns a non const reference to the maximal corner */ + EIGEN_DEVICE_FUNC inline VectorType&(max)() { return m_max; } + + /** \returns the center of the box */ + EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient) + center() const { + return (m_min + m_max) / RealScalar(2); + } + + /** \returns the lengths of the sides of the bounding box. + * Note that this function does not get the same + * result for integral or floating scalar types: see + */ + EIGEN_DEVICE_FUNC inline const CwiseBinaryOp, const VectorType, + const VectorType> + sizes() const { + return m_max - m_min; + } + + /** \returns the volume of the bounding box */ + EIGEN_DEVICE_FUNC inline Scalar volume() const { return isEmpty() ? Scalar(0) : sizes().prod(); } + + /** \returns an expression for the bounding box diagonal vector + * if the length of the diagonal is needed: diagonal().norm() + * will provide it. + */ + EIGEN_DEVICE_FUNC inline CwiseBinaryOp, const VectorType, + const VectorType> + diagonal() const { + return sizes(); + } + + /** \returns the vertex of the bounding box at the corner defined by + * the corner-id corner. It works only for a 1D, 2D or 3D bounding box. + * For 1D bounding boxes corners are named by 2 enum constants: + * BottomLeft and BottomRight. + * For 2D bounding boxes, corners are named by 4 enum constants: + * BottomLeft, BottomRight, TopLeft, TopRight. + * For 3D bounding boxes, the following names are added: + * BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil. + */ + EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const { + EIGEN_STATIC_ASSERT(AmbientDim_ <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE); + + VectorType res; + + Index mult = 1; + for (Index d = 0; d < dim(); ++d) { + if (mult & corner) + res[d] = m_max[d]; + else + res[d] = m_min[d]; + mult *= 2; + } + return res; + } + + /** \returns a random point inside the bounding box sampled with + * a uniform distribution */ + EIGEN_DEVICE_FUNC inline VectorType sample() const { + VectorType r(dim()); + for (Index d = 0; d < dim(); ++d) { + if (!ScalarTraits::IsInteger) { + r[d] = m_min[d] + (m_max[d] - m_min[d]) * internal::random(Scalar(0), Scalar(1)); + } else + r[d] = internal::random(m_min[d], m_max[d]); + } + return r; + } + + /** \returns true if the point \a p is inside the box \c *this. */ + template + EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase& p) const { + typename internal::nested_eval::type p_n(p.derived()); + return (m_min.array() <= p_n.array()).all() && (p_n.array() <= m_max.array()).all(); + } + + /** \returns true if the box \a b is entirely inside the box \c *this. */ + EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const { + return (m_min.array() <= (b.min)().array()).all() && ((b.max)().array() <= m_max.array()).all(); + } + + /** \returns true if the box \a b is intersecting the box \c *this. + * \sa intersection, clamp */ + EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const { + return (m_min.array() <= (b.max)().array()).all() && ((b.min)().array() <= m_max.array()).all(); + } + + /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. + * \sa extend(const AlignedBox&) */ + template + EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase& p) { + typename internal::nested_eval::type p_n(p.derived()); + m_min = m_min.cwiseMin(p_n); + m_max = m_max.cwiseMax(p_n); + return *this; + } + + /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. + * \sa merged, extend(const MatrixBase&) */ + EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b) { + m_min = m_min.cwiseMin(b.m_min); + m_max = m_max.cwiseMax(b.m_max); + return *this; + } + + /** Clamps \c *this by the box \a b and returns a reference to \c *this. + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersection(), intersects() */ + EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b) { + m_min = m_min.cwiseMax(b.m_min); + m_max = m_max.cwiseMin(b.m_max); + return *this; + } + + /** Returns an AlignedBox that is the intersection of \a b and \c *this + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersects(), clamp, contains() */ + EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const { + return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); + } + + /** Returns an AlignedBox that is the union of \a b and \c *this. + * \note Merging with an empty box may result in a box bigger than \c *this. + * \sa extend(const AlignedBox&) */ + EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const { + return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); + } + + /** Translate \c *this by the vector \a t and returns a reference to \c *this. */ + template + EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase& a_t) { + const typename internal::nested_eval::type t(a_t.derived()); + m_min += t; + m_max += t; + return *this; + } + + /** \returns a copy of \c *this translated by the vector \a t. */ + template + EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase& 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&) + */ + template + EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase& p) const; + + /** \returns the squared distance between the boxes \a b and \c *this, + * and zero if the boxes intersect. + * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&) + */ + EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const; + + /** \returns the distance between the point \a p and the box \c *this, + * and zero if \a p is inside the box. + * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&) + */ + template + EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase& p) const { + EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); + } + + /** \returns the distance between the boxes \a b and \c *this, + * and zero if the boxes intersect. + * \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&) + */ + EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const { + EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); + } + + /** + * Specialization of transform for pure translation. + */ + template + EIGEN_DEVICE_FUNC inline void transform( + const typename Transform::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 + EIGEN_DEVICE_FUNC inline void transform(const Transform& transform) { + // Only Affine and Isometry transforms are currently supported. + EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, + THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS); + + // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV(...) + // 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 + EIGEN_DEVICE_FUNC AlignedBox + transformed(const Transform& 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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline + typename internal::cast_return_type >::type + cast() const { + return typename internal::cast_return_type >::type( + *this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox& other) { + m_min = (other.min)().template cast(); + m_max = (other.max)().template cast(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other, + const RealScalar& prec = ScalarTraits::dummy_precision()) const { + return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); + } + + protected: + VectorType m_min, m_max; +}; + +template +template +EIGEN_DEVICE_FUNC inline Scalar AlignedBox::squaredExteriorDistance( + const MatrixBase& a_p) const { + typename internal::nested_eval::type p(a_p.derived()); + Scalar dist2(0); + Scalar aux; + for (Index k = 0; k < dim(); ++k) { + if (m_min[k] > p[k]) { + aux = m_min[k] - p[k]; + dist2 += aux * aux; + } else if (p[k] > m_max[k]) { + aux = p[k] - m_max[k]; + dist2 += aux * aux; + } + } + return dist2; +} + +template +EIGEN_DEVICE_FUNC inline Scalar AlignedBox::squaredExteriorDistance(const AlignedBox& b) const { + Scalar dist2(0); + Scalar aux; + for (Index k = 0; k < dim(); ++k) { + if (m_min[k] > b.m_max[k]) { + aux = m_min[k] - b.m_max[k]; + dist2 += aux * aux; + } else if (b.m_min[k] > m_max[k]) { + aux = b.m_min[k] - m_max[k]; + dist2 += aux * aux; + } + } + return dist2; +} + +/** \defgroup alignedboxtypedefs Global aligned box typedefs + * + * \ingroup Geometry_Module + * + * Eigen defines several typedef shortcuts for most common aligned box types. + * + * The general patterns are the following: + * + * \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, + * and where \c Type can be \c i for integer, \c f for float, \c d for double. + * + * 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. + * + * \sa class AlignedBox + */ + +#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ + /** \ingroup alignedboxtypedefs */ \ + typedef AlignedBox AlignedBox##SizeSuffix##TypeSuffix; + +#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X) + +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d) + +#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES +#undef EIGEN_MAKE_TYPEDEFS + +} // end namespace Eigen + +#endif // EIGEN_ALIGNEDBOX_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AngleAxis.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AngleAxis.h new file mode 100644 index 00000000000..a00ed178ddd --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/AngleAxis.h @@ -0,0 +1,245 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_ANGLEAXIS_H +#define EIGEN_ANGLEAXIS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class AngleAxis + * + * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis + * + * \param Scalar_ the scalar type, i.e., the type of the coefficients. + * + * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized. + * + * The following two typedefs are provided for convenience: + * \li \c AngleAxisf for \c float + * \li \c AngleAxisd for \c double + * + * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily + * mimic Euler-angles. Here is an example: + * \include AngleAxis_mimic_euler.cpp + * Output: \verbinclude AngleAxis_mimic_euler.out + * + * \note This class is not aimed to be used to store a rotation transformation, + * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) + * and transformation objects. + * + * \sa class Quaternion, class Transform, MatrixBase::UnitX() + */ + +namespace internal { +template +struct traits > { + typedef Scalar_ Scalar; +}; +} // namespace internal + +template +class AngleAxis : public RotationBase, 3> { + typedef RotationBase, 3> Base; + + public: + using Base::operator*; + + enum { Dim = 3 }; + /** the scalar type of the coefficients */ + typedef Scalar_ Scalar; + typedef Matrix Matrix3; + typedef Matrix Vector3; + typedef Quaternion QuaternionType; + + protected: + Vector3 m_axis; + Scalar m_angle; + + public: + /** Default constructor without initialization. */ + EIGEN_DEVICE_FUNC AngleAxis() {} + /** Constructs and initialize the angle-axis rotation from an \a angle in radian + * and an \a axis which \b must \b be \b normalized. + * + * \warning If the \a axis vector is not normalized, then the angle-axis object + * represents an invalid rotation. */ + template + EIGEN_DEVICE_FUNC inline AngleAxis(const Scalar& angle, const MatrixBase& axis) + : m_axis(axis), m_angle(angle) {} + /** Constructs and initialize the angle-axis rotation from a quaternion \a q. + * This function implicitly normalizes the quaternion \a q. + */ + template + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase& q) { + *this = q; + } + /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ + template + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase& m) { + *this = m; + } + + /** \returns the value of the rotation angle in radian */ + EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; } + /** \returns a read-write reference to the stored angle in radian */ + EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; } + + /** \returns the rotation axis */ + EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; } + /** \returns a read-write reference to the stored rotation axis. + * + * \warning The rotation axis must remain a \b unit vector. + */ + EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline QuaternionType operator*(const AngleAxis& other) const { + return QuaternionType(*this) * QuaternionType(other); + } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& other) const { + return QuaternionType(*this) * other; + } + + /** Concatenates two rotations */ + friend EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& a, const AngleAxis& b) { + return a * QuaternionType(b); + } + + /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ + EIGEN_DEVICE_FUNC AngleAxis inverse() const { return AngleAxis(-m_angle, m_axis); } + + template + EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase& q); + template + EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase& m); + + template + EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase& m); + EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const; + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() + const { + return typename internal::cast_return_type >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis& other) { + m_axis = other.axis().template cast(); + m_angle = Scalar(other.angle()); + } + + EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits::Real& prec = + NumTraits::dummy_precision()) const { + return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle, other.m_angle, prec); + } +}; + +/** \ingroup Geometry_Module + * single precision angle-axis type */ +typedef AngleAxis AngleAxisf; +/** \ingroup Geometry_Module + * double precision angle-axis type */ +typedef AngleAxis AngleAxisd; + +/** Set \c *this from a \b unit quaternion. + * + * The resulting axis is normalized, and the computed angle is in the [0,pi] range. + * + * This function implicitly normalizes the quaternion \a q. + */ +template +template +EIGEN_DEVICE_FUNC AngleAxis& AngleAxis::operator=(const QuaternionBase& q) { + EIGEN_USING_STD(atan2) + EIGEN_USING_STD(abs) + Scalar n = q.vec().norm(); + if (n < NumTraits::epsilon()) n = q.vec().stableNorm(); + + if (n != Scalar(0)) { + m_angle = Scalar(2) * atan2(n, abs(q.w())); + if (q.w() < Scalar(0)) n = -n; + m_axis = q.vec() / n; + } else { + m_angle = Scalar(0); + m_axis << Scalar(1), Scalar(0), Scalar(0); + } + return *this; +} + +/** Set \c *this from a 3x3 rotation matrix \a mat. + */ +template +template +EIGEN_DEVICE_FUNC AngleAxis& AngleAxis::operator=(const MatrixBase& mat) { + // Since a direct conversion would not be really faster, + // let's use the robust Quaternion implementation: + return *this = QuaternionType(mat); +} + +/** + * \brief Sets \c *this from a 3x3 rotation matrix. + **/ +template +template +EIGEN_DEVICE_FUNC AngleAxis& AngleAxis::fromRotationMatrix(const MatrixBase& mat) { + return *this = QuaternionType(mat); +} + +/** Constructs and \returns an equivalent 3x3 rotation matrix. + */ +template +typename AngleAxis::Matrix3 EIGEN_DEVICE_FUNC AngleAxis::toRotationMatrix(void) const { + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) + Matrix3 res; + Vector3 sin_axis = sin(m_angle) * m_axis; + Scalar c = cos(m_angle); + Vector3 cos1_axis = (Scalar(1) - c) * m_axis; + + Scalar tmp; + tmp = cos1_axis.x() * m_axis.y(); + res.coeffRef(0, 1) = tmp - sin_axis.z(); + res.coeffRef(1, 0) = tmp + sin_axis.z(); + + tmp = cos1_axis.x() * m_axis.z(); + res.coeffRef(0, 2) = tmp + sin_axis.y(); + res.coeffRef(2, 0) = tmp - sin_axis.y(); + + tmp = cos1_axis.y() * m_axis.z(); + res.coeffRef(1, 2) = tmp - sin_axis.x(); + res.coeffRef(2, 1) = tmp + sin_axis.x(); + + res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; + + return res; +} + +} // end namespace Eigen + +#endif // EIGEN_ANGLEAXIS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h new file mode 100644 index 00000000000..ad6b821bea1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h @@ -0,0 +1,203 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2023 Juraj Oršulić, University of Zagreb +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_EULERANGLES_H +#define EIGEN_EULERANGLES_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * + * \returns the canonical Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a + * a0,\a a1,\a a2) + * + * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. + * For instance, in: + * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode + * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that + * we have the following equality: + * \code + * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) + * * AngleAxisf(ea[1], Vector3f::UnitX()) + * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode + * This corresponds to the right-multiply conventions (with right hand side frames). + * + * For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]. + * For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi]. + * + * The approach used is also described here: + * https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf + * + * \sa class AngleAxis + */ +template +EIGEN_DEVICE_FUNC inline Matrix::Scalar, 3, 1> MatrixBase::canonicalEulerAngles( + Index a0, Index a1, Index a2) const { + /* Implemented from Graphics Gems IV */ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + + Matrix res; + + const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1; + const Index i = a0; + const Index j = (a0 + 1 + odd) % 3; + const Index k = (a0 + 2 - odd) % 3; + + if (a0 == a2) { + // Proper Euler angles (same first and last axis). + // The i, j, k indices enable addressing the input matrix as the XYX archetype matrix (see Graphics Gems IV), + // where e.g. coeff(k, i) means third column, first row in the XYX archetype matrix: + // c2 s2s1 s2c1 + // s2s3 -c2s1s3 + c1c3 -c2c1s3 - s1c3 + // -s2c3 c2s1c3 + c1s3 c2c1c3 - s1s3 + + // Note: s2 is always positive. + Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i)); + if (odd) { + res[0] = numext::atan2(coeff(j, i), coeff(k, i)); + // s2 is always positive, so res[1] will be within the canonical [0, pi] range + res[1] = numext::atan2(s2, coeff(i, i)); + } else { + // In the !odd case, signs of all three angles are flipped at the very end. To keep the solution within the + // canonical range, we flip the solution and make res[1] always negative here (since s2 is always positive, + // -atan2(s2, c2) will always be negative). The final flip at the end due to !odd will thus make res[1] positive + // and canonical. NB: in the general case, there are two correct solutions, but only one is canonical. For proper + // Euler angles, flipping from one solution to the other involves flipping the sign of the second angle res[1] and + // adding/subtracting pi to the first and third angles. The addition/subtraction of pi to the first angle res[0] + // is handled here by flipping the signs of arguments to atan2, while the calculation of the third angle does not + // need special adjustment since it uses the adjusted res[0] as the input and produces a correct result. + res[0] = numext::atan2(-coeff(j, i), -coeff(k, i)); + res[1] = -numext::atan2(s2, coeff(i, i)); + } + + // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, + // we can compute their respective rotation, and apply its inverse to M. Since the result must + // be a rotation around x, we have: + // + // c2 s1.s2 c1.s2 1 0 0 + // 0 c1 -s1 * M = 0 c3 s3 + // -s2 s1.c2 c1.c2 0 -s3 c3 + // + // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 + + Scalar s1 = numext::sin(res[0]); + Scalar c1 = numext::cos(res[0]); + res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j)); + } else { + // Tait-Bryan angles (all three axes are different; typically used for yaw-pitch-roll calculations). + // The i, j, k indices enable addressing the input matrix as the XYZ archetype matrix (see Graphics Gems IV), + // where e.g. coeff(k, i) means third column, first row in the XYZ archetype matrix: + // c2c3 s2s1c3 - c1s3 s2c1c3 + s1s3 + // c2s3 s2s1s3 + c1c3 s2c1s3 - s1c3 + // -s2 c2s1 c2c1 + + res[0] = numext::atan2(coeff(j, k), coeff(k, k)); + + Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j)); + // c2 is always positive, so the following atan2 will always return a result in the correct canonical middle angle + // range [-pi/2, pi/2] + res[1] = numext::atan2(-coeff(i, k), c2); + + Scalar s1 = numext::sin(res[0]); + Scalar c1 = numext::cos(res[0]); + res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j)); + } + if (!odd) { + res = -res; + } + + return res; +} + +/** \geometry_module \ingroup Geometry_Module + * + * + * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a + * a2) + * + * NB: The returned angles are in non-canonical ranges [0:pi]x[-pi:pi]x[-pi:pi]. For canonical Tait-Bryan/proper Euler + * ranges, use canonicalEulerAngles. + * + * \sa MatrixBase::canonicalEulerAngles + * \sa class AngleAxis + */ +template +EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix::Scalar, 3, 1> +MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const { + /* Implemented from Graphics Gems IV */ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + + Matrix res; + + const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1; + const Index i = a0; + const Index j = (a0 + 1 + odd) % 3; + const Index k = (a0 + 2 - odd) % 3; + + if (a0 == a2) { + res[0] = numext::atan2(coeff(j, i), coeff(k, i)); + if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) { + if (res[0] > Scalar(0)) { + res[0] -= Scalar(EIGEN_PI); + } else { + res[0] += Scalar(EIGEN_PI); + } + + Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i)); + res[1] = -numext::atan2(s2, coeff(i, i)); + } else { + Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i)); + res[1] = numext::atan2(s2, coeff(i, i)); + } + + // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, + // we can compute their respective rotation, and apply its inverse to M. Since the result must + // be a rotation around x, we have: + // + // c2 s1.s2 c1.s2 1 0 0 + // 0 c1 -s1 * M = 0 c3 s3 + // -s2 s1.c2 c1.c2 0 -s3 c3 + // + // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 + + Scalar s1 = numext::sin(res[0]); + Scalar c1 = numext::cos(res[0]); + res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j)); + } else { + res[0] = numext::atan2(coeff(j, k), coeff(k, k)); + Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j)); + if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) { + if (res[0] > Scalar(0)) { + res[0] -= Scalar(EIGEN_PI); + } else { + res[0] += Scalar(EIGEN_PI); + } + res[1] = numext::atan2(-coeff(i, k), -c2); + } else { + res[1] = numext::atan2(-coeff(i, k), c2); + } + Scalar s1 = numext::sin(res[0]); + Scalar c1 = numext::cos(res[0]); + res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j)); + } + if (!odd) { + res = -res; + } + + return res; +} + +} // end namespace Eigen + +#endif // EIGEN_EULERANGLES_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h new file mode 100644 index 00000000000..64c1b651a68 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h @@ -0,0 +1,455 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2010 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_HOMOGENEOUS_H +#define EIGEN_HOMOGENEOUS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Homogeneous + * + * \brief Expression of one (or a set of) homogeneous vector(s) + * + * \param MatrixType the type of the object in which we are making homogeneous + * + * This class represents an expression of one (or a set of) homogeneous vector(s). + * It is the return type of MatrixBase::homogeneous() and most of the time + * this is the only way it is used. + * + * \sa MatrixBase::homogeneous() + */ + +namespace internal { + +template +struct traits > : traits { + typedef typename traits::StorageKind StorageKind; + typedef typename ref_selector::type MatrixTypeNested; + typedef std::remove_reference_t MatrixTypeNested_; + enum { + RowsPlusOne = (MatrixType::RowsAtCompileTime != Dynamic) ? int(MatrixType::RowsAtCompileTime) + 1 : Dynamic, + ColsPlusOne = (MatrixType::ColsAtCompileTime != Dynamic) ? int(MatrixType::ColsAtCompileTime) + 1 : Dynamic, + RowsAtCompileTime = Direction == Vertical ? RowsPlusOne : MatrixType::RowsAtCompileTime, + ColsAtCompileTime = Direction == Horizontal ? ColsPlusOne : MatrixType::ColsAtCompileTime, + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + TmpFlags = MatrixTypeNested_::Flags & HereditaryBits, + Flags = ColsAtCompileTime == 1 ? (TmpFlags & ~RowMajorBit) + : RowsAtCompileTime == 1 ? (TmpFlags | RowMajorBit) + : TmpFlags + }; +}; + +template +struct homogeneous_left_product_impl; +template +struct homogeneous_right_product_impl; + +} // end namespace internal + +template +class Homogeneous : public MatrixBase >, internal::no_assignment_operator { + public: + typedef MatrixType NestedExpression; + enum { Direction = Direction_ }; + + typedef MatrixBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous) + + EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) : m_matrix(matrix) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { + return m_matrix.rows() + (int(Direction) == Vertical ? 1 : 0); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { + return m_matrix.cols() + (int(Direction) == Horizontal ? 1 : 0); + } + + EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; } + + template + EIGEN_DEVICE_FUNC inline const Product operator*(const MatrixBase& rhs) const { + eigen_assert(int(Direction) == Horizontal); + return Product(*this, rhs.derived()); + } + + template + friend EIGEN_DEVICE_FUNC inline const Product operator*(const MatrixBase& lhs, + const Homogeneous& rhs) { + eigen_assert(int(Direction) == Vertical); + return Product(lhs.derived(), rhs); + } + + template + friend EIGEN_DEVICE_FUNC inline const Product, Homogeneous> operator*( + const Transform& lhs, const Homogeneous& rhs) { + eigen_assert(int(Direction) == Vertical); + return Product, Homogeneous>(lhs, rhs); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::result_of::type redux( + const Func& func) const { + return func(m_matrix.redux(func), Scalar(1)); + } + + protected: + typename MatrixType::Nested m_matrix; +}; + +/** \geometry_module \ingroup Geometry_Module + * + * \returns a vector expression that is one longer than the vector argument, with the value 1 symbolically appended as + * the last coefficient. + * + * This can be used to convert affine coordinates to homogeneous coordinates. + * + * \only_for_vectors + * + * Example: \include MatrixBase_homogeneous.cpp + * Output: \verbinclude MatrixBase_homogeneous.out + * + * \sa VectorwiseOp::homogeneous(), class Homogeneous + */ +template +EIGEN_DEVICE_FUNC inline typename MatrixBase::HomogeneousReturnType MatrixBase::homogeneous() const { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + return HomogeneousReturnType(derived()); +} + +/** \geometry_module \ingroup Geometry_Module + * + * \returns an expression where the value 1 is symbolically appended as the final coefficient to each column (or row) of + * the matrix. + * + * This can be used to convert affine coordinates to homogeneous coordinates. + * + * Example: \include VectorwiseOp_homogeneous.cpp + * Output: \verbinclude VectorwiseOp_homogeneous.out + * + * \sa MatrixBase::homogeneous(), class Homogeneous */ +template +EIGEN_DEVICE_FUNC inline Homogeneous VectorwiseOp::homogeneous() + const { + return HomogeneousReturnType(_expression()); +} + +/** \geometry_module \ingroup Geometry_Module + * + * \brief homogeneous normalization + * + * \returns a vector expression of the N-1 first coefficients of \c *this divided by that last coefficient. + * + * This can be used to convert homogeneous coordinates to affine coordinates. + * + * It is essentially a shortcut for: + * \code + this->head(this->size()-1)/this->coeff(this->size()-1); + \endcode + * + * Example: \include MatrixBase_hnormalized.cpp + * Output: \verbinclude MatrixBase_hnormalized.out + * + * \sa VectorwiseOp::hnormalized() */ +template +EIGEN_DEVICE_FUNC inline const typename MatrixBase::HNormalizedReturnType MatrixBase::hnormalized() + const { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + return ConstStartMinusOne(derived(), 0, 0, ColsAtCompileTime == 1 ? size() - 1 : 1, + ColsAtCompileTime == 1 ? 1 : size() - 1) / + coeff(size() - 1); +} + +/** \geometry_module \ingroup Geometry_Module + * + * \brief column or row-wise homogeneous normalization + * + * \returns an expression of the first N-1 coefficients of each column (or row) of \c *this divided by the last + * coefficient of each column (or row). + * + * This can be used to convert homogeneous coordinates to affine coordinates. + * + * It is conceptually equivalent to calling MatrixBase::hnormalized() to each column (or row) of \c *this. + * + * Example: \include DirectionWise_hnormalized.cpp + * Output: \verbinclude DirectionWise_hnormalized.out + * + * \sa MatrixBase::hnormalized() */ +template +EIGEN_DEVICE_FUNC inline const typename VectorwiseOp::HNormalizedReturnType +VectorwiseOp::hnormalized() const { + return HNormalized_Block(_expression(), 0, 0, Direction == Vertical ? _expression().rows() - 1 : _expression().rows(), + Direction == Horizontal ? _expression().cols() - 1 : _expression().cols()) + .cwiseQuotient(Replicate < HNormalized_Factors, Direction == Vertical ? HNormalized_SizeMinusOne : 1, + Direction == Horizontal + ? HNormalized_SizeMinusOne + : 1 > (HNormalized_Factors(_expression(), Direction == Vertical ? _expression().rows() - 1 : 0, + Direction == Horizontal ? _expression().cols() - 1 : 0, + Direction == Vertical ? 1 : _expression().rows(), + Direction == Horizontal ? 1 : _expression().cols()), + Direction == Vertical ? _expression().rows() - 1 : 1, + Direction == Horizontal ? _expression().cols() - 1 : 1)); +} + +namespace internal { + +template +struct take_matrix_for_product { + typedef MatrixOrTransformType type; + EIGEN_DEVICE_FUNC static const type& run(const type& x) { return x; } +}; + +template +struct take_matrix_for_product > { + typedef Transform TransformType; + typedef std::add_const_t type; + EIGEN_DEVICE_FUNC static type run(const TransformType& x) { return x.affine(); } +}; + +template +struct take_matrix_for_product > { + typedef Transform TransformType; + typedef typename TransformType::MatrixType type; + EIGEN_DEVICE_FUNC static const type& run(const TransformType& x) { return x.matrix(); } +}; + +template +struct traits, Lhs> > { + typedef typename take_matrix_for_product::type LhsMatrixType; + typedef remove_all_t MatrixTypeCleaned; + typedef remove_all_t LhsMatrixTypeCleaned; + typedef typename make_proper_matrix_type< + typename traits::Scalar, LhsMatrixTypeCleaned::RowsAtCompileTime, + MatrixTypeCleaned::ColsAtCompileTime, MatrixTypeCleaned::PlainObject::Options, + LhsMatrixTypeCleaned::MaxRowsAtCompileTime, MatrixTypeCleaned::MaxColsAtCompileTime>::type ReturnType; +}; + +template +struct homogeneous_left_product_impl, Lhs> + : public ReturnByValue, Lhs> > { + typedef typename traits::LhsMatrixType LhsMatrixType; + typedef remove_all_t LhsMatrixTypeCleaned; + typedef remove_all_t LhsMatrixTypeNested; + EIGEN_DEVICE_FUNC homogeneous_left_product_impl(const Lhs& lhs, const MatrixType& rhs) + : m_lhs(take_matrix_for_product::run(lhs)), m_rhs(rhs) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + + template + EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { + // FIXME investigate how to allow lazy evaluation of this product when possible + dst = Block < const LhsMatrixTypeNested, LhsMatrixTypeNested::RowsAtCompileTime, + LhsMatrixTypeNested::ColsAtCompileTime == Dynamic + ? Dynamic + : LhsMatrixTypeNested::ColsAtCompileTime - 1 > (m_lhs, 0, 0, m_lhs.rows(), m_lhs.cols() - 1) * m_rhs; + dst += m_lhs.col(m_lhs.cols() - 1).rowwise().template replicate(m_rhs.cols()); + } + + typename LhsMatrixTypeCleaned::Nested m_lhs; + typename MatrixType::Nested m_rhs; +}; + +template +struct traits, Rhs> > { + typedef + typename make_proper_matrix_type::Scalar, MatrixType::RowsAtCompileTime, + Rhs::ColsAtCompileTime, MatrixType::PlainObject::Options, + MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime>::type ReturnType; +}; + +template +struct homogeneous_right_product_impl, Rhs> + : public ReturnByValue, Rhs> > { + typedef remove_all_t RhsNested; + EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + + template + EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { + // FIXME investigate how to allow lazy evaluation of this product when possible + dst = m_lhs * Block < const RhsNested, + RhsNested::RowsAtCompileTime == Dynamic ? Dynamic : RhsNested::RowsAtCompileTime - 1, + RhsNested::ColsAtCompileTime > (m_rhs, 0, 0, m_rhs.rows() - 1, m_rhs.cols()); + dst += m_rhs.row(m_rhs.rows() - 1).colwise().template replicate(m_lhs.rows()); + } + + typename MatrixType::Nested m_lhs; + typename Rhs::Nested m_rhs; +}; + +template +struct evaluator_traits > { + typedef typename storage_kind_to_evaluator_kind::Kind Kind; + typedef HomogeneousShape Shape; +}; + +template <> +struct AssignmentKind { + typedef Dense2Dense Kind; +}; + +template +struct unary_evaluator, IndexBased> + : evaluator::PlainObject> { + typedef Homogeneous XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : Base(), m_temp(op) { + internal::construct_at(this, m_temp); + } + + protected: + PlainObject m_temp; +}; + +// dense = homogeneous +template +struct Assignment, internal::assign_op, + Dense2Dense> { + typedef Homogeneous SrcXprType; + EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src, + const internal::assign_op&) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + dst.template topRows(src.nestedExpression().rows()) = src.nestedExpression(); + dst.row(dst.rows() - 1).setOnes(); + } +}; + +// dense = homogeneous +template +struct Assignment, internal::assign_op, + Dense2Dense> { + typedef Homogeneous SrcXprType; + EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src, + const internal::assign_op&) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + dst.template leftCols(src.nestedExpression().cols()) = src.nestedExpression(); + dst.col(dst.cols() - 1).setOnes(); + } +}; + +template +struct generic_product_impl, Rhs, HomogeneousShape, DenseShape, ProductTag> { + template + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Homogeneous& lhs, const Rhs& rhs) { + homogeneous_right_product_impl, Rhs>(lhs.nestedExpression(), rhs).evalTo(dst); + } +}; + +template +struct homogeneous_right_product_refactoring_helper { + enum { Dim = Lhs::ColsAtCompileTime, Rows = Lhs::RowsAtCompileTime }; + typedef typename Rhs::template ConstNRowsBlockXpr::Type LinearBlockConst; + typedef std::remove_const_t LinearBlock; + typedef typename Rhs::ConstRowXpr ConstantColumn; + typedef Replicate ConstantBlock; + typedef Product LinearProduct; + typedef CwiseBinaryOp, const LinearProduct, + const ConstantBlock> + Xpr; +}; + +template +struct product_evaluator, ProductTag, HomogeneousShape, DenseShape> + : public evaluator< + typename homogeneous_right_product_refactoring_helper::Xpr> { + typedef Product XprType; + typedef homogeneous_right_product_refactoring_helper helper; + typedef typename helper::ConstantBlock ConstantBlock; + typedef typename helper::Xpr RefactoredXpr; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base(xpr.lhs().nestedExpression().lazyProduct( + xpr.rhs().template topRows(xpr.lhs().nestedExpression().cols())) + + ConstantBlock(xpr.rhs().row(xpr.rhs().rows() - 1), xpr.lhs().rows(), 1)) {} +}; + +template +struct generic_product_impl, DenseShape, HomogeneousShape, ProductTag> { + template + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous& rhs) { + homogeneous_left_product_impl, Lhs>(lhs, rhs.nestedExpression()).evalTo(dst); + } +}; + +// TODO: the following specialization is to address a regression from 3.2 to 3.3 +// In the future, this path should be optimized. +template +struct generic_product_impl, TriangularShape, HomogeneousShape, ProductTag> { + template + static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous& rhs) { + dst.noalias() = lhs * rhs.eval(); + } +}; + +template +struct homogeneous_left_product_refactoring_helper { + enum { Dim = Rhs::RowsAtCompileTime, Cols = Rhs::ColsAtCompileTime }; + typedef typename Lhs::template ConstNColsBlockXpr::Type LinearBlockConst; + typedef std::remove_const_t LinearBlock; + typedef typename Lhs::ConstColXpr ConstantColumn; + typedef Replicate ConstantBlock; + typedef Product LinearProduct; + typedef CwiseBinaryOp, const LinearProduct, + const ConstantBlock> + Xpr; +}; + +template +struct product_evaluator, ProductTag, DenseShape, HomogeneousShape> + : public evaluator::Xpr> { + typedef Product XprType; + typedef homogeneous_left_product_refactoring_helper helper; + typedef typename helper::ConstantBlock ConstantBlock; + typedef typename helper::Xpr RefactoredXpr; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base(xpr.lhs() + .template leftCols(xpr.rhs().nestedExpression().rows()) + .lazyProduct(xpr.rhs().nestedExpression()) + + ConstantBlock(xpr.lhs().col(xpr.lhs().cols() - 1), 1, xpr.rhs().cols())) {} +}; + +template +struct generic_product_impl, Homogeneous, DenseShape, + HomogeneousShape, ProductTag> { + typedef Transform TransformType; + template + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const TransformType& lhs, const Homogeneous& rhs) { + homogeneous_left_product_impl, TransformType>(lhs, rhs.nestedExpression()) + .evalTo(dst); + } +}; + +template +struct permutation_matrix_product + : public permutation_matrix_product {}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_HOMOGENEOUS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Hyperplane.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Hyperplane.h new file mode 100644 index 00000000000..0fa0319a03d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Hyperplane.h @@ -0,0 +1,273 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2008 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_HYPERPLANE_H +#define EIGEN_HYPERPLANE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Hyperplane + * + * \brief A hyperplane + * + * A hyperplane is an affine subspace of dimension n-1 in a space of dimension n. + * For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane. + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients + * \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic. + * Notice that the dimension of the hyperplane is AmbientDim_-1. + * + * This class represents an hyperplane as the zero set of the implicit equation + * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part) + * and \f$ d \f$ is the distance (offset) to the origin. + */ +template +class Hyperplane { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, + AmbientDim_ == Dynamic ? Dynamic : AmbientDim_ + 1) + enum { AmbientDimAtCompileTime = AmbientDim_, Options = Options_ }; + typedef Scalar_ Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef Matrix VectorType; + typedef Matrix + Coefficients; + typedef Block NormalReturnType; + typedef const Block ConstNormalReturnType; + + /** Default constructor without initialization */ + EIGEN_DEVICE_FUNC inline Hyperplane() {} + + template + EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane& other) + : m_coeffs(other.coeffs()) {} + + /** Constructs a dynamic-size hyperplane with \a _dim the dimension + * of the ambient space */ + EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim + 1) {} + + /** Construct a plane from its normal \a n and a point \a e onto the plane. + * \warning the vector normal is assumed to be normalized. + */ + EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e) : m_coeffs(n.size() + 1) { + normal() = n; + offset() = -n.dot(e); + } + + /** Constructs a plane from its normal \a n and distance to the origin \a d + * such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$. + * \warning the vector normal is assumed to be normalized. + */ + EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d) : m_coeffs(n.size() + 1) { + normal() = n; + offset() = d; + } + + /** Constructs a hyperplane passing through the two points. If the dimension of the ambient space + * is greater than 2, then there isn't uniqueness, so an arbitrary choice is made. + */ + EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) { + Hyperplane result(p0.size()); + result.normal() = (p1 - p0).unitOrthogonal(); + result.offset() = -p0.dot(result.normal()); + return result; + } + + /** Constructs a hyperplane passing through the three points. The dimension of the ambient space + * is required to be exactly 3. + */ + EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3) + Hyperplane result(p0.size()); + VectorType v0(p2 - p0), v1(p1 - p0); + result.normal() = v0.cross(v1); + RealScalar norm = result.normal().norm(); + if (norm <= v0.norm() * v1.norm() * NumTraits::epsilon()) { + Matrix m; + m << v0.transpose(), v1.transpose(); + JacobiSVD, ComputeFullV> svd(m); + result.normal() = svd.matrixV().col(2); + } else + result.normal() /= norm; + result.offset() = -p0.dot(result.normal()); + return result; + } + + /** Constructs a hyperplane passing through the parametrized line \a parametrized. + * If the dimension of the ambient space is greater than 2, then there isn't uniqueness, + * so an arbitrary choice is made. + */ + // FIXME to be consistent with the rest this could be implemented as a static Through function ?? + EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine& parametrized) { + normal() = parametrized.direction().unitOrthogonal(); + offset() = -parametrized.origin().dot(normal()); + } + + EIGEN_DEVICE_FUNC ~Hyperplane() {} + + /** \returns the dimension in which the plane holds */ + EIGEN_DEVICE_FUNC inline Index dim() const { + return AmbientDimAtCompileTime == Dynamic ? m_coeffs.size() - 1 : Index(AmbientDimAtCompileTime); + } + + /** normalizes \c *this */ + EIGEN_DEVICE_FUNC void normalize(void) { m_coeffs /= normal().norm(); } + + /** \returns the signed distance between the plane \c *this and a point \a p. + * \sa absDistance() + */ + EIGEN_DEVICE_FUNC inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); } + + /** \returns the absolute distance between the plane \c *this and a point \a p. + * \sa signedDistance() + */ + EIGEN_DEVICE_FUNC inline Scalar absDistance(const VectorType& p) const { return numext::abs(signedDistance(p)); } + + /** \returns the projection of a point \a p onto the plane \c *this. + */ + EIGEN_DEVICE_FUNC inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); } + + /** \returns a constant reference to the unit normal vector of the plane, which corresponds + * to the linear part of the implicit equation. + */ + EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const { + return ConstNormalReturnType(m_coeffs, 0, 0, dim(), 1); + } + + /** \returns a non-constant reference to the unit normal vector of the plane, which corresponds + * to the linear part of the implicit equation. + */ + EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs, 0, 0, dim(), 1); } + + /** \returns the distance to the origin, which is also the "constant term" of the implicit equation + * \warning the vector normal is assumed to be normalized. + */ + EIGEN_DEVICE_FUNC inline const Scalar& offset() const { return m_coeffs.coeff(dim()); } + + /** \returns a non-constant reference to the distance to the origin, which is also the constant part + * of the implicit equation */ + EIGEN_DEVICE_FUNC inline Scalar& offset() { return m_coeffs(dim()); } + + /** \returns a constant reference to the coefficients c_i of the plane equation: + * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ + */ + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } + + /** \returns a non-constant reference to the coefficients c_i of the plane equation: + * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ + */ + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } + + /** \returns the intersection of *this with \a other. + * + * \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines. + * + * \note If \a other is approximately parallel to *this, this method will return any point on *this. + */ + EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) + Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0); + // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests + // whether the two lines are approximately parallel. + if (internal::isMuchSmallerThan(det, Scalar(1))) { // special case where the two lines are approximately parallel. + // Pick any point on the first line. + if (numext::abs(coeffs().coeff(1)) > numext::abs(coeffs().coeff(0))) + return VectorType(coeffs().coeff(1), -coeffs().coeff(2) / coeffs().coeff(1) - coeffs().coeff(0)); + else + return VectorType(-coeffs().coeff(2) / coeffs().coeff(0) - coeffs().coeff(1), coeffs().coeff(0)); + } else { // general case + Scalar invdet = Scalar(1) / det; + return VectorType( + invdet * (coeffs().coeff(1) * other.coeffs().coeff(2) - other.coeffs().coeff(1) * coeffs().coeff(2)), + invdet * (other.coeffs().coeff(0) * coeffs().coeff(2) - coeffs().coeff(0) * other.coeffs().coeff(2))); + } + } + + /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this. + * + * \param mat the Dim x Dim transformation matrix + * \param traits specifies whether the matrix \a mat represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + */ + template + EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase& mat, TransformTraits traits = Affine) { + if (traits == Affine) { + normal() = mat.inverse().transpose() * normal(); + m_coeffs /= normal().norm(); + } else if (traits == Isometry) + normal() = mat * normal(); + else { + eigen_assert(0 && "invalid traits value in Hyperplane::transform()"); + } + return *this; + } + + /** Applies the transformation \a t to \c *this and returns a reference to \c *this. + * + * \param t the transformation of dimension Dim + * \param traits specifies whether the transformation \a t represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + * Other kind of transformations are not supported. + */ + template + EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform& t, + TransformTraits traits = Affine) { + transform(t.linear(), traits); + offset() -= normal().dot(t.translation()); + return *this; + } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline + typename internal::cast_return_type >::type + cast() const { + return + typename internal::cast_return_type >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit Hyperplane( + const Hyperplane& other) { + m_coeffs = other.coeffs().template cast(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + template + EIGEN_DEVICE_FUNC bool isApprox( + const Hyperplane& other, + const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const { + return m_coeffs.isApprox(other.m_coeffs, prec); + } + + protected: + Coefficients m_coeffs; +}; + +} // end namespace Eigen + +#endif // EIGEN_HYPERPLANE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/InternalHeaderCheck.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/InternalHeaderCheck.h new file mode 100644 index 00000000000..a1159a3c69c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/InternalHeaderCheck.h @@ -0,0 +1,3 @@ +#ifndef EIGEN_GEOMETRY_MODULE_H +#error "Please include Eigen/Geometry instead of including headers inside the src directory directly." +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h new file mode 100644 index 00000000000..a8e050236e1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h @@ -0,0 +1,259 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_ORTHOMETHODS_H +#define EIGEN_ORTHOMETHODS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// Vector3 version (default) +template +struct cross_impl { + typedef typename ScalarBinaryOpTraits::Scalar, + typename internal::traits::Scalar>::ReturnType Scalar; + typedef Matrix::RowsAtCompileTime, MatrixBase::ColsAtCompileTime> return_type; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE return_type run(const MatrixBase& first, + const MatrixBase& second) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3) + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3) + + // Note that there is no need for an expression here since the compiler + // optimize such a small temporary very well (even within a complex expression) + typename internal::nested_eval::type lhs(first.derived()); + typename internal::nested_eval::type rhs(second.derived()); + return return_type(numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), + numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), + numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))); + } +}; + +// Vector2 version +template +struct cross_impl { + typedef typename ScalarBinaryOpTraits::Scalar, + typename internal::traits::Scalar>::ReturnType Scalar; + typedef Scalar return_type; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE return_type run(const MatrixBase& first, + const MatrixBase& second) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 2); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 2); + typename internal::nested_eval::type lhs(first.derived()); + typename internal::nested_eval::type rhs(second.derived()); + return numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)); + } +}; + +} // end namespace internal + +/** \geometry_module \ingroup Geometry_Module + * + * \returns the cross product of \c *this and \a other. This is either a scalar for size-2 vectors or a size-3 vector + * for size-3 vectors. + * + * This method is implemented for two different cases: between vectors of fixed size 2 and between vectors of fixed + * size 3. + * + * For vectors of size 3, the output is simply the traditional cross product. + * + * For vectors of size 2, the output is a scalar. + * Given vectors \f$ v = \begin{bmatrix} v_1 & v_2 \end{bmatrix} \f$ and \f$ w = \begin{bmatrix} w_1 & w_2 \end{bmatrix} + * \f$, the result is simply \f$ v\times w = \overline{v_1 w_2 - v_2 w_1} = \text{conj}\left|\begin{smallmatrix} v_1 & + * w_1 \\ v_2 & w_2 \end{smallmatrix}\right| \f$; or, to put it differently, it is the third coordinate of the cross + * product of \f$ \begin{bmatrix} v_1 & v_2 & v_3 \end{bmatrix} \f$ and \f$ \begin{bmatrix} w_1 & w_2 & w_3 + * \end{bmatrix} \f$. For real-valued inputs, the result can be interpreted as the signed area of a parallelogram + * spanned by the two vectors. + * + * \note With complex numbers, the cross product is implemented as + * \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times + * \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} + \mathbf{b} \times \mathbf{c})\f$ + * + * \sa MatrixBase::cross3() + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +#ifndef EIGEN_PARSED_BY_DOXYGEN + typename internal::cross_impl::return_type +#else + inline std::conditional_t +#endif + MatrixBase::cross(const MatrixBase& other) const { + return internal::cross_impl::run(*this, other); +} + +namespace internal { + +template ::Flags & evaluator::Flags) & PacketAccessBit)> +struct cross3_impl { + EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type::type run(const VectorLhs& lhs, + const VectorRhs& rhs) { + return typename internal::plain_matrix_type::type( + numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), + numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), + numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), 0); + } +}; + +} // namespace internal + +/** \geometry_module \ingroup Geometry_Module + * + * \returns the cross product of \c *this and \a other using only the x, y, and z coefficients + * + * The size of \c *this and \a other must be four. This function is especially useful + * when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization. + * + * \sa MatrixBase::cross() + */ +template +template +EIGEN_DEVICE_FUNC inline typename MatrixBase::PlainObject MatrixBase::cross3( + const MatrixBase& other) const { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4) + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 4) + + typedef typename internal::nested_eval::type DerivedNested; + typedef typename internal::nested_eval::type OtherDerivedNested; + DerivedNested lhs(derived()); + OtherDerivedNested rhs(other.derived()); + + return internal::cross3_impl, + internal::remove_all_t>::run(lhs, rhs); +} + +/** \geometry_module \ingroup Geometry_Module + * + * \returns a matrix expression of the cross product of each column or row + * of the referenced expression with the \a other vector. + * + * The referenced matrix must have one dimension equal to 3. + * The result matrix has the same dimensions than the referenced one. + * + * \sa MatrixBase::cross() */ +template +template +EIGEN_DEVICE_FUNC const typename VectorwiseOp::CrossReturnType +VectorwiseOp::cross(const MatrixBase& other) const { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3) + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + typename internal::nested_eval::type mat(_expression()); + typename internal::nested_eval::type vec(other.derived()); + + CrossReturnType res(_expression().rows(), _expression().cols()); + if (Direction == Vertical) { + eigen_assert(CrossReturnType::RowsAtCompileTime == 3 && "the matrix must have exactly 3 rows"); + res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate(); + res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate(); + res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate(); + } else { + eigen_assert(CrossReturnType::ColsAtCompileTime == 3 && "the matrix must have exactly 3 columns"); + res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate(); + res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate(); + res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate(); + } + return res; +} + +namespace internal { + +template +struct unitOrthogonal_selector { + typedef typename plain_matrix_type::type VectorType; + typedef typename traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Matrix Vector2; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { + VectorType perp = VectorType::Zero(src.size()); + Index maxi = 0; + Index sndi = 0; + src.cwiseAbs().maxCoeff(&maxi); + if (maxi == 0) sndi = 1; + RealScalar invnm = RealScalar(1) / (Vector2() << src.coeff(sndi), src.coeff(maxi)).finished().norm(); + perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm; + perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm; + + return perp; + } +}; + +template +struct unitOrthogonal_selector { + typedef typename plain_matrix_type::type VectorType; + typedef typename traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { + VectorType perp; + /* Let us compute the crossed product of *this with a vector + * that is not too close to being colinear to *this. + */ + + /* unless the x and y coords are both close to zero, we can + * simply take ( -y, x, 0 ) and normalize it. + */ + if ((!isMuchSmallerThan(src.x(), src.z())) || (!isMuchSmallerThan(src.y(), src.z()))) { + RealScalar invnm = RealScalar(1) / src.template head<2>().norm(); + perp.coeffRef(0) = -numext::conj(src.y()) * invnm; + perp.coeffRef(1) = numext::conj(src.x()) * invnm; + perp.coeffRef(2) = 0; + } + /* if both x and y are close to zero, then the vector is close + * to the z-axis, so it's far from colinear to the x-axis for instance. + * So we take the crossed product with (1,0,0) and normalize it. + */ + else { + RealScalar invnm = RealScalar(1) / src.template tail<2>().norm(); + perp.coeffRef(0) = 0; + perp.coeffRef(1) = -numext::conj(src.z()) * invnm; + perp.coeffRef(2) = numext::conj(src.y()) * invnm; + } + + return perp; + } +}; + +template +struct unitOrthogonal_selector { + typedef typename plain_matrix_type::type VectorType; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { + return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); + } +}; + +} // end namespace internal + +/** \geometry_module \ingroup Geometry_Module + * + * \returns a unit vector which is orthogonal to \c *this + * + * The size of \c *this must be at least 2. If the size is exactly 2, + * then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized(). + * + * \sa cross() + */ +template +EIGEN_DEVICE_FUNC typename MatrixBase::PlainObject MatrixBase::unitOrthogonal() const { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return internal::unitOrthogonal_selector::run(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_ORTHOMETHODS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/ParametrizedLine.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/ParametrizedLine.h new file mode 100644 index 00000000000..5bbd87432a9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/ParametrizedLine.h @@ -0,0 +1,232 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2008 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_PARAMETRIZEDLINE_H +#define EIGEN_PARAMETRIZEDLINE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class ParametrizedLine + * + * \brief A parametrized line + * + * A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit + * direction vector \f$ \mathbf{d} \f$ such that the line corresponds to + * the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ t \in \mathbf{R} \f$. + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients + * \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic. + */ +template +class ParametrizedLine { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, AmbientDim_) + enum { AmbientDimAtCompileTime = AmbientDim_, Options = Options_ }; + typedef Scalar_ Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef Matrix VectorType; + + /** Default constructor without initialization */ + EIGEN_DEVICE_FUNC inline ParametrizedLine() {} + + template + EIGEN_DEVICE_FUNC ParametrizedLine(const ParametrizedLine& other) + : m_origin(other.origin()), m_direction(other.direction()) {} + + /** Constructs a dynamic-size line with \a _dim the dimension + * of the ambient space */ + EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(Index _dim) : m_origin(_dim), m_direction(_dim) {} + + /** Initializes a parametrized line of direction \a direction and origin \a origin. + * \warning the vector direction is assumed to be normalized. + */ + EIGEN_DEVICE_FUNC ParametrizedLine(const VectorType& origin, const VectorType& direction) + : m_origin(origin), m_direction(direction) {} + + template + EIGEN_DEVICE_FUNC explicit ParametrizedLine(const Hyperplane& hyperplane); + + /** Constructs a parametrized line going from \a p0 to \a p1. */ + EIGEN_DEVICE_FUNC static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) { + return ParametrizedLine(p0, (p1 - p0).normalized()); + } + + EIGEN_DEVICE_FUNC ~ParametrizedLine() {} + + /** \returns the dimension in which the line holds */ + EIGEN_DEVICE_FUNC inline Index dim() const { return m_direction.size(); } + + EIGEN_DEVICE_FUNC const VectorType& origin() const { return m_origin; } + EIGEN_DEVICE_FUNC VectorType& origin() { return m_origin; } + + EIGEN_DEVICE_FUNC const VectorType& direction() const { return m_direction; } + EIGEN_DEVICE_FUNC VectorType& direction() { return m_direction; } + + /** \returns the squared distance of a point \a p to its projection onto the line \c *this. + * \sa distance() + */ + EIGEN_DEVICE_FUNC RealScalar squaredDistance(const VectorType& p) const { + VectorType diff = p - origin(); + return (diff - direction().dot(diff) * direction()).squaredNorm(); + } + /** \returns the distance of a point \a p to its projection onto the line \c *this. + * \sa squaredDistance() + */ + EIGEN_DEVICE_FUNC RealScalar distance(const VectorType& p) const { + EIGEN_USING_STD(sqrt) return sqrt(squaredDistance(p)); + } + + /** \returns the projection of a point \a p onto the line \c *this. */ + EIGEN_DEVICE_FUNC VectorType projection(const VectorType& p) const { + return origin() + direction().dot(p - origin()) * direction(); + } + + EIGEN_DEVICE_FUNC VectorType pointAt(const Scalar& t) const; + + template + EIGEN_DEVICE_FUNC Scalar + intersectionParameter(const Hyperplane& hyperplane) const; + + template + EIGEN_DEVICE_FUNC Scalar intersection(const Hyperplane& hyperplane) const; + + template + EIGEN_DEVICE_FUNC VectorType + intersectionPoint(const Hyperplane& hyperplane) const; + + /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this. + * + * \param mat the Dim x Dim transformation matrix + * \param traits specifies whether the matrix \a mat represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + */ + template + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const MatrixBase& mat, + TransformTraits traits = Affine) { + if (traits == Affine) + direction() = (mat * direction()).normalized(); + else if (traits == Isometry) + direction() = mat * direction(); + else { + eigen_assert(0 && "invalid traits value in ParametrizedLine::transform()"); + } + origin() = mat * origin(); + return *this; + } + + /** Applies the transformation \a t to \c *this and returns a reference to \c *this. + * + * \param t the transformation of dimension Dim + * \param traits specifies whether the transformation \a t represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + * Other kind of transformations are not supported. + */ + template + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform( + const Transform& t, TransformTraits traits = Affine) { + transform(t.linear(), traits); + origin() += t.translation(); + return *this; + } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline + typename internal::cast_return_type >::type + cast() const { + return typename internal::cast_return_type< + ParametrizedLine, ParametrizedLine >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit ParametrizedLine( + const ParametrizedLine& other) { + m_origin = other.origin().template cast(); + m_direction = other.direction().template cast(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const ParametrizedLine& other, const typename NumTraits::Real& prec = + NumTraits::dummy_precision()) const { + return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); + } + + protected: + VectorType m_origin, m_direction; +}; + +/** Constructs a parametrized line from a 2D hyperplane + * + * \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line + */ +template +template +EIGEN_DEVICE_FUNC inline ParametrizedLine::ParametrizedLine( + const Hyperplane& hyperplane) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) + direction() = hyperplane.normal().unitOrthogonal(); + origin() = -hyperplane.normal() * hyperplane.offset(); +} + +/** \returns the point at \a t along this line + */ +template +EIGEN_DEVICE_FUNC inline typename ParametrizedLine::VectorType +ParametrizedLine::pointAt(const Scalar_& t) const { + return origin() + (direction() * t); +} + +/** \returns the parameter value of the intersection between \c *this and the given \a hyperplane + */ +template +template +EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine::intersectionParameter( + const Hyperplane& hyperplane) const { + return -(hyperplane.offset() + hyperplane.normal().dot(origin())) / hyperplane.normal().dot(direction()); +} + +/** \deprecated use intersectionParameter() + * \returns the parameter value of the intersection between \c *this and the given \a hyperplane + */ +template +template +EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine::intersection( + const Hyperplane& hyperplane) const { + return intersectionParameter(hyperplane); +} + +/** \returns the point of the intersection between \c *this and the given hyperplane + */ +template +template +EIGEN_DEVICE_FUNC inline typename ParametrizedLine::VectorType +ParametrizedLine::intersectionPoint( + const Hyperplane& hyperplane) const { + return pointAt(intersectionParameter(hyperplane)); +} + +} // end namespace Eigen + +#endif // EIGEN_PARAMETRIZEDLINE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h new file mode 100644 index 00000000000..1d8ded9bd63 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h @@ -0,0 +1,857 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2009 Mathieu Gautier +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_QUATERNION_H +#define EIGEN_QUATERNION_H +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/*************************************************************************** + * Definition of QuaternionBase + * The implementation is at the end of the file + ***************************************************************************/ + +namespace internal { +template +struct quaternionbase_assign_impl; +} + +/** \geometry_module \ingroup Geometry_Module + * \class QuaternionBase + * \brief Base class for quaternion expressions + * \tparam Derived derived type (CRTP) + * \sa class Quaternion + */ +template +class QuaternionBase : public RotationBase { + public: + typedef RotationBase Base; + + using Base::operator*; + using Base::derived; + + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef typename internal::traits::Coefficients Coefficients; + typedef typename Coefficients::CoeffReturnType CoeffReturnType; + typedef std::conditional_t::Flags& LvalueBit), Scalar&, CoeffReturnType> + NonConstCoeffReturnType; + + enum { Flags = Eigen::internal::traits::Flags }; + + // typedef typename Matrix Coefficients; + /** the type of a 3D vector */ + typedef Matrix Vector3; + /** the equivalent rotation matrix type */ + typedef Matrix Matrix3; + /** the equivalent angle-axis type */ + typedef AngleAxis AngleAxisType; + + /** \returns the \c x coefficient */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); } + /** \returns the \c y coefficient */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); } + /** \returns the \c z coefficient */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); } + /** \returns the \c w coefficient */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); } + + /** \returns a reference to the \c x coefficient (if Derived is a non-const lvalue) */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); } + /** \returns a reference to the \c y coefficient (if Derived is a non-const lvalue) */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); } + /** \returns a reference to the \c z coefficient (if Derived is a non-const lvalue) */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); } + /** \returns a reference to the \c w coefficient (if Derived is a non-const lvalue) */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); } + + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + EIGEN_DEVICE_FUNC inline const VectorBlock vec() const { return coeffs().template head<3>(); } + + /** \returns a vector expression of the imaginary part (x,y,z) */ + EIGEN_DEVICE_FUNC inline VectorBlock vec() { return coeffs().template head<3>(); } + + /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ + EIGEN_DEVICE_FUNC inline const typename internal::traits::Coefficients& coeffs() const { + return derived().coeffs(); + } + + /** \returns a vector expression of the coefficients (x,y,z,w) */ + EIGEN_DEVICE_FUNC inline typename internal::traits::Coefficients& coeffs() { return derived().coeffs(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase& operator=(const QuaternionBase& other); + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase& other); + + // disabled this copy operator as it is giving very strange compilation errors when compiling + // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's + // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase + // we didn't have to add, in addition to templated operator=, such a non-templated copy operator. + // Derived& operator=(const QuaternionBase& other) + // { return operator=(other); } + + EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa); + template + EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase& m); + + /** \returns a quaternion representing an identity rotation + * \sa MatrixBase::Identity() + */ + EIGEN_DEVICE_FUNC static inline Quaternion Identity() { + return Quaternion(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); + } + + /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() + */ + EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() { + coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); + return *this; + } + + /** \returns the squared norm of the quaternion's coefficients + * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() + */ + EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } + + /** \returns the norm of the quaternion's coefficients + * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() + */ + EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); } + + /** Normalizes the quaternion \c *this + * \sa normalized(), MatrixBase::normalize() */ + EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); } + /** \returns a normalized copy of \c *this + * \sa normalize(), MatrixBase::normalized() */ + EIGEN_DEVICE_FUNC inline Quaternion normalized() const { return Quaternion(coeffs().normalized()); } + + /** \returns the dot product of \c *this and \a other + * Geometrically speaking, the dot product of two unit quaternions + * corresponds to the cosine of half the angle between the two rotations. + * \sa angularDistance() + */ + template + EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase& other) const { + return coeffs().dot(other.coeffs()); + } + + template + EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase& other) const; + + /** \returns an equivalent 3x3 rotation matrix */ + EIGEN_DEVICE_FUNC inline Matrix3 toRotationMatrix() const; + + /** \returns the quaternion which transform \a a into \a b through a rotation */ + template + EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion operator*(const QuaternionBase& q) const; + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const QuaternionBase& q); + + /** \returns the quaternion describing the inverse rotation */ + EIGEN_DEVICE_FUNC Quaternion inverse() const; + + /** \returns the conjugated quaternion */ + EIGEN_DEVICE_FUNC Quaternion conjugate() const; + + template + EIGEN_DEVICE_FUNC Quaternion slerp(const Scalar& t, const QuaternionBase& other) const; + + /** \returns true if each coefficients of \c *this and \a other are all exactly equal. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator!= */ + template + EIGEN_DEVICE_FUNC inline bool operator==(const QuaternionBase& other) const { + return coeffs() == other.coeffs(); + } + + /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator== */ + template + EIGEN_DEVICE_FUNC inline bool operator!=(const QuaternionBase& other) const { + return coeffs() != other.coeffs(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + template + EIGEN_DEVICE_FUNC bool isApprox(const QuaternionBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const { + return coeffs().isApprox(other.coeffs(), prec); + } + + /** return the result vector of \a v through the rotation*/ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const; + +#else + + template + EIGEN_DEVICE_FUNC inline std::enable_if_t::value, const Derived&> cast() + const { + return derived(); + } + + template + EIGEN_DEVICE_FUNC inline std::enable_if_t::value, + Quaternion > + cast() const { + return Quaternion(coeffs().template cast()); + } +#endif + +#ifndef EIGEN_NO_IO + friend std::ostream& operator<<(std::ostream& s, const QuaternionBase& q) { + s << q.x() << "i + " << q.y() << "j + " << q.z() << "k" + << " + " << q.w(); + return s; + } +#endif + +#ifdef EIGEN_QUATERNIONBASE_PLUGIN +#include EIGEN_QUATERNIONBASE_PLUGIN +#endif + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase) +}; + +/*************************************************************************** + * Definition/implementation of Quaternion + ***************************************************************************/ + +/** \geometry_module \ingroup Geometry_Module + * + * \class Quaternion + * + * \brief The quaternion class used to represent 3D orientations and rotations + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients + * \tparam Options_ controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is + * AutoAlign. + * + * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of + * orientations and rotations of objects in three dimensions. Compared to other representations + * like Euler angles or 3x3 matrices, quaternions offer the following advantages: + * \li \b compact storage (4 scalars) + * \li \b efficient to compose (28 flops), + * \li \b stable spherical interpolation + * + * The following two typedefs are provided for convenience: + * \li \c Quaternionf for \c float + * \li \c Quaterniond for \c double + * + * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not + * normalized. + * + * \sa class AngleAxis, class Transform + */ + +namespace internal { +template +struct traits > { + typedef Quaternion PlainObject; + typedef Scalar_ Scalar; + typedef Matrix Coefficients; + enum { Alignment = internal::traits::Alignment, Flags = LvalueBit }; +}; +} // namespace internal + +template +class Quaternion : public QuaternionBase > { + public: + typedef QuaternionBase > Base; + enum { NeedsAlignment = internal::traits::Alignment > 0 }; + + typedef Scalar_ Scalar; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) + using Base::operator*=; + + typedef typename internal::traits::Coefficients Coefficients; + typedef typename Base::AngleAxisType AngleAxisType; + + /** Default constructor leaving the quaternion uninitialized. */ + EIGEN_DEVICE_FUNC inline Quaternion() {} + + /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from + * its four coefficients \a w, \a x, \a y and \a z. + * + * \warning Note the order of the arguments: the real \a w coefficient first, + * while internally the coefficients are stored in the following order: + * [\c x, \c y, \c z, \c w] + */ + EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) + : m_coeffs(x, y, z, w) {} + + /** Constructs and initializes a quaternion from its real part as a scalar, + * and its imaginary part as a 3-vector [\c x, \c y, \c z] + */ + template + EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Eigen::MatrixBase& vec) + : m_coeffs(vec.x(), vec.y(), vec.z(), w) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + } + + /** Constructs and initialize a quaternion from the array data */ + EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {} + + /** Copy constructor */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase& other) { + this->Base::operator=(other); + } + + /** Constructs and initializes a quaternion from the angle-axis \a aa */ + EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } + + /** Constructs and initializes a quaternion from either: + * - a rotation matrix expression, + * - a 4D vector expression representing quaternion coefficients in the order [\c x, \c y, \c z, \c w]. + */ + template + EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase& other) { + *this = other; + } + + /** Explicit copy constructor with scalar conversion */ + template + EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion& other) { + m_coeffs = other.coeffs().template cast(); + } + + // We define a copy constructor, which means we don't get an implicit move constructor or assignment operator. + /** Default move constructor */ + EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) + EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + : m_coeffs(std::move(other.coeffs())) {} + + /** Default move assignment operator */ + EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) + EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { + m_coeffs = std::move(other.coeffs()); + return *this; + } + + EIGEN_DEVICE_FUNC static Quaternion UnitRandom(); + + template + EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase& a, const MatrixBase& b); + + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment)) + +#ifdef EIGEN_QUATERNION_PLUGIN +#include EIGEN_QUATERNION_PLUGIN +#endif + + protected: + Coefficients m_coeffs; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + EIGEN_STATIC_ASSERT((Options_ & DontAlign) == Options_, INVALID_MATRIX_TEMPLATE_PARAMETERS) +#endif +}; + +/** \ingroup Geometry_Module + * single precision quaternion type */ +typedef Quaternion Quaternionf; +/** \ingroup Geometry_Module + * double precision quaternion type */ +typedef Quaternion Quaterniond; + +/*************************************************************************** + * Specialization of Map> + ***************************************************************************/ + +namespace internal { +template +struct traits, Options_> > + : traits > { + typedef Map, Options_> Coefficients; +}; +} // namespace internal + +namespace internal { +template +struct traits, Options_> > + : traits > { + typedef Map, Options_> Coefficients; + typedef traits > TraitsBase; + enum { Flags = TraitsBase::Flags & ~LvalueBit }; +}; +} // namespace internal + +/** \ingroup Geometry_Module + * \brief Quaternion expression mapping a constant memory buffer + * + * \tparam Scalar_ the type of the Quaternion coefficients + * \tparam Options_ see class Map + * + * This is a specialization of class Map for Quaternion. This class allows to view + * a 4 scalar memory buffer as an Eigen's Quaternion object. + * + * \sa class Map, class Quaternion, class QuaternionBase + */ +template +class Map, Options_> : public QuaternionBase, Options_> > { + public: + typedef QuaternionBase, Options_> > Base; + + typedef Scalar_ Scalar; + typedef typename internal::traits::Coefficients Coefficients; + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped Quaternion object from the pointer \a coeffs + * + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: + * \code *coeffs == {x, y, z, w} \endcode + * + * If the template parameter Options_ is set to #Aligned, then the pointer coeffs must be aligned. */ + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} + + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } + + protected: + const Coefficients m_coeffs; +}; + +/** \ingroup Geometry_Module + * \brief Expression of a quaternion from a memory buffer + * + * \tparam Scalar_ the type of the Quaternion coefficients + * \tparam Options_ see class Map + * + * This is a specialization of class Map for Quaternion. This class allows to view + * a 4 scalar memory buffer as an Eigen's Quaternion object. + * + * \sa class Map, class Quaternion, class QuaternionBase + */ +template +class Map, Options_> : public QuaternionBase, Options_> > { + public: + typedef QuaternionBase, Options_> > Base; + + typedef Scalar_ Scalar; + typedef typename internal::traits::Coefficients Coefficients; + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped Quaternion object from the pointer \a coeffs + * + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: + * \code *coeffs == {x, y, z, w} \endcode + * + * If the template parameter Options_ is set to #Aligned, then the pointer coeffs must be aligned. */ + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} + + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } + + protected: + Coefficients m_coeffs; +}; + +/** \ingroup Geometry_Module + * Map an unaligned array of single precision scalars as a quaternion */ +typedef Map, 0> QuaternionMapf; +/** \ingroup Geometry_Module + * Map an unaligned array of double precision scalars as a quaternion */ +typedef Map, 0> QuaternionMapd; +/** \ingroup Geometry_Module + * Map a 16-byte aligned array of single precision scalars as a quaternion */ +typedef Map, Aligned> QuaternionMapAlignedf; +/** \ingroup Geometry_Module + * Map a 16-byte aligned array of double precision scalars as a quaternion */ +typedef Map, Aligned> QuaternionMapAlignedd; + +/*************************************************************************** + * Implementation of QuaternionBase methods + ***************************************************************************/ + +// Generic Quaternion * Quaternion product +// This product can be specialized for a given architecture via the Arch template argument. +namespace internal { +template +struct quat_product { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion run(const QuaternionBase& a, + const QuaternionBase& b) { + return Quaternion(a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), + a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), + a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), + a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()); + } +}; +} // namespace internal + +/** \returns the concatenation of two rotations as a quaternion-quaternion product */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion::Scalar> +QuaternionBase::operator*(const QuaternionBase& other) const { + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + return internal::quat_product::Scalar>::run(*this, other); +} + +/** \sa operator*(Quaternion) */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase::operator*=( + const QuaternionBase& other) { + derived() = derived() * other.derived(); + return derived(); +} + +/** Rotation of a vector by a quaternion. + * \remarks If the quaternion is used to rotate several points (>1) + * then it is much more efficient to first convert it to a 3x3 Matrix. + * Comparison of the operation cost for n transformations: + * - Quaternion2: 30n + * - Via a Matrix3: 24 + 15n + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase::Vector3 +QuaternionBase::_transformVector(const Vector3& v) const { + // Note that this algorithm comes from the optimization by hand + // of the conversion to a Matrix followed by a Matrix/Vector product. + // It appears to be much faster than the common algorithm found + // in the literature (30 versus 39 flops). It also requires two + // Vector3 as temporaries. + Vector3 uv = this->vec().cross(v); + uv += uv; + return v + this->w() * uv + this->vec().cross(uv); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase& QuaternionBase::operator=( + const QuaternionBase& other) { + coeffs() = other.coeffs(); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase::operator=( + const QuaternionBase& other) { + coeffs() = other.coeffs(); + return derived(); +} + +/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase::operator=(const AngleAxisType& aa) { + EIGEN_USING_STD(cos) + EIGEN_USING_STD(sin) + Scalar ha = Scalar(0.5) * aa.angle(); // Scalar(0.5) to suppress precision loss warnings + this->w() = cos(ha); + this->vec() = sin(ha) * aa.axis(); + return derived(); +} + +/** Set \c *this from the expression \a xpr: + * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion + * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix + * and \a xpr is converted to a quaternion + */ + +template +template +EIGEN_DEVICE_FUNC inline Derived& QuaternionBase::operator=(const MatrixBase& xpr) { + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + internal::quaternionbase_assign_impl::run(*this, xpr.derived()); + return derived(); +} + +/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to + * be normalized, otherwise the result is undefined. + */ +template +EIGEN_DEVICE_FUNC inline typename QuaternionBase::Matrix3 QuaternionBase::toRotationMatrix( + void) const { + // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) + // if not inlined then the cost of the return by value is huge ~ +35%, + // however, not inlining this function is an order of magnitude slower, so + // it has to be inlined, and so the return by value is not an issue + Matrix3 res; + + const Scalar tx = Scalar(2) * this->x(); + const Scalar ty = Scalar(2) * this->y(); + const Scalar tz = Scalar(2) * this->z(); + const Scalar twx = tx * this->w(); + const Scalar twy = ty * this->w(); + const Scalar twz = tz * this->w(); + const Scalar txx = tx * this->x(); + const Scalar txy = ty * this->x(); + const Scalar txz = tz * this->x(); + const Scalar tyy = ty * this->y(); + const Scalar tyz = tz * this->y(); + const Scalar tzz = tz * this->z(); + + res.coeffRef(0, 0) = Scalar(1) - (tyy + tzz); + res.coeffRef(0, 1) = txy - twz; + res.coeffRef(0, 2) = txz + twy; + res.coeffRef(1, 0) = txy + twz; + res.coeffRef(1, 1) = Scalar(1) - (txx + tzz); + res.coeffRef(1, 2) = tyz - twx; + res.coeffRef(2, 0) = txz - twy; + res.coeffRef(2, 1) = tyz + twx; + res.coeffRef(2, 2) = Scalar(1) - (txx + tyy); + + return res; +} + +/** Sets \c *this to be a quaternion representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns a reference to \c *this. + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ +template +template +EIGEN_DEVICE_FUNC inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase& a, + const MatrixBase& b) { + EIGEN_USING_STD(sqrt) + Vector3 v0 = a.normalized(); + Vector3 v1 = b.normalized(); + Scalar c = v1.dot(v0); + + // if dot == -1, vectors are nearly opposites + // => accurately compute the rotation axis by computing the + // intersection of the two planes. This is done by solving: + // x^T v0 = 0 + // x^T v1 = 0 + // under the constraint: + // ||x|| = 1 + // which yields a singular value problem + if (c < Scalar(-1) + NumTraits::dummy_precision()) { + c = numext::maxi(c, Scalar(-1)); + Matrix m; + m << v0.transpose(), v1.transpose(); + JacobiSVD, ComputeFullV> svd(m); + Vector3 axis = svd.matrixV().col(2); + + Scalar w2 = (Scalar(1) + c) * Scalar(0.5); + this->w() = sqrt(w2); + this->vec() = axis * sqrt(Scalar(1) - w2); + return derived(); + } + Vector3 axis = v0.cross(v1); + Scalar s = sqrt((Scalar(1) + c) * Scalar(2)); + Scalar invs = Scalar(1) / s; + this->vec() = axis * invs; + this->w() = s * Scalar(0.5); + + return derived(); +} + +/** \returns a random unit quaternion following a uniform distribution law on SO(3) + * + * \note The implementation is based on http://planning.cs.uiuc.edu/node198.html + */ +template +EIGEN_DEVICE_FUNC Quaternion Quaternion::UnitRandom() { + EIGEN_USING_STD(sqrt) + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) + const Scalar u1 = internal::random(0, 1), u2 = internal::random(0, 2 * EIGEN_PI), + u3 = internal::random(0, 2 * EIGEN_PI); + const Scalar a = sqrt(Scalar(1) - u1), b = sqrt(u1); + return Quaternion(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)); +} + +/** Returns a quaternion representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns resulting quaternion + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ +template +template +EIGEN_DEVICE_FUNC Quaternion Quaternion::FromTwoVectors( + const MatrixBase& a, const MatrixBase& b) { + Quaternion quat; + quat.setFromTwoVectors(a, b); + return quat; +} + +/** \returns the multiplicative inverse of \c *this + * Note that in most cases, i.e., if you simply want the opposite rotation, + * and/or the quaternion is normalized, then it is enough to use the conjugate. + * + * \sa QuaternionBase::conjugate() + */ +template +EIGEN_DEVICE_FUNC inline Quaternion::Scalar> QuaternionBase::inverse() + const { + // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? + Scalar n2 = this->squaredNorm(); + if (n2 > Scalar(0)) + return Quaternion(conjugate().coeffs() / n2); + else { + // return an invalid result to flag the error + return Quaternion(Coefficients::Zero()); + } +} + +// Generic conjugate of a Quaternion +namespace internal { +template +struct quat_conj { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion run(const QuaternionBase& q) { + return Quaternion(q.w(), -q.x(), -q.y(), -q.z()); + } +}; +} // namespace internal + +/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse + * if the quaternion is normalized. + * The conjugate of a quaternion represents the opposite rotation. + * + * \sa Quaternion2::inverse() + */ +template +EIGEN_DEVICE_FUNC inline Quaternion::Scalar> QuaternionBase::conjugate() + const { + return internal::quat_conj::Scalar>::run(*this); +} + +/** \returns the angle (in radian) between two rotations + * \sa dot() + */ +template +template +EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar QuaternionBase::angularDistance( + const QuaternionBase& other) const { + EIGEN_USING_STD(atan2) + Quaternion d = (*this) * other.conjugate(); + return Scalar(2) * atan2(d.vec().norm(), numext::abs(d.w())); +} + +/** \returns the spherical linear interpolation between the two quaternions + * \c *this and \a other at the parameter \a t in [0;1]. + * + * This represents an interpolation for a constant motion between \c *this and \a other, + * see also http://en.wikipedia.org/wiki/Slerp. + */ +template +template +EIGEN_DEVICE_FUNC Quaternion::Scalar> QuaternionBase::slerp( + const Scalar& t, const QuaternionBase& other) const { + EIGEN_USING_STD(acos) + EIGEN_USING_STD(sin) + const Scalar one = Scalar(1) - NumTraits::epsilon(); + Scalar d = this->dot(other); + Scalar absD = numext::abs(d); + + Scalar scale0; + Scalar scale1; + + if (absD >= one) { + scale0 = Scalar(1) - t; + scale1 = t; + } else { + // theta is the angle between the 2 quaternions + Scalar theta = acos(absD); + Scalar sinTheta = sin(theta); + + scale0 = sin((Scalar(1) - t) * theta) / sinTheta; + scale1 = sin((t * theta)) / sinTheta; + } + if (d < Scalar(0)) scale1 = -scale1; + + return Quaternion(scale0 * coeffs() + scale1 * other.coeffs()); +} + +namespace internal { + +// set from a rotation matrix +template +struct quaternionbase_assign_impl { + typedef typename Other::Scalar Scalar; + template + EIGEN_DEVICE_FUNC static inline void run(QuaternionBase& q, const Other& a_mat) { + const typename internal::nested_eval::type mat(a_mat); + EIGEN_USING_STD(sqrt) + // This algorithm comes from "Quaternion Calculus and Fast Animation", + // Ken Shoemake, 1987 SIGGRAPH course notes + Scalar t = mat.trace(); + if (t > Scalar(0)) { + t = sqrt(t + Scalar(1.0)); + q.w() = Scalar(0.5) * t; + t = Scalar(0.5) / t; + q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t; + q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t; + q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t; + } else { + Index i = 0; + if (mat.coeff(1, 1) > mat.coeff(0, 0)) i = 1; + if (mat.coeff(2, 2) > mat.coeff(i, i)) i = 2; + Index j = (i + 1) % 3; + Index k = (j + 1) % 3; + + t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0)); + q.coeffs().coeffRef(i) = Scalar(0.5) * t; + t = Scalar(0.5) / t; + q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t; + q.coeffs().coeffRef(j) = (mat.coeff(j, i) + mat.coeff(i, j)) * t; + q.coeffs().coeffRef(k) = (mat.coeff(k, i) + mat.coeff(i, k)) * t; + } + } +}; + +// set from a vector of coefficients assumed to be a quaternion +template +struct quaternionbase_assign_impl { + typedef typename Other::Scalar Scalar; + template + EIGEN_DEVICE_FUNC static inline void run(QuaternionBase& q, const Other& vec) { + q.coeffs() = vec; + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_QUATERNION_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Rotation2D.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Rotation2D.h new file mode 100644 index 00000000000..59180253a51 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Rotation2D.h @@ -0,0 +1,201 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_ROTATION2D_H +#define EIGEN_ROTATION2D_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Rotation2D + * + * \brief Represents a rotation/orientation in a 2 dimensional space. + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients + * + * This class is equivalent to a single scalar representing a counter clock wise rotation + * as a single angle in radian. It provides some additional features such as the automatic + * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar + * interface to Quaternion in order to facilitate the writing of generic algorithms + * dealing with rotations. + * + * \sa class Quaternion, class Transform + */ + +namespace internal { + +template +struct traits > { + typedef Scalar_ Scalar; +}; +} // end namespace internal + +template +class Rotation2D : public RotationBase, 2> { + typedef RotationBase, 2> Base; + + public: + using Base::operator*; + + enum { Dim = 2 }; + /** the scalar type of the coefficients */ + typedef Scalar_ Scalar; + typedef Matrix Vector2; + typedef Matrix Matrix2; + + protected: + Scalar m_angle; + + public: + /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ + EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {} + + /** Default constructor without initialization. The represented rotation is undefined. */ + EIGEN_DEVICE_FUNC Rotation2D() {} + + /** Construct a 2D rotation from a 2x2 rotation matrix \a mat. + * + * \sa fromRotationMatrix() + */ + template + EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase& m) { + fromRotationMatrix(m.derived()); + } + + /** \returns the rotation angle */ + EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; } + + /** \returns a read-write reference to the rotation angle */ + EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; } + + /** \returns the rotation angle in [0,2pi] */ + EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const { + Scalar tmp = numext::fmod(m_angle, Scalar(2 * EIGEN_PI)); + return tmp < Scalar(0) ? tmp + Scalar(2 * EIGEN_PI) : tmp; + } + + /** \returns the rotation angle in [-pi,pi] */ + EIGEN_DEVICE_FUNC inline Scalar smallestAngle() const { + Scalar tmp = numext::fmod(m_angle, Scalar(2 * EIGEN_PI)); + if (tmp > Scalar(EIGEN_PI)) + tmp -= Scalar(2 * EIGEN_PI); + else if (tmp < -Scalar(EIGEN_PI)) + tmp += Scalar(2 * EIGEN_PI); + return tmp; + } + + /** \returns the inverse rotation */ + EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const { + return Rotation2D(m_angle + other.m_angle); + } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other) { + m_angle += other.m_angle; + return *this; + } + + /** Applies the rotation to a 2D vector */ + EIGEN_DEVICE_FUNC Vector2 operator*(const Vector2& vec) const { return toRotationMatrix() * vec; } + + template + EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase& m); + EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const; + + /** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle from the rotation matrix. + * + * This method is an alias for fromRotationMatrix() + * + * \sa fromRotationMatrix() + */ + template + EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase& m) { + return fromRotationMatrix(m.derived()); + } + + /** \returns the spherical interpolation between \c *this and \a other using + * parameter \a t. It is in fact equivalent to a linear interpolation. + */ + EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const { + Scalar dist = Rotation2D(other.m_angle - m_angle).smallestAngle(); + return Rotation2D(m_angle + dist * t); + } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() + const { + return typename internal::cast_return_type >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D& other) { + m_angle = Scalar(other.angle()); + } + + EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits::Real& prec = + NumTraits::dummy_precision()) const { + return internal::isApprox(m_angle, other.m_angle, prec); + } +}; + +/** \ingroup Geometry_Module + * single precision 2D rotation type */ +typedef Rotation2D Rotation2Df; +/** \ingroup Geometry_Module + * double precision 2D rotation type */ +typedef Rotation2D Rotation2Dd; + +/** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle + * from the rotation matrix. + */ +template +template +EIGEN_DEVICE_FUNC Rotation2D& Rotation2D::fromRotationMatrix(const MatrixBase& mat) { + EIGEN_USING_STD(atan2) + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 2 && Derived::ColsAtCompileTime == 2, + YOU_MADE_A_PROGRAMMING_MISTAKE) + m_angle = atan2(mat.coeff(1, 0), mat.coeff(0, 0)); + return *this; +} + +/** Constructs and \returns an equivalent 2x2 rotation matrix. + */ +template +typename Rotation2D::Matrix2 EIGEN_DEVICE_FUNC Rotation2D::toRotationMatrix(void) const { + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) + Scalar sinA = sin(m_angle); + Scalar cosA = cos(m_angle); + return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); +} + +} // end namespace Eigen + +#endif // EIGEN_ROTATION2D_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/RotationBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/RotationBase.h new file mode 100644 index 00000000000..3a3a3e31c68 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/RotationBase.h @@ -0,0 +1,209 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_ROTATIONBASE_H +#define EIGEN_ROTATIONBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +// forward declaration +namespace internal { +template +struct rotation_base_generic_product_selector; +} + +/** \class RotationBase + * + * \brief Common base class for compact rotation representations + * + * \tparam Derived is the derived type, i.e., a rotation type + * \tparam Dim_ the dimension of the space + */ +template +class RotationBase { + public: + enum { Dim = Dim_ }; + /** the scalar type of the coefficients */ + typedef typename internal::traits::Scalar Scalar; + + /** corresponding linear transformation matrix type */ + typedef Matrix RotationMatrixType; + typedef Matrix VectorType; + + public: + EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast(this); } + EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast(this); } + + /** \returns an equivalent rotation matrix */ + EIGEN_DEVICE_FUNC inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } + + /** \returns an equivalent rotation matrix + * This function is added to be conform with the Transform class' naming scheme. + */ + EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } + + /** \returns the inverse rotation */ + EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); } + + /** \returns the concatenation of the rotation \c *this with a translation \a t */ + EIGEN_DEVICE_FUNC inline Transform operator*(const Translation& t) const { + return Transform(*this) * t; + } + + /** \returns the concatenation of the rotation \c *this with a uniform scaling \a s */ + EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const UniformScaling& s) const { + return toRotationMatrix() * s.factor(); + } + + /** \returns the concatenation of the rotation \c *this with a generic expression \a e + * \a e can be: + * - a DimxDim linear transformation matrix + * - a DimxDim diagonal matrix (axis aligned scaling) + * - a vector of size Dim + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename internal::rotation_base_generic_product_selector::ReturnType + operator*(const EigenBase& e) const { + return internal::rotation_base_generic_product_selector::run(derived(), e.derived()); + } + + /** \returns the concatenation of a linear transformation \a l with the rotation \a r */ + template + friend EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase& l, const Derived& r) { + return l.derived() * r.toRotationMatrix(); + } + + /** \returns the concatenation of a scaling \a l with the rotation \a r */ + EIGEN_DEVICE_FUNC friend inline Transform operator*(const DiagonalMatrix& l, + const Derived& r) { + Transform res(r); + res.linear().applyOnTheLeft(l); + return res; + } + + /** \returns the concatenation of the rotation \c *this with a transformation \a t */ + template + EIGEN_DEVICE_FUNC inline Transform operator*( + const Transform& t) const { + return toRotationMatrix() * t; + } + + template + EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const { + return toRotationMatrix() * v; + } +}; + +namespace internal { + +// implementation of the generic product rotation * matrix +template +struct rotation_base_generic_product_selector { + enum { Dim = RotationDerived::Dim }; + typedef Matrix ReturnType; + EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m) { + return r.toRotationMatrix() * m; + } +}; + +template +struct rotation_base_generic_product_selector, false> { + typedef Transform ReturnType; + EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, + const DiagonalMatrix& m) { + ReturnType res(r); + res.linear() *= m; + return res; + } +}; + +template +struct rotation_base_generic_product_selector { + enum { Dim = RotationDerived::Dim }; + typedef Matrix ReturnType; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) { + return r._transformVector(v); + } +}; + +} // end namespace internal + +/** \geometry_module + * + * \brief Constructs a Dim x Dim rotation matrix from the rotation \a r + */ +template +template +EIGEN_DEVICE_FUNC Matrix::Matrix( + const RotationBase& r) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim)) + *this = r.toRotationMatrix(); +} + +/** \geometry_module + * + * \brief Set a Dim x Dim rotation matrix from the rotation \a r + */ +template +template +EIGEN_DEVICE_FUNC Matrix& +Matrix::operator=( + const RotationBase& r) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim)) + return *this = r.toRotationMatrix(); +} + +namespace internal { + +/** \internal + * + * Helper function to return an arbitrary rotation object to a rotation matrix. + * + * \tparam Scalar the numeric type of the matrix coefficients + * \tparam Dim the dimension of the current space + * + * It returns a Dim x Dim fixed size matrix. + * + * Default specializations are provided for: + * - any scalar type (2D), + * - any matrix expression, + * - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D) + * + * Currently toRotationMatrix is only used by Transform. + * + * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis + */ +template +EIGEN_DEVICE_FUNC static inline Matrix toRotationMatrix(const Scalar& s) { + EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + return Rotation2D(s).toRotationMatrix(); +} + +template +EIGEN_DEVICE_FUNC static inline Matrix toRotationMatrix(const RotationBase& r) { + return r.toRotationMatrix(); +} + +template +EIGEN_DEVICE_FUNC static inline const MatrixBase& toRotationMatrix(const MatrixBase& mat) { + EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime == Dim && OtherDerived::ColsAtCompileTime == Dim, + YOU_MADE_A_PROGRAMMING_MISTAKE) + return mat; +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_ROTATIONBASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Scaling.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Scaling.h new file mode 100644 index 00000000000..a0604cee16a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Scaling.h @@ -0,0 +1,195 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_SCALING_H +#define EIGEN_SCALING_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class UniformScaling + * + * \brief Represents a generic uniform scaling transformation + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients. + * + * This class represent a uniform scaling transformation. It is the return + * type of Scaling(Scalar), and most of the time this is the only way it + * is used. In particular, this class is not aimed to be used to store a scaling transformation, + * but rather to make easier the constructions and updates of Transform objects. + * + * To represent an axis aligned scaling, use the DiagonalMatrix class. + * + * \sa Scaling(), class DiagonalMatrix, MatrixBase::asDiagonal(), class Translation, class Transform + */ + +namespace internal { +// This helper helps nvcc+MSVC to properly parse this file. +// See bug 1412. +template +struct uniformscaling_times_affine_returntype { + enum { NewMode = int(Mode) == int(Isometry) ? Affine : Mode }; + typedef Transform type; +}; +} // namespace internal + +template +class UniformScaling { + public: + /** the scalar type of the coefficients */ + typedef Scalar_ Scalar; + + protected: + Scalar m_factor; + + public: + /** Default constructor without initialization. */ + UniformScaling() {} + /** Constructs and initialize a uniform scaling transformation */ + explicit inline UniformScaling(const Scalar& s) : m_factor(s) {} + + inline const Scalar& factor() const { return m_factor; } + inline Scalar& factor() { return m_factor; } + + /** Concatenates two uniform scaling */ + inline UniformScaling operator*(const UniformScaling& other) const { + return UniformScaling(m_factor * other.factor()); + } + + /** Concatenates a uniform scaling and a translation */ + template + inline Transform operator*(const Translation& t) const; + + /** Concatenates a uniform scaling and an affine transformation */ + template + inline typename internal::uniformscaling_times_affine_returntype::type operator*( + const Transform& t) const { + typename internal::uniformscaling_times_affine_returntype::type res = t; + res.prescale(factor()); + return res; + } + + /** Concatenates a uniform scaling and a linear transformation matrix */ + // TODO returns an expression + template + inline typename Eigen::internal::plain_matrix_type::type operator*(const MatrixBase& other) const { + return other * m_factor; + } + + template + inline Matrix operator*(const RotationBase& r) const { + return r.toRotationMatrix() * m_factor; + } + + /** \returns the inverse scaling */ + inline UniformScaling inverse() const { return UniformScaling(Scalar(1) / m_factor); } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + inline UniformScaling cast() const { + return UniformScaling(NewScalarType(m_factor)); + } + + /** Copy constructor with scalar type conversion */ + template + inline explicit UniformScaling(const UniformScaling& other) { + m_factor = Scalar(other.factor()); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + bool isApprox(const UniformScaling& other, + const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const { + return internal::isApprox(m_factor, other.factor(), prec); + } +}; + +/** \addtogroup Geometry_Module */ +//@{ + +/** Concatenates a linear transformation matrix and a uniform scaling + * \relates UniformScaling + */ +// NOTE this operator is defined in MatrixBase and not as a friend function +// of UniformScaling to fix an internal crash of Intel's ICC +template +EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived, Scalar, product) +operator*(const MatrixBase& matrix, const UniformScaling& s) { + return matrix.derived() * s.factor(); +} + +/** Constructs a uniform scaling from scale factor \a s */ +inline UniformScaling Scaling(float s) { return UniformScaling(s); } +/** Constructs a uniform scaling from scale factor \a s */ +inline UniformScaling Scaling(double s) { return UniformScaling(s); } +/** Constructs a uniform scaling from scale factor \a s */ +template +inline UniformScaling > Scaling(const std::complex& s) { + return UniformScaling >(s); +} + +/** Constructs a 2D axis aligned scaling */ +template +inline DiagonalMatrix Scaling(const Scalar& sx, const Scalar& sy) { + return DiagonalMatrix(sx, sy); +} +/** Constructs a 3D axis aligned scaling */ +template +inline DiagonalMatrix Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz) { + return DiagonalMatrix(sx, sy, sz); +} + +/** Constructs an axis aligned scaling expression from vector expression \a coeffs + * This is an alias for coeffs.asDiagonal() + */ +template +inline const DiagonalWrapper Scaling(const MatrixBase& coeffs) { + return coeffs.asDiagonal(); +} + +/** Constructs an axis aligned scaling expression from vector \a coeffs when passed as an rvalue reference */ +template +inline typename DiagonalWrapper::PlainObject Scaling(MatrixBase&& coeffs) { + return typename DiagonalWrapper::PlainObject(std::move(coeffs.derived())); +} + +/** \deprecated */ +typedef DiagonalMatrix AlignedScaling2f; +/** \deprecated */ +typedef DiagonalMatrix AlignedScaling2d; +/** \deprecated */ +typedef DiagonalMatrix AlignedScaling3f; +/** \deprecated */ +typedef DiagonalMatrix AlignedScaling3d; +//@} + +template +template +inline Transform UniformScaling::operator*(const Translation& t) const { + Transform res; + res.matrix().setZero(); + res.linear().diagonal().fill(factor()); + res.translation() = factor() * t.vector(); + res(Dim, Dim) = Scalar(1); + return res; +} + +} // end namespace Eigen + +#endif // EIGEN_SCALING_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h new file mode 100644 index 00000000000..fd3fc58f46d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h @@ -0,0 +1,1484 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2009 Benoit Jacob +// Copyright (C) 2010 Hauke Heibel +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_TRANSFORM_H +#define EIGEN_TRANSFORM_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct transform_traits { + enum { + Dim = Transform::Dim, + HDim = Transform::HDim, + Mode = Transform::Mode, + IsProjective = (int(Mode) == int(Projective)) + }; +}; + +template ::IsProjective ? 0 + : int(MatrixType::RowsAtCompileTime) == int(transform_traits::HDim) ? 1 + : 2, + int RhsCols = MatrixType::ColsAtCompileTime> +struct transform_right_product_impl; + +template +struct transform_left_product_impl; + +template ::IsProjective || transform_traits::IsProjective> +struct transform_transform_product_impl; + +template +struct transform_construct_from_matrix; + +template +struct transform_take_affine_part; + +template +struct traits > { + typedef Scalar_ Scalar; + typedef Eigen::Index StorageIndex; + typedef Dense StorageKind; + enum { + Dim1 = Dim_ == Dynamic ? Dim_ : Dim_ + 1, + RowsAtCompileTime = Mode_ == Projective ? Dim1 : Dim_, + ColsAtCompileTime = Dim1, + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + Flags = 0 + }; +}; + +template +struct transform_make_affine; + +} // end namespace internal + +/** \geometry_module \ingroup Geometry_Module + * + * \class Transform + * + * \brief Represents an homogeneous transformation in a N dimensional space + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients + * \tparam Dim_ the dimension of the space + * \tparam Mode_ the type of the transformation. Can be: + * - #Affine: the transformation is stored as a (Dim+1)^2 matrix, + * where the last row is assumed to be [0 ... 0 1]. + * - #AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix. + * - #Projective: the transformation is stored as a (Dim+1)^2 matrix + * without any assumption. + * - #Isometry: same as #Affine with the additional assumption that + * the linear part represents a rotation. This assumption is exploited + * to speed up some functions such as inverse() and rotation(). + * \tparam Options_ has the same meaning as in class Matrix. It allows to specify DontAlign and/or RowMajor. + * These Options are passed directly to the underlying matrix type. + * + * The homography is internally represented and stored by a matrix which + * is available through the matrix() method. To understand the behavior of + * this class you have to think a Transform object as its internal + * matrix representation. The chosen convention is right multiply: + * + * \code v' = T * v \endcode + * + * Therefore, an affine transformation matrix M is shaped like this: + * + * \f$ \left( \begin{array}{cc} + * linear & translation\\ + * 0 ... 0 & 1 + * \end{array} \right) \f$ + * + * Note that for a projective transformation the last row can be anything, + * and then the interpretation of different parts might be slightly different. + * + * However, unlike a plain matrix, the Transform class provides many features + * simplifying both its assembly and usage. In particular, it can be composed + * with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix) + * and can be directly used to transform implicit homogeneous vectors. All these + * operations are handled via the operator*. For the composition of transformations, + * its principle consists to first convert the right/left hand sides of the product + * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product. + * Of course, internally, operator* tries to perform the minimal number of operations + * according to the nature of each terms. Likewise, when applying the transform + * to points, the latters are automatically promoted to homogeneous vectors + * before doing the matrix product. The conventions to homogeneous representations + * are performed as follow: + * + * \b Translation t (Dim)x(1): + * \f$ \left( \begin{array}{cc} + * I & t \\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Rotation R (Dim)x(Dim): + * \f$ \left( \begin{array}{cc} + * R & 0\\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Scaling \b DiagonalMatrix S (Dim)x(Dim): + * \f$ \left( \begin{array}{cc} + * S & 0\\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Column \b point v (Dim)x(1): + * \f$ \left( \begin{array}{c} + * v\\ + * 1 + * \end{array} \right) \f$ + * + * \b Set \b of \b column \b points V1...Vn (Dim)x(n): + * \f$ \left( \begin{array}{ccc} + * v_1 & ... & v_n\\ + * 1 & ... & 1 + * \end{array} \right) \f$ + * + * The concatenation of a Transform object with any kind of other transformation + * always returns a Transform object. + * + * A little exception to the "as pure matrix product" rule is the case of the + * transformation of non homogeneous vectors by an affine transformation. In + * that case the last matrix row can be ignored, and the product returns non + * homogeneous vectors. + * + * Since, for instance, a Dim x Dim matrix is interpreted as a linear transformation, + * it is not possible to directly transform Dim vectors stored in a Dim x Dim matrix. + * The solution is either to use a Dim x Dynamic matrix or explicitly request a + * vector transformation by making the vector homogeneous: + * \code + * m' = T * m.colwise().homogeneous(); + * \endcode + * Note that there is zero overhead. + * + * Conversion methods from/to Qt's QMatrix and QTransform are available if the + * preprocessor token EIGEN_QT_SUPPORT is defined. + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_TRANSFORM_PLUGIN. + * + * \sa class Matrix, class Quaternion + */ +template +class Transform { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, + Dim_ == Dynamic ? Dynamic : (Dim_ + 1) * (Dim_ + 1)) + enum { + Mode = Mode_, + Options = Options_, + Dim = Dim_, ///< space dimension in which the transformation holds + HDim = Dim_ + 1, ///< size of a respective homogeneous vector + Rows = int(Mode) == (AffineCompact) ? Dim : HDim + }; + /** the scalar type of the coefficients */ + typedef Scalar_ Scalar; + typedef Eigen::Index StorageIndex; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + /** type of the matrix used to represent the transformation */ + typedef typename internal::make_proper_matrix_type::type MatrixType; + /** constified MatrixType */ + typedef const MatrixType ConstMatrixType; + /** type of the matrix used to represent the linear part of the transformation */ + typedef Matrix LinearMatrixType; + /** type of read/write reference to the linear part of the transformation */ + typedef Block LinearPart; + /** type of read reference to the linear part of the transformation */ + typedef const Block + ConstLinearPart; + /** type of read/write reference to the affine part of the transformation */ + typedef std::conditional_t > AffinePart; + /** type of read reference to the affine part of the transformation */ + typedef std::conditional_t > + ConstAffinePart; + /** type of a vector */ + typedef Matrix VectorType; + /** type of a read/write reference to the translation part of the rotation */ + typedef Block::Flags & RowMajorBit)> TranslationPart; + /** type of a read reference to the translation part of the rotation */ + typedef const Block::Flags & RowMajorBit)> + ConstTranslationPart; + /** corresponding translation type */ + typedef Translation TranslationType; + + // this intermediate enum is needed to avoid an ICE with gcc 3.4 and 4.0 + enum { TransformTimeDiagonalMode = ((Mode == int(Isometry)) ? Affine : int(Mode)) }; + /** The return type of the product between a diagonal matrix and a transform */ + typedef Transform TransformTimeDiagonalReturnType; + + protected: + MatrixType m_matrix; + + public: + /** Default constructor without initialization of the meaningful coefficients. + * If Mode==Affine or Mode==Isometry, then the last row is set to [0 ... 0 1] */ + EIGEN_DEVICE_FUNC inline Transform() { + check_template_params(); + internal::transform_make_affine<(int(Mode) == Affine || int(Mode) == Isometry) ? Affine : AffineCompact>::run( + m_matrix); + } + + EIGEN_DEVICE_FUNC inline explicit Transform(const TranslationType& t) { + check_template_params(); + *this = t; + } + EIGEN_DEVICE_FUNC inline explicit Transform(const UniformScaling& s) { + check_template_params(); + *this = s; + } + template + EIGEN_DEVICE_FUNC inline explicit Transform(const RotationBase& r) { + check_template_params(); + *this = r; + } + + typedef internal::transform_take_affine_part take_affine_part; + + /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */ + template + EIGEN_DEVICE_FUNC inline explicit Transform(const EigenBase& other) { + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY); + + check_template_params(); + internal::transform_construct_from_matrix::run(this, other.derived()); + } + + /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */ + template + EIGEN_DEVICE_FUNC inline Transform& operator=(const EigenBase& other) { + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY); + + internal::transform_construct_from_matrix::run(this, other.derived()); + return *this; + } + + template + EIGEN_DEVICE_FUNC inline Transform(const Transform& other) { + check_template_params(); + // only the options change, we can directly copy the matrices + m_matrix = other.matrix(); + } + + template + EIGEN_DEVICE_FUNC inline Transform(const Transform& other) { + check_template_params(); + // prevent conversions as: + // Affine | AffineCompact | Isometry = Projective + EIGEN_STATIC_ASSERT(internal::check_implication(OtherMode == int(Projective), Mode == int(Projective)), + YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION) + + // prevent conversions as: + // Isometry = Affine | AffineCompact + EIGEN_STATIC_ASSERT( + internal::check_implication(OtherMode == int(Affine) || OtherMode == int(AffineCompact), Mode != int(Isometry)), + YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION) + + enum { + ModeIsAffineCompact = Mode == int(AffineCompact), + OtherModeIsAffineCompact = OtherMode == int(AffineCompact) + }; + + if (EIGEN_CONST_CONDITIONAL(ModeIsAffineCompact == OtherModeIsAffineCompact)) { + // We need the block expression because the code is compiled for all + // combinations of transformations and will trigger a compile time error + // if one tries to assign the matrices directly + m_matrix.template block(0, 0) = other.matrix().template block(0, 0); + makeAffine(); + } else if (EIGEN_CONST_CONDITIONAL(OtherModeIsAffineCompact)) { + typedef typename Transform::MatrixType OtherMatrixType; + internal::transform_construct_from_matrix::run(this, other.matrix()); + } else { + // here we know that Mode == AffineCompact and OtherMode != AffineCompact. + // if OtherMode were Projective, the static assert above would already have caught it. + // So the only possibility is that OtherMode == Affine + linear() = other.linear(); + translation() = other.translation(); + } + } + + template + EIGEN_DEVICE_FUNC Transform(const ReturnByValue& other) { + check_template_params(); + other.evalTo(*this); + } + + template + EIGEN_DEVICE_FUNC Transform& operator=(const ReturnByValue& other) { + other.evalTo(*this); + return *this; + } + +#ifdef EIGEN_QT_SUPPORT +#if (QT_VERSION < QT_VERSION_CHECK(6, 0, 0)) + inline Transform(const QMatrix& other); + inline Transform& operator=(const QMatrix& other); + inline QMatrix toQMatrix(void) const; +#endif + inline Transform(const QTransform& other); + inline Transform& operator=(const QTransform& other); + inline QTransform toQTransform(void) const; +#endif + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { + return int(Mode) == int(Projective) ? m_matrix.cols() : (m_matrix.cols() - 1); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + /** shortcut for m_matrix(row,col); + * \sa MatrixBase::operator(Index,Index) const */ + EIGEN_DEVICE_FUNC inline Scalar operator()(Index row, Index col) const { return m_matrix(row, col); } + /** shortcut for m_matrix(row,col); + * \sa MatrixBase::operator(Index,Index) */ + EIGEN_DEVICE_FUNC inline Scalar& operator()(Index row, Index col) { return m_matrix(row, col); } + + /** \returns a read-only expression of the transformation matrix */ + EIGEN_DEVICE_FUNC inline const MatrixType& matrix() const { return m_matrix; } + /** \returns a writable expression of the transformation matrix */ + EIGEN_DEVICE_FUNC inline MatrixType& matrix() { return m_matrix; } + + /** \returns a read-only expression of the linear part of the transformation */ + EIGEN_DEVICE_FUNC inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix, 0, 0); } + /** \returns a writable expression of the linear part of the transformation */ + EIGEN_DEVICE_FUNC inline LinearPart linear() { return LinearPart(m_matrix, 0, 0); } + + /** \returns a read-only expression of the Dim x HDim affine part of the transformation */ + EIGEN_DEVICE_FUNC inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); } + /** \returns a writable expression of the Dim x HDim affine part of the transformation */ + EIGEN_DEVICE_FUNC inline AffinePart affine() { return take_affine_part::run(m_matrix); } + + /** \returns a read-only expression of the translation vector of the transformation */ + EIGEN_DEVICE_FUNC inline ConstTranslationPart translation() const { return ConstTranslationPart(m_matrix, 0, Dim); } + /** \returns a writable expression of the translation vector of the transformation */ + EIGEN_DEVICE_FUNC inline TranslationPart translation() { return TranslationPart(m_matrix, 0, Dim); } + + /** \returns an expression of the product between the transform \c *this and a matrix expression \a other. + * + * The right-hand-side \a other can be either: + * \li an homogeneous vector of size Dim+1, + * \li a set of homogeneous vectors of size Dim+1 x N, + * \li a transformation matrix of size Dim+1 x Dim+1. + * + * Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be: + * \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode), + * \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + + * this->translation()\endcode), + * + * In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other. + * + * If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> + * type, or do your own cooking. + * + * Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only: + * \code + * Affine3f A; + * Vector3f v1, v2; + * v2 = A.linear() * v1; + * \endcode + * + */ + // note: this function is defined here because some compilers cannot find the respective declaration + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl::ResultType + operator*(const EigenBase& other) const { + return internal::transform_right_product_impl::run(*this, other.derived()); + } + + /** \returns the product expression of a transformation matrix \a a times a transform \a b + * + * The left hand side \a other can be either: + * \li a linear transformation matrix of size Dim x Dim, + * \li an affine transformation matrix of size Dim x Dim+1, + * \li a general transformation matrix of size Dim+1 x Dim+1. + */ + template + friend EIGEN_DEVICE_FUNC inline const typename internal::transform_left_product_impl::ResultType + operator*(const EigenBase& a, const Transform& b) { + return internal::transform_left_product_impl::run(a.derived(), b); + } + + /** \returns The product expression of a transform \a a times a diagonal matrix \a b + * + * The rhs diagonal matrix is interpreted as an affine scaling transformation. The + * product results in a Transform of the same type (mode) as the lhs only if the lhs + * mode is no isometry. In that case, the returned transform is an affinity. + */ + template + EIGEN_DEVICE_FUNC inline const TransformTimeDiagonalReturnType operator*( + const DiagonalBase& b) const { + TransformTimeDiagonalReturnType res(*this); + res.linearExt() *= b; + return res; + } + + /** \returns The product expression of a diagonal matrix \a a times a transform \a b + * + * The lhs diagonal matrix is interpreted as an affine scaling transformation. The + * product results in a Transform of the same type (mode) as the lhs only if the lhs + * mode is no isometry. In that case, the returned transform is an affinity. + */ + template + EIGEN_DEVICE_FUNC friend inline TransformTimeDiagonalReturnType operator*(const DiagonalBase& a, + const Transform& b) { + TransformTimeDiagonalReturnType res; + res.linear().noalias() = a * b.linear(); + res.translation().noalias() = a * b.translation(); + if (EIGEN_CONST_CONDITIONAL(Mode != int(AffineCompact))) res.matrix().row(Dim) = b.matrix().row(Dim); + return res; + } + + template + EIGEN_DEVICE_FUNC inline Transform& operator*=(const EigenBase& other) { + return *this = *this * other; + } + + /** Concatenates two transformations */ + EIGEN_DEVICE_FUNC inline const Transform operator*(const Transform& other) const { + return internal::transform_transform_product_impl::run(*this, other); + } + +#if EIGEN_COMP_ICC + private: + // this intermediate structure permits to workaround a bug in ICC 11: + // error: template instantiation resulted in unexpected function type of "Eigen::Transform + // (const Eigen::Transform &) const" + // (the meaning of a name may have changed since the template declaration -- the type of the template is: + // "Eigen::internal::transform_transform_product_impl, + // Eigen::Transform, >::ResultType (const Eigen::Transform &) const") + // + template + struct icc_11_workaround { + typedef internal::transform_transform_product_impl > + ProductType; + typedef typename ProductType::ResultType ResultType; + }; + + public: + /** Concatenates two different transformations */ + template + inline typename icc_11_workaround::ResultType operator*( + const Transform& other) const { + typedef typename icc_11_workaround::ProductType ProductType; + return ProductType::run(*this, other); + } +#else + /** Concatenates two different transformations */ + template + EIGEN_DEVICE_FUNC inline + typename internal::transform_transform_product_impl >::ResultType + operator*(const Transform& other) const { + return internal::transform_transform_product_impl >::run( + *this, other); + } +#endif + + /** \sa MatrixBase::setIdentity() */ + EIGEN_DEVICE_FUNC void setIdentity() { m_matrix.setIdentity(); } + + /** + * \brief Returns an identity transformation. + * \todo In the future this function should be returning a Transform expression. + */ + EIGEN_DEVICE_FUNC static const Transform Identity() { return Transform(MatrixType::Identity()); } + + template + EIGEN_DEVICE_FUNC inline Transform& scale(const MatrixBase& other); + + template + EIGEN_DEVICE_FUNC inline Transform& prescale(const MatrixBase& other); + + EIGEN_DEVICE_FUNC inline Transform& scale(const Scalar& s); + EIGEN_DEVICE_FUNC inline Transform& prescale(const Scalar& s); + + template + EIGEN_DEVICE_FUNC inline Transform& translate(const MatrixBase& other); + + template + EIGEN_DEVICE_FUNC inline Transform& pretranslate(const MatrixBase& other); + + template + EIGEN_DEVICE_FUNC inline Transform& rotate(const RotationType& rotation); + + template + EIGEN_DEVICE_FUNC inline Transform& prerotate(const RotationType& rotation); + + EIGEN_DEVICE_FUNC Transform& shear(const Scalar& sx, const Scalar& sy); + EIGEN_DEVICE_FUNC Transform& preshear(const Scalar& sx, const Scalar& sy); + + EIGEN_DEVICE_FUNC inline Transform& operator=(const TranslationType& t); + + EIGEN_DEVICE_FUNC inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } + + EIGEN_DEVICE_FUNC inline Transform operator*(const TranslationType& t) const; + + EIGEN_DEVICE_FUNC inline Transform& operator=(const UniformScaling& t); + + EIGEN_DEVICE_FUNC inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } + + EIGEN_DEVICE_FUNC inline TransformTimeDiagonalReturnType operator*(const UniformScaling& s) const { + TransformTimeDiagonalReturnType res = *this; + res.scale(s.factor()); + return res; + } + + EIGEN_DEVICE_FUNC inline Transform& operator*=(const DiagonalMatrix& s) { + linearExt() *= s; + return *this; + } + + template + EIGEN_DEVICE_FUNC inline Transform& operator=(const RotationBase& r); + template + EIGEN_DEVICE_FUNC inline Transform& operator*=(const RotationBase& r) { + return rotate(r.toRotationMatrix()); + } + template + EIGEN_DEVICE_FUNC inline Transform operator*(const RotationBase& r) const; + + typedef std::conditional_t RotationReturnType; + EIGEN_DEVICE_FUNC RotationReturnType rotation() const; + + template + EIGEN_DEVICE_FUNC void computeRotationScaling(RotationMatrixType* rotation, ScalingMatrixType* scaling) const; + template + EIGEN_DEVICE_FUNC void computeScalingRotation(ScalingMatrixType* scaling, RotationMatrixType* rotation) const; + + template + EIGEN_DEVICE_FUNC Transform& fromPositionOrientationScale(const MatrixBase& position, + const OrientationType& orientation, + const MatrixBase& scale); + + EIGEN_DEVICE_FUNC inline Transform inverse(TransformTraits traits = (TransformTraits)Mode) const; + + /** \returns a const pointer to the column major internal matrix */ + EIGEN_DEVICE_FUNC const Scalar* data() const { return m_matrix.data(); } + /** \returns a non-const pointer to the column major internal matrix */ + EIGEN_DEVICE_FUNC Scalar* data() { return m_matrix.data(); } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline + typename internal::cast_return_type >::type + cast() const { + return typename internal::cast_return_type >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit Transform(const Transform& other) { + check_template_params(); + m_matrix = other.matrix().template cast(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const Transform& other, const typename NumTraits::Real& prec = + NumTraits::dummy_precision()) const { + return m_matrix.isApprox(other.m_matrix, prec); + } + + /** Sets the last row to [0 ... 0 1] + */ + EIGEN_DEVICE_FUNC void makeAffine() { internal::transform_make_affine::run(m_matrix); } + + /** \internal + * \returns the Dim x Dim linear part if the transformation is affine, + * and the HDim x Dim part for projective transformations. + */ + EIGEN_DEVICE_FUNC inline Block linearExt() { + return m_matrix.template block < int(Mode) == int(Projective) ? HDim : Dim, Dim > (0, 0); + } + /** \internal + * \returns the Dim x Dim linear part if the transformation is affine, + * and the HDim x Dim part for projective transformations. + */ + EIGEN_DEVICE_FUNC inline const Block linearExt() const { + return m_matrix.template block < int(Mode) == int(Projective) ? HDim : Dim, Dim > (0, 0); + } + + /** \internal + * \returns the translation part if the transformation is affine, + * and the last column for projective transformations. + */ + EIGEN_DEVICE_FUNC inline Block translationExt() { + return m_matrix.template block < int(Mode) == int(Projective) ? HDim : Dim, 1 > (0, Dim); + } + /** \internal + * \returns the translation part if the transformation is affine, + * and the last column for projective transformations. + */ + EIGEN_DEVICE_FUNC inline const Block translationExt() + const { + return m_matrix.template block < int(Mode) == int(Projective) ? HDim : Dim, 1 > (0, Dim); + } + +#ifdef EIGEN_TRANSFORM_PLUGIN +#include EIGEN_TRANSFORM_PLUGIN +#endif + + protected: +#ifndef EIGEN_PARSED_BY_DOXYGEN + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void check_template_params() { + EIGEN_STATIC_ASSERT((Options & (DontAlign | RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS) + } +#endif +}; + +/** \ingroup Geometry_Module */ +typedef Transform Isometry2f; +/** \ingroup Geometry_Module */ +typedef Transform Isometry3f; +/** \ingroup Geometry_Module */ +typedef Transform Isometry2d; +/** \ingroup Geometry_Module */ +typedef Transform Isometry3d; + +/** \ingroup Geometry_Module */ +typedef Transform Affine2f; +/** \ingroup Geometry_Module */ +typedef Transform Affine3f; +/** \ingroup Geometry_Module */ +typedef Transform Affine2d; +/** \ingroup Geometry_Module */ +typedef Transform Affine3d; + +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact2f; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact3f; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact2d; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact3d; + +/** \ingroup Geometry_Module */ +typedef Transform Projective2f; +/** \ingroup Geometry_Module */ +typedef Transform Projective3f; +/** \ingroup Geometry_Module */ +typedef Transform Projective2d; +/** \ingroup Geometry_Module */ +typedef Transform Projective3d; + +/************************** +*** Optional QT support *** +**************************/ + +#ifdef EIGEN_QT_SUPPORT + +#if (QT_VERSION < QT_VERSION_CHECK(6, 0, 0)) +/** Initializes \c *this from a QMatrix assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +Transform::Transform(const QMatrix& other) { + check_template_params(); + *this = other; +} + +/** Set \c *this from a QMatrix assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +Transform& Transform::operator=(const QMatrix& other) { + EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) + m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(); + else + m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(), 0, 0, 1; + return *this; +} + +/** \returns a QMatrix from \c *this assuming the dimension is 2. + * + * \warning this conversion might loss data if \c *this is not affine + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +QMatrix Transform::toQMatrix(void) const { + check_template_params(); + EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + return QMatrix(m_matrix.coeff(0, 0), m_matrix.coeff(1, 0), m_matrix.coeff(0, 1), m_matrix.coeff(1, 1), + m_matrix.coeff(0, 2), m_matrix.coeff(1, 2)); +} +#endif + +/** Initializes \c *this from a QTransform assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +Transform::Transform(const QTransform& other) { + check_template_params(); + *this = other; +} + +/** Set \c *this from a QTransform assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +Transform& Transform::operator=(const QTransform& other) { + check_template_params(); + EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) + m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(); + else + m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(), other.m13(), other.m23(), + other.m33(); + return *this; +} + +/** \returns a QTransform from \c *this assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template +QTransform Transform::toQTransform(void) const { + EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) + return QTransform(m_matrix.coeff(0, 0), m_matrix.coeff(1, 0), m_matrix.coeff(0, 1), m_matrix.coeff(1, 1), + m_matrix.coeff(0, 2), m_matrix.coeff(1, 2)); + else + return QTransform(m_matrix.coeff(0, 0), m_matrix.coeff(1, 0), m_matrix.coeff(2, 0), m_matrix.coeff(0, 1), + m_matrix.coeff(1, 1), m_matrix.coeff(2, 1), m_matrix.coeff(0, 2), m_matrix.coeff(1, 2), + m_matrix.coeff(2, 2)); +} +#endif + +/********************* +*** Procedural API *** +*********************/ + +/** Applies on the right the non uniform scale transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \sa prescale() + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::scale( + const MatrixBase& other) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, int(Dim)) + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + linearExt().noalias() = (linearExt() * other.asDiagonal()); + return *this; +} + +/** Applies on the right a uniform scale of a factor \a c to \c *this + * and returns a reference to \c *this. + * \sa prescale(Scalar) + */ +template +EIGEN_DEVICE_FUNC inline Transform& Transform::scale( + const Scalar& s) { + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + linearExt() *= s; + return *this; +} + +/** Applies on the left the non uniform scale transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \sa scale() + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::prescale( + const MatrixBase& other) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, int(Dim)) + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + affine().noalias() = (other.asDiagonal() * affine()); + return *this; +} + +/** Applies on the left a uniform scale of a factor \a c to \c *this + * and returns a reference to \c *this. + * \sa scale(Scalar) + */ +template +EIGEN_DEVICE_FUNC inline Transform& Transform::prescale( + const Scalar& s) { + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + m_matrix.template topRows() *= s; + return *this; +} + +/** Applies on the right the translation matrix represented by the vector \a other + * to \c *this and returns a reference to \c *this. + * \sa pretranslate() + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::translate( + const MatrixBase& other) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, int(Dim)) + translationExt() += linearExt() * other; + return *this; +} + +/** Applies on the left the translation matrix represented by the vector \a other + * to \c *this and returns a reference to \c *this. + * \sa translate() + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::pretranslate( + const MatrixBase& other) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, int(Dim)) + if (EIGEN_CONST_CONDITIONAL(int(Mode) == int(Projective))) + affine() += other * m_matrix.row(Dim); + else + translation() += other; + return *this; +} + +/** Applies on the right the rotation represented by the rotation \a rotation + * to \c *this and returns a reference to \c *this. + * + * The template parameter \a RotationType is the type of the rotation which + * must be known by internal::toRotationMatrix<>. + * + * Natively supported types includes: + * - any scalar (2D), + * - a Dim x Dim matrix expression, + * - a Quaternion (3D), + * - a AngleAxis (3D) + * + * This mechanism is easily extendable to support user types such as Euler angles, + * or a pair of Quaternion for 4D rotations. + * + * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType) + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::rotate( + const RotationType& rotation) { + linearExt() *= internal::toRotationMatrix(rotation); + return *this; +} + +/** Applies on the left the rotation represented by the rotation \a rotation + * to \c *this and returns a reference to \c *this. + * + * See rotate() for further details. + * + * \sa rotate() + */ +template +template +EIGEN_DEVICE_FUNC Transform& Transform::prerotate( + const RotationType& rotation) { + m_matrix.template block(0, 0) = + internal::toRotationMatrix(rotation) * m_matrix.template block(0, 0); + return *this; +} + +/** Applies on the right the shear transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \warning 2D only. + * \sa preshear() + */ +template +EIGEN_DEVICE_FUNC Transform& Transform::shear( + const Scalar& sx, const Scalar& sy) { + EIGEN_STATIC_ASSERT(int(Dim) == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + VectorType tmp = linear().col(0) * sy + linear().col(1); + linear() << linear().col(0) + linear().col(1) * sx, tmp; + return *this; +} + +/** Applies on the left the shear transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \warning 2D only. + * \sa shear() + */ +template +EIGEN_DEVICE_FUNC Transform& Transform::preshear( + const Scalar& sx, const Scalar& sy) { + EIGEN_STATIC_ASSERT(int(Dim) == 2, YOU_MADE_A_PROGRAMMING_MISTAKE) + EIGEN_STATIC_ASSERT(Mode != int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) + m_matrix.template block(0, 0) = + LinearMatrixType({{1, sy}, {sx, 1}}) * m_matrix.template block(0, 0); + return *this; +} + +/****************************************************** +*** Scaling, Translation and Rotation compatibility *** +******************************************************/ + +template +EIGEN_DEVICE_FUNC inline Transform& Transform::operator=( + const TranslationType& t) { + linear().setIdentity(); + translation() = t.vector(); + makeAffine(); + return *this; +} + +template +EIGEN_DEVICE_FUNC inline Transform Transform::operator*( + const TranslationType& t) const { + Transform res = *this; + res.translate(t.vector()); + return res; +} + +template +EIGEN_DEVICE_FUNC inline Transform& Transform::operator=( + const UniformScaling& s) { + m_matrix.setZero(); + linear().diagonal().fill(s.factor()); + makeAffine(); + return *this; +} + +template +template +EIGEN_DEVICE_FUNC inline Transform& Transform::operator=( + const RotationBase& r) { + linear() = internal::toRotationMatrix(r); + translation().setZero(); + makeAffine(); + return *this; +} + +template +template +EIGEN_DEVICE_FUNC inline Transform Transform::operator*( + const RotationBase& r) const { + Transform res = *this; + res.rotate(r.derived()); + return res; +} + +/************************ +*** Special functions *** +************************/ + +namespace internal { +template +struct transform_rotation_impl { + template + EIGEN_DEVICE_FUNC static inline const typename TransformType::LinearMatrixType run(const TransformType& t) { + typedef typename TransformType::LinearMatrixType LinearMatrixType; + LinearMatrixType result; + t.computeRotationScaling(&result, (LinearMatrixType*)0); + return result; + } +}; +template <> +struct transform_rotation_impl { + template + EIGEN_DEVICE_FUNC static inline typename TransformType::ConstLinearPart run(const TransformType& t) { + return t.linear(); + } +}; +} // namespace internal +/** \returns the rotation part of the transformation + * + * If Mode==Isometry, then this method is an alias for linear(), + * otherwise it calls computeRotationScaling() to extract the rotation + * through a SVD decomposition. + * + * \svd_module + * + * \sa computeRotationScaling(), computeScalingRotation(), class SVD + */ +template +EIGEN_DEVICE_FUNC typename Transform::RotationReturnType +Transform::rotation() const { + return internal::transform_rotation_impl::run(*this); +} + +/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * + * + * \svd_module + * + * \sa computeScalingRotation(), rotation(), class SVD + */ +template +template +EIGEN_DEVICE_FUNC void Transform::computeRotationScaling(RotationMatrixType* rotation, + ScalingMatrixType* scaling) const { + // Note that JacobiSVD is faster than BDCSVD for small matrices. + JacobiSVD svd(linear()); + + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) + ? Scalar(-1) + : Scalar(1); // so x has absolute value 1 + VectorType sv(svd.singularValues()); + sv.coeffRef(Dim - 1) *= x; + if (scaling) *scaling = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); + if (rotation) { + LinearMatrixType m(svd.matrixU()); + m.col(Dim - 1) *= x; + *rotation = m * svd.matrixV().adjoint(); + } +} + +/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * + * + * \svd_module + * + * \sa computeRotationScaling(), rotation(), class SVD + */ +template +template +EIGEN_DEVICE_FUNC void Transform::computeScalingRotation( + ScalingMatrixType* scaling, RotationMatrixType* rotation) const { + // Note that JacobiSVD is faster than BDCSVD for small matrices. + JacobiSVD svd(linear()); + + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) + ? Scalar(-1) + : Scalar(1); // so x has absolute value 1 + VectorType sv(svd.singularValues()); + sv.coeffRef(Dim - 1) *= x; + if (scaling) *scaling = svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint(); + if (rotation) { + LinearMatrixType m(svd.matrixU()); + m.col(Dim - 1) *= x; + *rotation = m * svd.matrixV().adjoint(); + } +} + +/** Convenient method to set \c *this from a position, orientation and scale + * of a 3D object. + */ +template +template +EIGEN_DEVICE_FUNC Transform& +Transform::fromPositionOrientationScale(const MatrixBase& position, + const OrientationType& orientation, + const MatrixBase& scale) { + linear() = internal::toRotationMatrix(orientation); + linear() *= scale.asDiagonal(); + translation() = position; + makeAffine(); + return *this; +} + +namespace internal { + +template +struct transform_make_affine { + template + EIGEN_DEVICE_FUNC static void run(MatrixType& mat) { + static const int Dim = MatrixType::ColsAtCompileTime - 1; + mat.template block<1, Dim>(Dim, 0).setZero(); + mat.coeffRef(Dim, Dim) = typename MatrixType::Scalar(1); + } +}; + +template <> +struct transform_make_affine { + template + EIGEN_DEVICE_FUNC static void run(MatrixType&) {} +}; + +// selector needed to avoid taking the inverse of a 3x4 matrix +template +struct projective_transform_inverse { + EIGEN_DEVICE_FUNC static inline void run(const TransformType&, TransformType&) {} +}; + +template +struct projective_transform_inverse { + EIGEN_DEVICE_FUNC static inline void run(const TransformType& m, TransformType& res) { + res.matrix() = m.matrix().inverse(); + } +}; + +} // end namespace internal + +/** + * + * \returns the inverse transformation according to some given knowledge + * on \c *this. + * + * \param hint allows to optimize the inversion process when the transformation + * is known to be not a general transformation (optional). The possible values are: + * - #Projective if the transformation is not necessarily affine, i.e., if the + * last row is not guaranteed to be [0 ... 0 1] + * - #Affine if the last row can be assumed to be [0 ... 0 1] + * - #Isometry if the transformation is only a concatenations of translations + * and rotations. + * The default is the template class parameter \c Mode. + * + * \warning unless \a traits is always set to NoShear or NoScaling, this function + * requires the generic inverse method of MatrixBase defined in the LU module. If + * you forget to include this module, then you will get hard to debug linking errors. + * + * \sa MatrixBase::inverse() + */ +template +EIGEN_DEVICE_FUNC Transform Transform::inverse( + TransformTraits hint) const { + Transform res; + if (hint == Projective) { + internal::projective_transform_inverse::run(*this, res); + } else { + if (hint == Isometry) { + res.matrix().template topLeftCorner() = linear().transpose(); + } else if (hint & Affine) { + res.matrix().template topLeftCorner() = linear().inverse(); + } else { + eigen_assert(false && "Invalid transform traits in Transform::Inverse"); + } + // translation and remaining parts + res.matrix().template topRightCorner() = -res.matrix().template topLeftCorner() * translation(); + res.makeAffine(); // we do need this, because in the beginning res is uninitialized + } + return res; +} + +namespace internal { + +/***************************************************** +*** Specializations of take affine part *** +*****************************************************/ + +template +struct transform_take_affine_part { + typedef typename TransformType::MatrixType MatrixType; + typedef typename TransformType::AffinePart AffinePart; + typedef typename TransformType::ConstAffinePart ConstAffinePart; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AffinePart run(MatrixType& m) { + return m.template block(0, 0); + } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ConstAffinePart run(const MatrixType& m) { + return m.template block(0, 0); + } +}; + +template +struct transform_take_affine_part > { + typedef typename Transform::MatrixType MatrixType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE MatrixType& run(MatrixType& m) { return m; } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const MatrixType& run(const MatrixType& m) { return m; } +}; + +/***************************************************** +*** Specializations of construct from matrix *** +*****************************************************/ + +template +struct transform_construct_from_matrix { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run( + Transform* transform, const Other& other) { + transform->linear() = other; + transform->translation().setZero(); + transform->makeAffine(); + } +}; + +template +struct transform_construct_from_matrix { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run( + Transform* transform, const Other& other) { + transform->affine() = other; + transform->makeAffine(); + } +}; + +template +struct transform_construct_from_matrix { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run( + Transform* transform, const Other& other) { + transform->matrix() = other; + } +}; + +template +struct transform_construct_from_matrix { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run( + Transform* transform, const Other& other) { + transform->matrix() = other.template block(0, 0); + } +}; + +/********************************************************** +*** Specializations of operator* with rhs EigenBase *** +**********************************************************/ + +template +struct transform_product_result { + enum { + Mode = (LhsMode == (int)Projective || RhsMode == (int)Projective) ? Projective + : (LhsMode == (int)Affine || RhsMode == (int)Affine) ? Affine + : (LhsMode == (int)AffineCompact || RhsMode == (int)AffineCompact) ? AffineCompact + : (LhsMode == (int)Isometry || RhsMode == (int)Isometry) ? Isometry + : Projective + }; +}; + +template +struct transform_right_product_impl { + typedef typename MatrixType::PlainObject ResultType; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { + return T.matrix() * other; + } +}; + +template +struct transform_right_product_impl { + enum { + Dim = TransformType::Dim, + HDim = TransformType::HDim, + OtherRows = MatrixType::RowsAtCompileTime, + OtherCols = MatrixType::ColsAtCompileTime + }; + + typedef typename MatrixType::PlainObject ResultType; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { + EIGEN_STATIC_ASSERT(OtherRows == HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); + + typedef Block TopLeftLhs; + + ResultType res(other.rows(), other.cols()); + TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() = T.affine() * other; + res.row(OtherRows - 1) = other.row(OtherRows - 1); + + return res; + } +}; + +template +struct transform_right_product_impl { + enum { + Dim = TransformType::Dim, + HDim = TransformType::HDim, + OtherRows = MatrixType::RowsAtCompileTime, + OtherCols = MatrixType::ColsAtCompileTime + }; + + typedef typename MatrixType::PlainObject ResultType; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { + EIGEN_STATIC_ASSERT(OtherRows == Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); + + typedef Block TopLeftLhs; + ResultType res( + Replicate(T.translation(), 1, other.cols())); + TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() += T.linear() * other; + + return res; + } +}; + +template +struct transform_right_product_impl // rhs is a vector of size Dim +{ + typedef typename TransformType::MatrixType TransformMatrix; + enum { + Dim = TransformType::Dim, + HDim = TransformType::HDim, + OtherRows = MatrixType::RowsAtCompileTime, + WorkingRows = plain_enum_min(TransformMatrix::RowsAtCompileTime, HDim) + }; + + typedef typename MatrixType::PlainObject ResultType; + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { + EIGEN_STATIC_ASSERT(OtherRows == Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); + + Matrix rhs; + rhs.template head() = other; + rhs[Dim] = typename ResultType::Scalar(1); + Matrix res(T.matrix() * rhs); + return res.template head(); + } +}; + +/********************************************************** +*** Specializations of operator* with lhs EigenBase *** +**********************************************************/ + +// generic HDim x HDim matrix * T => Projective +template +struct transform_left_product_impl { + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Other& other, const TransformType& tr) { + return ResultType(other * tr.matrix()); + } +}; + +// generic HDim x HDim matrix * AffineCompact => Projective +template +struct transform_left_product_impl { + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Other& other, const TransformType& tr) { + ResultType res; + res.matrix().noalias() = other.template block(0, 0) * tr.matrix(); + res.matrix().col(Dim) += other.col(Dim); + return res; + } +}; + +// affine matrix * T +template +struct transform_left_product_impl { + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef TransformType ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Other& other, const TransformType& tr) { + ResultType res; + res.affine().noalias() = other * tr.matrix(); + res.matrix().row(Dim) = tr.matrix().row(Dim); + return res; + } +}; + +// affine matrix * AffineCompact +template +struct transform_left_product_impl { + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef TransformType ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Other& other, const TransformType& tr) { + ResultType res; + res.matrix().noalias() = other.template block(0, 0) * tr.matrix(); + res.translation() += other.col(Dim); + return res; + } +}; + +// linear matrix * T +template +struct transform_left_product_impl { + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef TransformType ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Other& other, const TransformType& tr) { + TransformType res; + if (Mode != int(AffineCompact)) res.matrix().row(Dim) = tr.matrix().row(Dim); + res.matrix().template topRows().noalias() = other * tr.matrix().template topRows(); + return res; + } +}; + +/********************************************************** +*** Specializations of operator* with another Transform *** +**********************************************************/ + +template +struct transform_transform_product_impl, + Transform, false> { + enum { ResultMode = transform_product_result::Mode }; + typedef Transform Lhs; + typedef Transform Rhs; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Lhs& lhs, const Rhs& rhs) { + ResultType res; + res.linear() = lhs.linear() * rhs.linear(); + res.translation() = lhs.linear() * rhs.translation() + lhs.translation(); + res.makeAffine(); + return res; + } +}; + +template +struct transform_transform_product_impl, + Transform, true> { + typedef Transform Lhs; + typedef Transform Rhs; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Lhs& lhs, const Rhs& rhs) { + return ResultType(lhs.matrix() * rhs.matrix()); + } +}; + +template +struct transform_transform_product_impl, + Transform, true> { + typedef Transform Lhs; + typedef Transform Rhs; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Lhs& lhs, const Rhs& rhs) { + ResultType res; + res.matrix().template topRows() = lhs.matrix() * rhs.matrix(); + res.matrix().row(Dim) = rhs.matrix().row(Dim); + return res; + } +}; + +template +struct transform_transform_product_impl, + Transform, true> { + typedef Transform Lhs; + typedef Transform Rhs; + typedef Transform ResultType; + static EIGEN_DEVICE_FUNC ResultType run(const Lhs& lhs, const Rhs& rhs) { + ResultType res(lhs.matrix().template leftCols() * rhs.matrix()); + res.matrix().col(Dim) += lhs.matrix().col(Dim); + return res; + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_TRANSFORM_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h new file mode 100644 index 00000000000..682c4c70da9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h @@ -0,0 +1,204 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_TRANSLATION_H +#define EIGEN_TRANSLATION_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Translation + * + * \brief Represents a translation transformation + * + * \tparam Scalar_ the scalar type, i.e., the type of the coefficients. + * \tparam Dim_ the dimension of the space, can be a compile time value or Dynamic + * + * \note This class is not aimed to be used to store a translation transformation, + * but rather to make easier the constructions and updates of Transform objects. + * + * \sa class Scaling, class Transform + */ +template +class Translation { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, Dim_) + /** dimension of the space */ + enum { Dim = Dim_ }; + /** the scalar type of the coefficients */ + typedef Scalar_ Scalar; + /** corresponding vector type */ + typedef Matrix VectorType; + /** corresponding linear transformation matrix type */ + typedef Matrix LinearMatrixType; + /** corresponding affine transformation type */ + typedef Transform AffineTransformType; + /** corresponding isometric transformation type */ + typedef Transform IsometryTransformType; + + protected: + VectorType m_coeffs; + + public: + /** Default constructor without initialization. */ + EIGEN_DEVICE_FUNC Translation() {} + /** */ + EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy) { + eigen_assert(Dim == 2); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + } + /** */ + EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) { + eigen_assert(Dim == 3); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + m_coeffs.z() = sz; + } + /** Constructs and initialize the translation transformation from a vector of translation coefficients */ + EIGEN_DEVICE_FUNC explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} + + /** \brief Returns the x-translation by value. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar x() const { return m_coeffs.x(); } + /** \brief Returns the y-translation by value. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar y() const { return m_coeffs.y(); } + /** \brief Returns the z-translation by value. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar z() const { return m_coeffs.z(); } + + /** \brief Returns the x-translation as a reference. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& x() { return m_coeffs.x(); } + /** \brief Returns the y-translation as a reference. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& y() { return m_coeffs.y(); } + /** \brief Returns the z-translation as a reference. **/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& z() { return m_coeffs.z(); } + + EIGEN_DEVICE_FUNC const VectorType& vector() const { return m_coeffs; } + EIGEN_DEVICE_FUNC VectorType& vector() { return m_coeffs; } + + EIGEN_DEVICE_FUNC const VectorType& translation() const { return m_coeffs; } + EIGEN_DEVICE_FUNC VectorType& translation() { return m_coeffs; } + + /** Concatenates two translation */ + EIGEN_DEVICE_FUNC inline Translation operator*(const Translation& other) const { + return Translation(m_coeffs + other.m_coeffs); + } + + /** Concatenates a translation and a uniform scaling */ + EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const UniformScaling& other) const; + + /** Concatenates a translation and a linear transformation */ + template + EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase& linear) const; + + /** Concatenates a translation and a rotation */ + template + EIGEN_DEVICE_FUNC inline IsometryTransformType operator*(const RotationBase& r) const { + return *this * IsometryTransformType(r); + } + + /** \returns the concatenation of a linear transformation \a l with the translation \a t */ + // its a nightmare to define a templated friend function outside its declaration + template + friend EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase& linear, + const Translation& t) { + AffineTransformType res; + res.matrix().setZero(); + res.linear() = linear.derived(); + res.translation() = linear.derived() * t.m_coeffs; + res.matrix().row(Dim).setZero(); + res(Dim, Dim) = Scalar(1); + return res; + } + + /** Concatenates a translation and a transformation */ + template + EIGEN_DEVICE_FUNC inline Transform operator*( + const Transform& t) const { + Transform res = t; + res.pretranslate(m_coeffs); + return res; + } + + /** Applies translation to vector */ + template + inline std::enable_if_t operator*(const MatrixBase& vec) const { + return m_coeffs + vec.derived(); + } + + /** \returns the inverse translation (opposite) */ + Translation inverse() const { return Translation(-m_coeffs); } + + static const Translation Identity() { return Translation(VectorType::Zero()); } + + /** \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 + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type + cast() const { + return typename internal::cast_return_type >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit Translation(const Translation& other) { + m_coeffs = other.vector().template cast(); + } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const Translation& other, const typename NumTraits::Real& prec = + NumTraits::dummy_precision()) const { + return m_coeffs.isApprox(other.m_coeffs, prec); + } +}; + +/** \addtogroup Geometry_Module */ +//@{ +typedef Translation Translation2f; +typedef Translation Translation2d; +typedef Translation Translation3f; +typedef Translation Translation3d; +//@} + +template +EIGEN_DEVICE_FUNC inline typename Translation::AffineTransformType Translation::operator*( + const UniformScaling& other) const { + AffineTransformType res; + res.matrix().setZero(); + res.linear().diagonal().fill(other.factor()); + res.translation() = m_coeffs; + res(Dim, Dim) = Scalar(1); + return res; +} + +template +template +EIGEN_DEVICE_FUNC inline typename Translation::AffineTransformType Translation::operator*( + const EigenBase& linear) const { + AffineTransformType res; + res.matrix().setZero(); + res.linear() = linear.derived(); + res.translation() = m_coeffs; + res.matrix().row(Dim).setZero(); + res(Dim, Dim) = Scalar(1); + return res; +} + +} // end namespace Eigen + +#endif // EIGEN_TRANSLATION_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h new file mode 100644 index 00000000000..f8138b9aeb5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h @@ -0,0 +1,165 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Hauke Heibel +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_UMEYAMA_H +#define EIGEN_UMEYAMA_H + +// This file requires the user to include +// * Eigen/Core +// * Eigen/LU +// * Eigen/SVD +// * Eigen/Array + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +#ifndef EIGEN_PARSED_BY_DOXYGEN + +// These helpers are required since it allows to use mixed types as parameters +// for the Umeyama. The problem with mixed parameters is that the return type +// cannot trivially be deduced when float and double types are mixed. +namespace internal { + +// Compile time return type deduction for different MatrixBase types. +// Different means here different alignment and parameters but the same underlying +// real scalar type. +template +struct umeyama_transform_matrix_type { + enum { + MinRowsAtCompileTime = + internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime), + + // When possible we want to choose some small fixed size value since the result + // is likely to fit on the stack. So here, min_size_prefer_dynamic is not what we want. + HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime) + 1 + }; + + typedef Matrix::Scalar, HomogeneousDimension, HomogeneousDimension, + AutoAlign | (traits::Flags & RowMajorBit ? RowMajor : ColMajor), HomogeneousDimension, + HomogeneousDimension> + type; +}; + +} // namespace internal + +#endif + +/** + * \geometry_module \ingroup Geometry_Module + * + * \brief Returns the transformation between two point sets. + * + * The algorithm is based on: + * "Least-squares estimation of transformation parameters between two point patterns", + * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 + * + * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that + * \f{align*} + * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 + * \f} + * is minimized. + * + * The algorithm is based on the analysis of the covariance matrix + * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ + * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where + * \f$d\f$ is corresponding to the dimension (which is typically small). + * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ + * though the actual computational effort lies in the covariance + * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when + * the input point sets have dimension \f$d \times m\f$. + * + * Currently the method is working only for floating point matrices. + * + * \todo Should the return type of umeyama() become a Transform? + * + * \param src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$. + * \param dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. + * \param with_scaling Sets \f$ c=1 \f$ when false is passed. + * \return The homogeneous transformation + * \f{align*} + * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} + * \f} + * minimizing the residual above. This transformation is always returned as an + * Eigen::Matrix. + */ +template +typename internal::umeyama_transform_matrix_type::type umeyama( + const MatrixBase& src, const MatrixBase& dst, bool with_scaling = true) { + typedef typename internal::umeyama_transform_matrix_type::type TransformationMatrixType; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) + EIGEN_STATIC_ASSERT( + (internal::is_same::Scalar>::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + enum { Dimension = internal::min_size_prefer_dynamic(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; + + typedef Matrix VectorType; + typedef Matrix MatrixType; + typedef typename internal::plain_matrix_type_row_major::type RowMajorMatrixType; + + const Index m = src.rows(); // dimension + const Index n = src.cols(); // number of measurements + + // required for demeaning ... + const RealScalar one_over_n = RealScalar(1) / static_cast(n); + + // computation of mean + const VectorType src_mean = src.rowwise().sum() * one_over_n; + const VectorType dst_mean = dst.rowwise().sum() * one_over_n; + + // demeaning of src and dst points + const RowMajorMatrixType src_demean = src.colwise() - src_mean; + const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean; + + // Eq. (38) + const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose(); + + JacobiSVD svd(sigma); + + // Initialize the resulting transformation with an identity matrix... + TransformationMatrixType Rt = TransformationMatrixType::Identity(m + 1, m + 1); + + // Eq. (39) + VectorType S = VectorType::Ones(m); + + if (svd.matrixU().determinant() * svd.matrixV().determinant() < 0) { + Index tmp = m - 1; + S(tmp) = -1; + } + + // Eq. (40) and (43) + Rt.block(0, 0, m, m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); + + if (with_scaling) { + // Eq. (36)-(37) + const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n; + + // Eq. (42) + const Scalar c = Scalar(1) / src_var * svd.singularValues().dot(S); + + // Eq. (41) + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= c * Rt.topLeftCorner(m, m) * src_mean; + Rt.block(0, 0, m, m) *= c; + } else { + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m, m) * src_mean; + } + + return Rt; +} + +} // end namespace Eigen + +#endif // EIGEN_UMEYAMA_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/arch/Geometry_SIMD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/arch/Geometry_SIMD.h new file mode 100644 index 00000000000..e8b210e7017 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/arch/Geometry_SIMD.h @@ -0,0 +1,152 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Rohit Garg +// Copyright (C) 2009-2010 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// 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/. + +#ifndef EIGEN_GEOMETRY_SIMD_H +#define EIGEN_GEOMETRY_SIMD_H + +// IWYU pragma: private +#include "../InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct quat_product { + enum { + AAlignment = traits::Alignment, + BAlignment = traits::Alignment, + ResAlignment = traits >::Alignment + }; + static inline Quaternion run(const QuaternionBase& _a, const QuaternionBase& _b) { + evaluator ae(_a.coeffs()); + evaluator be(_b.coeffs()); + Quaternion res; + const float neg_zero = numext::bit_cast(0x80000000u); + const float arr[4] = {0.f, 0.f, 0.f, neg_zero}; + const Packet4f mask = ploadu(arr); + Packet4f a = ae.template packet(0); + Packet4f b = be.template packet(0); + Packet4f s1 = pmul(vec4f_swizzle1(a, 1, 2, 0, 2), vec4f_swizzle1(b, 2, 0, 1, 2)); + Packet4f s2 = pmul(vec4f_swizzle1(a, 3, 3, 3, 1), vec4f_swizzle1(b, 0, 1, 2, 1)); + pstoret( + &res.x(), padd(psub(pmul(a, vec4f_swizzle1(b, 3, 3, 3, 3)), + pmul(vec4f_swizzle1(a, 2, 0, 1, 0), vec4f_swizzle1(b, 1, 2, 0, 0))), + pxor(mask, padd(s1, s2)))); + + return res; + } +}; + +template +struct quat_conj { + enum { ResAlignment = traits >::Alignment }; + static inline Quaternion run(const QuaternionBase& q) { + evaluator qe(q.coeffs()); + Quaternion res; + const float neg_zero = numext::bit_cast(0x80000000u); + const float arr[4] = {neg_zero, neg_zero, neg_zero, 0.f}; + const Packet4f mask = ploadu(arr); + pstoret(&res.x(), + pxor(mask, qe.template packet::Alignment, Packet4f>(0))); + return res; + } +}; + +template +struct cross3_impl { + using DstPlainType = typename plain_matrix_type::type; + static constexpr int DstAlignment = evaluator::Alignment; + static constexpr int LhsAlignment = evaluator::Alignment; + static constexpr int RhsAlignment = evaluator::Alignment; + static inline DstPlainType run(const VectorLhs& lhs, const VectorRhs& rhs) { + evaluator lhs_eval(lhs); + evaluator rhs_eval(rhs); + Packet4f a = lhs_eval.template packet(0); + Packet4f b = rhs_eval.template packet(0); + Packet4f mul1 = pmul(vec4f_swizzle1(a, 1, 2, 0, 3), vec4f_swizzle1(b, 2, 0, 1, 3)); + Packet4f mul2 = pmul(vec4f_swizzle1(a, 2, 0, 1, 3), vec4f_swizzle1(b, 1, 2, 0, 3)); + DstPlainType res; + pstoret(&res.x(), psub(mul1, mul2)); + return res; + } +}; + +#if (defined EIGEN_VECTORIZE_SSE) || (EIGEN_ARCH_ARM64) + +template +struct quat_product { + enum { BAlignment = traits::Alignment, ResAlignment = traits >::Alignment }; + + static inline Quaternion run(const QuaternionBase& _a, const QuaternionBase& _b) { + Quaternion res; + + evaluator ae(_a.coeffs()); + evaluator be(_b.coeffs()); + + const double* a = _a.coeffs().data(); + Packet2d b_xy = be.template packet(0); + Packet2d b_zw = be.template packet(2); + Packet2d a_xx = pset1(a[0]); + Packet2d a_yy = pset1(a[1]); + Packet2d a_zz = pset1(a[2]); + Packet2d a_ww = pset1(a[3]); + + // two temporaries: + Packet2d t1, t2; + + /* + * t1 = ww*xy + yy*zw + * t2 = zz*xy - xx*zw + * res.xy = t1 +/- swap(t2) + */ + t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw)); + t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw)); + pstoret(&res.x(), paddsub(t1, preverse(t2))); + + /* + * t1 = ww*zw - yy*xy + * t2 = zz*zw + xx*xy + * res.zw = t1 -/+ swap(t2) = swap( swap(t1) +/- t2) + */ + t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy)); + t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy)); + pstoret(&res.z(), preverse(paddsub(preverse(t1), t2))); + + return res; + } +}; + +template +struct quat_conj { + enum { ResAlignment = traits >::Alignment }; + static inline Quaternion run(const QuaternionBase& q) { + evaluator qe(q.coeffs()); + Quaternion res; + const double neg_zero = numext::bit_cast(0x8000000000000000ull); + const double arr1[2] = {neg_zero, neg_zero}; + const double arr2[2] = {neg_zero, 0.0}; + const Packet2d mask0 = ploadu(arr1); + const Packet2d mask2 = ploadu(arr2); + pstoret(&res.x(), + pxor(mask0, qe.template packet::Alignment, Packet2d>(0))); + pstoret(&res.z(), + pxor(mask2, qe.template packet::Alignment, Packet2d>(2))); + return res; + } +}; + +#endif // end EIGEN_VECTORIZE_SSE_OR_EIGEN_ARCH_ARM64 + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_GEOMETRY_SIMD_H diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 3ae22482492..029902b58cd 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -359,8 +359,18 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) { TEST(DifferentialDrivePoseEstimatorTest, TestReset) { frc::DifferentialDriveKinematics kinematics{1_m}; frc::DifferentialDrivePoseEstimator estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + kinematics, + frc::Rotation2d{}, + 0_m, + 0_m, + frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, + {1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0}}; + + // Test initial pose + EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); + EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); + EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); // Test reset position estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m, diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index ef2fe8919fd..f1f7be98a17 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -368,8 +368,17 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) { frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::MecanumDrivePoseEstimator estimator{ - kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, - frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + kinematics, + frc::Rotation2d{}, + frc::MecanumDriveWheelPositions{}, + frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, + {1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0}}; + + // Test initial pose + EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); + EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); + EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); // Test reset position estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index ec19d387f56..d62200176c0 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -394,10 +394,15 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { frc::Rotation2d{}, {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, - frc::Pose2d{}, + frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + // Test initial pose + EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); + EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); + EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + // Test reset position { frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};