diff options
Diffstat (limited to 'Eigen/src/Eigen2Support')
28 files changed, 4929 insertions, 0 deletions
diff --git a/Eigen/src/Eigen2Support/Block.h b/Eigen/src/Eigen2Support/Block.h new file mode 100644 index 000000000..604456f40 --- /dev/null +++ b/Eigen/src/Eigen2Support/Block.h @@ -0,0 +1,126 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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_BLOCK2_H +#define EIGEN_BLOCK2_H + +namespace Eigen { + +/** \returns a dynamic-size expression of a corner of *this. + * + * \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight, + * \a Eigen::BottomLeft, \a Eigen::BottomRight. + * \param cRows the number of rows in the corner + * \param cCols the number of columns in the corner + * + * Example: \include MatrixBase_corner_enum_int_int.cpp + * Output: \verbinclude MatrixBase_corner_enum_int_int.out + * + * \note Even though the returned expression has dynamic size, in the case + * when it is applied to a fixed-size matrix, it inherits a fixed maximal size, + * which means that evaluating it does not cause a dynamic memory allocation. + * + * \sa class Block, block(Index,Index,Index,Index) + */ +template<typename Derived> +inline Block<Derived> DenseBase<Derived> + ::corner(CornerType type, Index cRows, Index cCols) +{ + switch(type) + { + default: + eigen_assert(false && "Bad corner type."); + case TopLeft: + return Block<Derived>(derived(), 0, 0, cRows, cCols); + case TopRight: + return Block<Derived>(derived(), 0, cols() - cCols, cRows, cCols); + case BottomLeft: + return Block<Derived>(derived(), rows() - cRows, 0, cRows, cCols); + case BottomRight: + return Block<Derived>(derived(), rows() - cRows, cols() - cCols, cRows, cCols); + } +} + +/** This is the const version of corner(CornerType, Index, Index).*/ +template<typename Derived> +inline const Block<Derived> +DenseBase<Derived>::corner(CornerType type, Index cRows, Index cCols) const +{ + switch(type) + { + default: + eigen_assert(false && "Bad corner type."); + case TopLeft: + return Block<Derived>(derived(), 0, 0, cRows, cCols); + case TopRight: + return Block<Derived>(derived(), 0, cols() - cCols, cRows, cCols); + case BottomLeft: + return Block<Derived>(derived(), rows() - cRows, 0, cRows, cCols); + case BottomRight: + return Block<Derived>(derived(), rows() - cRows, cols() - cCols, cRows, cCols); + } +} + +/** \returns a fixed-size expression of a corner of *this. + * + * \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight, + * \a Eigen::BottomLeft, \a Eigen::BottomRight. + * + * The template parameters CRows and CCols arethe number of rows and columns in the corner. + * + * Example: \include MatrixBase_template_int_int_corner_enum.cpp + * Output: \verbinclude MatrixBase_template_int_int_corner_enum.out + * + * \sa class Block, block(Index,Index,Index,Index) + */ +template<typename Derived> +template<int CRows, int CCols> +inline Block<Derived, CRows, CCols> +DenseBase<Derived>::corner(CornerType type) +{ + switch(type) + { + default: + eigen_assert(false && "Bad corner type."); + case TopLeft: + return Block<Derived, CRows, CCols>(derived(), 0, 0); + case TopRight: + return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols); + case BottomLeft: + return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0); + case BottomRight: + return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols); + } +} + +/** This is the const version of corner<int, int>(CornerType).*/ +template<typename Derived> +template<int CRows, int CCols> +inline const Block<Derived, CRows, CCols> +DenseBase<Derived>::corner(CornerType type) const +{ + switch(type) + { + default: + eigen_assert(false && "Bad corner type."); + case TopLeft: + return Block<Derived, CRows, CCols>(derived(), 0, 0); + case TopRight: + return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols); + case BottomLeft: + return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0); + case BottomRight: + return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols); + } +} + +} // end namespace Eigen + +#endif // EIGEN_BLOCK2_H diff --git a/Eigen/src/Eigen2Support/CMakeLists.txt b/Eigen/src/Eigen2Support/CMakeLists.txt new file mode 100644 index 000000000..7ae41b3cb --- /dev/null +++ b/Eigen/src/Eigen2Support/CMakeLists.txt @@ -0,0 +1,8 @@ +FILE(GLOB Eigen_Eigen2Support_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Eigen2Support_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Eigen2Support COMPONENT Devel + ) + +ADD_SUBDIRECTORY(Geometry)
\ No newline at end of file diff --git a/Eigen/src/Eigen2Support/Cwise.h b/Eigen/src/Eigen2Support/Cwise.h new file mode 100644 index 000000000..d95009b6e --- /dev/null +++ b/Eigen/src/Eigen2Support/Cwise.h @@ -0,0 +1,192 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> +// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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_CWISE_H +#define EIGEN_CWISE_H + +namespace Eigen { + +/** \internal + * convenient macro to defined the return type of a cwise binary operation */ +#define EIGEN_CWISE_BINOP_RETURN_TYPE(OP) \ + CwiseBinaryOp<OP<typename internal::traits<ExpressionType>::Scalar>, ExpressionType, OtherDerived> + +/** \internal + * convenient macro to defined the return type of a cwise unary operation */ +#define EIGEN_CWISE_UNOP_RETURN_TYPE(OP) \ + CwiseUnaryOp<OP<typename internal::traits<ExpressionType>::Scalar>, ExpressionType> + +/** \internal + * convenient macro to defined the return type of a cwise comparison to a scalar */ +#define EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(OP) \ + CwiseBinaryOp<OP<typename internal::traits<ExpressionType>::Scalar>, ExpressionType, \ + typename ExpressionType::ConstantReturnType > + +/** \class Cwise + * + * \brief Pseudo expression providing additional coefficient-wise operations + * + * \param ExpressionType the type of the object on which to do coefficient-wise operations + * + * This class represents an expression with additional coefficient-wise features. + * It is the return type of MatrixBase::cwise() + * and most of the time this is the only way it is used. + * + * Example: \include MatrixBase_cwise_const.cpp + * Output: \verbinclude MatrixBase_cwise_const.out + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_CWISE_PLUGIN. + * + * \sa MatrixBase::cwise() const, MatrixBase::cwise() + */ +template<typename ExpressionType> class Cwise +{ + public: + + typedef typename internal::traits<ExpressionType>::Scalar Scalar; + typedef typename internal::conditional<internal::must_nest_by_value<ExpressionType>::ret, + ExpressionType, const ExpressionType&>::type ExpressionTypeNested; + typedef CwiseUnaryOp<internal::scalar_add_op<Scalar>, ExpressionType> ScalarAddReturnType; + + inline Cwise(const ExpressionType& matrix) : m_matrix(matrix) {} + + /** \internal */ + inline const ExpressionType& _expression() const { return m_matrix; } + + template<typename OtherDerived> + const EIGEN_CWISE_PRODUCT_RETURN_TYPE(ExpressionType,OtherDerived) + operator*(const MatrixBase<OtherDerived> &other) const; + + template<typename OtherDerived> + const EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_quotient_op) + operator/(const MatrixBase<OtherDerived> &other) const; + + /** \deprecated ArrayBase::min() */ + template<typename OtherDerived> + const EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_min_op) + (min)(const MatrixBase<OtherDerived> &other) const + { return EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_min_op)(_expression(), other.derived()); } + + /** \deprecated ArrayBase::max() */ + template<typename OtherDerived> + const EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_max_op) + (max)(const MatrixBase<OtherDerived> &other) const + { return EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_max_op)(_expression(), other.derived()); } + + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_abs_op) abs() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_abs2_op) abs2() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_square_op) square() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_cube_op) cube() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_inverse_op) inverse() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_sqrt_op) sqrt() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_exp_op) exp() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_log_op) log() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_cos_op) cos() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_sin_op) sin() const; + const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_pow_op) pow(const Scalar& exponent) const; + + const ScalarAddReturnType + operator+(const Scalar& scalar) const; + + /** \relates Cwise */ + friend const ScalarAddReturnType + operator+(const Scalar& scalar, const Cwise& mat) + { return mat + scalar; } + + ExpressionType& operator+=(const Scalar& scalar); + + const ScalarAddReturnType + operator-(const Scalar& scalar) const; + + ExpressionType& operator-=(const Scalar& scalar); + + template<typename OtherDerived> + inline ExpressionType& operator*=(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + inline ExpressionType& operator/=(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less) + operator<(const MatrixBase<OtherDerived>& other) const; + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal) + operator<=(const MatrixBase<OtherDerived>& other) const; + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater) + operator>(const MatrixBase<OtherDerived>& other) const; + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal) + operator>=(const MatrixBase<OtherDerived>& other) const; + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to) + operator==(const MatrixBase<OtherDerived>& other) const; + + template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to) + operator!=(const MatrixBase<OtherDerived>& other) const; + + // comparisons to a scalar value + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less) + operator<(Scalar s) const; + + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal) + operator<=(Scalar s) const; + + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater) + operator>(Scalar s) const; + + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal) + operator>=(Scalar s) const; + + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to) + operator==(Scalar s) const; + + const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to) + operator!=(Scalar s) const; + + // allow to extend Cwise outside Eigen + #ifdef EIGEN_CWISE_PLUGIN + #include EIGEN_CWISE_PLUGIN + #endif + + protected: + ExpressionTypeNested m_matrix; +}; + + +/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations + * + * Example: \include MatrixBase_cwise_const.cpp + * Output: \verbinclude MatrixBase_cwise_const.out + * + * \sa class Cwise, cwise() + */ +template<typename Derived> +inline const Cwise<Derived> MatrixBase<Derived>::cwise() const +{ + return derived(); +} + +/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations + * + * Example: \include MatrixBase_cwise.cpp + * Output: \verbinclude MatrixBase_cwise.out + * + * \sa class Cwise, cwise() const + */ +template<typename Derived> +inline Cwise<Derived> MatrixBase<Derived>::cwise() +{ + return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_CWISE_H diff --git a/Eigen/src/Eigen2Support/CwiseOperators.h b/Eigen/src/Eigen2Support/CwiseOperators.h new file mode 100644 index 000000000..482f30648 --- /dev/null +++ b/Eigen/src/Eigen2Support/CwiseOperators.h @@ -0,0 +1,298 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_ARRAY_CWISE_OPERATORS_H +#define EIGEN_ARRAY_CWISE_OPERATORS_H + +namespace Eigen { + +/*************************************************************************** +* The following functions were defined in Core +***************************************************************************/ + + +/** \deprecated ArrayBase::abs() */ +template<typename ExpressionType> +EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_abs_op) +Cwise<ExpressionType>::abs() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::abs2() */ +template<typename ExpressionType> +EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_abs2_op) +Cwise<ExpressionType>::abs2() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::exp() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_exp_op) +Cwise<ExpressionType>::exp() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::log() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_log_op) +Cwise<ExpressionType>::log() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::operator*() */ +template<typename ExpressionType> +template<typename OtherDerived> +EIGEN_STRONG_INLINE const EIGEN_CWISE_PRODUCT_RETURN_TYPE(ExpressionType,OtherDerived) +Cwise<ExpressionType>::operator*(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_PRODUCT_RETURN_TYPE(ExpressionType,OtherDerived)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator/() */ +template<typename ExpressionType> +template<typename OtherDerived> +EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_quotient_op) +Cwise<ExpressionType>::operator/(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(internal::scalar_quotient_op)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator*=() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline ExpressionType& Cwise<ExpressionType>::operator*=(const MatrixBase<OtherDerived> &other) +{ + return m_matrix.const_cast_derived() = *this * other; +} + +/** \deprecated ArrayBase::operator/=() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline ExpressionType& Cwise<ExpressionType>::operator/=(const MatrixBase<OtherDerived> &other) +{ + return m_matrix.const_cast_derived() = *this / other; +} + +/*************************************************************************** +* The following functions were defined in Array +***************************************************************************/ + +// -- unary operators -- + +/** \deprecated ArrayBase::sqrt() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_sqrt_op) +Cwise<ExpressionType>::sqrt() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::cos() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_cos_op) +Cwise<ExpressionType>::cos() const +{ + return _expression(); +} + + +/** \deprecated ArrayBase::sin() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_sin_op) +Cwise<ExpressionType>::sin() const +{ + return _expression(); +} + + +/** \deprecated ArrayBase::log() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_pow_op) +Cwise<ExpressionType>::pow(const Scalar& exponent) const +{ + return EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_pow_op)(_expression(), internal::scalar_pow_op<Scalar>(exponent)); +} + + +/** \deprecated ArrayBase::inverse() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_inverse_op) +Cwise<ExpressionType>::inverse() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::square() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_square_op) +Cwise<ExpressionType>::square() const +{ + return _expression(); +} + +/** \deprecated ArrayBase::cube() */ +template<typename ExpressionType> +inline const EIGEN_CWISE_UNOP_RETURN_TYPE(internal::scalar_cube_op) +Cwise<ExpressionType>::cube() const +{ + return _expression(); +} + + +// -- binary operators -- + +/** \deprecated ArrayBase::operator<() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less) +Cwise<ExpressionType>::operator<(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::<=() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal) +Cwise<ExpressionType>::operator<=(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator>() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater) +Cwise<ExpressionType>::operator>(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator>=() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal) +Cwise<ExpressionType>::operator>=(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator==() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to) +Cwise<ExpressionType>::operator==(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)(_expression(), other.derived()); +} + +/** \deprecated ArrayBase::operator!=() */ +template<typename ExpressionType> +template<typename OtherDerived> +inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to) +Cwise<ExpressionType>::operator!=(const MatrixBase<OtherDerived> &other) const +{ + return EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)(_expression(), other.derived()); +} + +// comparisons to scalar value + +/** \deprecated ArrayBase::operator<(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less) +Cwise<ExpressionType>::operator<(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +/** \deprecated ArrayBase::operator<=(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal) +Cwise<ExpressionType>::operator<=(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +/** \deprecated ArrayBase::operator>(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater) +Cwise<ExpressionType>::operator>(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +/** \deprecated ArrayBase::operator>=(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal) +Cwise<ExpressionType>::operator>=(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +/** \deprecated ArrayBase::operator==(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to) +Cwise<ExpressionType>::operator==(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +/** \deprecated ArrayBase::operator!=(Scalar) */ +template<typename ExpressionType> +inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to) +Cwise<ExpressionType>::operator!=(Scalar s) const +{ + return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)(_expression(), + typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s)); +} + +// scalar addition + +/** \deprecated ArrayBase::operator+(Scalar) */ +template<typename ExpressionType> +inline const typename Cwise<ExpressionType>::ScalarAddReturnType +Cwise<ExpressionType>::operator+(const Scalar& scalar) const +{ + return typename Cwise<ExpressionType>::ScalarAddReturnType(m_matrix, internal::scalar_add_op<Scalar>(scalar)); +} + +/** \deprecated ArrayBase::operator+=(Scalar) */ +template<typename ExpressionType> +inline ExpressionType& Cwise<ExpressionType>::operator+=(const Scalar& scalar) +{ + return m_matrix.const_cast_derived() = *this + scalar; +} + +/** \deprecated ArrayBase::operator-(Scalar) */ +template<typename ExpressionType> +inline const typename Cwise<ExpressionType>::ScalarAddReturnType +Cwise<ExpressionType>::operator-(const Scalar& scalar) const +{ + return *this + (-scalar); +} + +/** \deprecated ArrayBase::operator-=(Scalar) */ +template<typename ExpressionType> +inline ExpressionType& Cwise<ExpressionType>::operator-=(const Scalar& scalar) +{ + return m_matrix.const_cast_derived() = *this - scalar; +} + +} // end namespace Eigen + +#endif // EIGEN_ARRAY_CWISE_OPERATORS_H diff --git a/Eigen/src/Eigen2Support/Geometry/AlignedBox.h b/Eigen/src/Eigen2Support/Geometry/AlignedBox.h new file mode 100644 index 000000000..5c928e8fc --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/AlignedBox.h @@ -0,0 +1,159 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * \nonstableyet + * + * \class AlignedBox + * + * \brief An axis aligned box + * + * \param _Scalar the type of the scalar coefficients + * \param _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. + */ +template <typename _Scalar, int _AmbientDim> +class AlignedBox +{ +public: +EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1) + enum { AmbientDimAtCompileTime = _AmbientDim }; + typedef _Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; + + /** Default constructor initializing a null box. */ + inline explicit AlignedBox() + { if (AmbientDimAtCompileTime!=Dynamic) setNull(); } + + /** Constructs a null box with \a _dim the dimension of the ambient space. */ + inline explicit AlignedBox(int _dim) : m_min(_dim), m_max(_dim) + { setNull(); } + + /** Constructs a box with extremities \a _min and \a _max. */ + inline AlignedBox(const VectorType& _min, const VectorType& _max) : m_min(_min), m_max(_max) {} + + /** Constructs a box containing a single point \a p. */ + inline explicit AlignedBox(const VectorType& p) : m_min(p), m_max(p) {} + + ~AlignedBox() {} + + /** \returns the dimension in which the box holds */ + inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size()-1 : AmbientDimAtCompileTime; } + + /** \returns true if the box is null, i.e, empty. */ + inline bool isNull() const { return (m_min.cwise() > m_max).any(); } + + /** Makes \c *this a null/empty box. */ + inline void setNull() + { + m_min.setConstant( (std::numeric_limits<Scalar>::max)()); + m_max.setConstant(-(std::numeric_limits<Scalar>::max)()); + } + + /** \returns the minimal corner */ + inline const VectorType& (min)() const { return m_min; } + /** \returns a non const reference to the minimal corner */ + inline VectorType& (min)() { return m_min; } + /** \returns the maximal corner */ + inline const VectorType& (max)() const { return m_max; } + /** \returns a non const reference to the maximal corner */ + inline VectorType& (max)() { return m_max; } + + /** \returns true if the point \a p is inside the box \c *this. */ + inline bool contains(const VectorType& p) const + { return (m_min.cwise()<=p).all() && (p.cwise()<=m_max).all(); } + + /** \returns true if the box \a b is entirely inside the box \c *this. */ + inline bool contains(const AlignedBox& b) const + { return (m_min.cwise()<=(b.min)()).all() && ((b.max)().cwise()<=m_max).all(); } + + /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */ + inline AlignedBox& extend(const VectorType& p) + { m_min = (m_min.cwise().min)(p); m_max = (m_max.cwise().max)(p); return *this; } + + /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */ + inline AlignedBox& extend(const AlignedBox& b) + { m_min = (m_min.cwise().min)(b.m_min); m_max = (m_max.cwise().max)(b.m_max); return *this; } + + /** Clamps \c *this by the box \a b and returns a reference to \c *this. */ + inline AlignedBox& clamp(const AlignedBox& b) + { m_min = (m_min.cwise().max)(b.m_min); m_max = (m_max.cwise().min)(b.m_max); return *this; } + + /** Translate \c *this by the vector \a t and returns a reference to \c *this. */ + inline AlignedBox& translate(const VectorType& t) + { m_min += t; m_max += t; return *this; } + + /** \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() + */ + inline Scalar squaredExteriorDistance(const VectorType& p) 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() + */ + inline Scalar exteriorDistance(const VectorType& p) const + { return ei_sqrt(squaredExteriorDistance(p)); } + + /** \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<typename NewScalarType> + inline typename internal::cast_return_type<AlignedBox, + AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const + { + return typename internal::cast_return_type<AlignedBox, + AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other) + { + m_min = (other.min)().template cast<Scalar>(); + m_max = (other.max)().template cast<Scalar>(); + } + + /** \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 AlignedBox& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); } + +protected: + + VectorType m_min, m_max; +}; + +template<typename Scalar,int AmbiantDim> +inline Scalar AlignedBox<Scalar,AmbiantDim>::squaredExteriorDistance(const VectorType& p) const +{ + Scalar dist2(0); + Scalar aux; + for (int k=0; k<dim(); ++k) + { + if ((aux = (p[k]-m_min[k]))<Scalar(0)) + dist2 += aux*aux; + else if ( (aux = (m_max[k]-p[k]))<Scalar(0)) + dist2 += aux*aux; + } + return dist2; +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/All.h b/Eigen/src/Eigen2Support/Geometry/All.h new file mode 100644 index 000000000..e0b00fccc --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/All.h @@ -0,0 +1,115 @@ +#ifndef EIGEN2_GEOMETRY_MODULE_H +#define EIGEN2_GEOMETRY_MODULE_H + +#include <limits> + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#if EIGEN2_SUPPORT_STAGE < STAGE20_RESOLVE_API_CONFLICTS +#include "RotationBase.h" +#include "Rotation2D.h" +#include "Quaternion.h" +#include "AngleAxis.h" +#include "Transform.h" +#include "Translation.h" +#include "Scaling.h" +#include "AlignedBox.h" +#include "Hyperplane.h" +#include "ParametrizedLine.h" +#endif + + +#define RotationBase eigen2_RotationBase +#define Rotation2D eigen2_Rotation2D +#define Rotation2Df eigen2_Rotation2Df +#define Rotation2Dd eigen2_Rotation2Dd + +#define Quaternion eigen2_Quaternion +#define Quaternionf eigen2_Quaternionf +#define Quaterniond eigen2_Quaterniond + +#define AngleAxis eigen2_AngleAxis +#define AngleAxisf eigen2_AngleAxisf +#define AngleAxisd eigen2_AngleAxisd + +#define Transform eigen2_Transform +#define Transform2f eigen2_Transform2f +#define Transform2d eigen2_Transform2d +#define Transform3f eigen2_Transform3f +#define Transform3d eigen2_Transform3d + +#define Translation eigen2_Translation +#define Translation2f eigen2_Translation2f +#define Translation2d eigen2_Translation2d +#define Translation3f eigen2_Translation3f +#define Translation3d eigen2_Translation3d + +#define Scaling eigen2_Scaling +#define Scaling2f eigen2_Scaling2f +#define Scaling2d eigen2_Scaling2d +#define Scaling3f eigen2_Scaling3f +#define Scaling3d eigen2_Scaling3d + +#define AlignedBox eigen2_AlignedBox + +#define Hyperplane eigen2_Hyperplane +#define ParametrizedLine eigen2_ParametrizedLine + +#define ei_toRotationMatrix eigen2_ei_toRotationMatrix +#define ei_quaternion_assign_impl eigen2_ei_quaternion_assign_impl +#define ei_transform_product_impl eigen2_ei_transform_product_impl + +#include "RotationBase.h" +#include "Rotation2D.h" +#include "Quaternion.h" +#include "AngleAxis.h" +#include "Transform.h" +#include "Translation.h" +#include "Scaling.h" +#include "AlignedBox.h" +#include "Hyperplane.h" +#include "ParametrizedLine.h" + +#undef ei_toRotationMatrix +#undef ei_quaternion_assign_impl +#undef ei_transform_product_impl + +#undef RotationBase +#undef Rotation2D +#undef Rotation2Df +#undef Rotation2Dd + +#undef Quaternion +#undef Quaternionf +#undef Quaterniond + +#undef AngleAxis +#undef AngleAxisf +#undef AngleAxisd + +#undef Transform +#undef Transform2f +#undef Transform2d +#undef Transform3f +#undef Transform3d + +#undef Translation +#undef Translation2f +#undef Translation2d +#undef Translation3f +#undef Translation3d + +#undef Scaling +#undef Scaling2f +#undef Scaling2d +#undef Scaling3f +#undef Scaling3d + +#undef AlignedBox + +#undef Hyperplane +#undef ParametrizedLine + +#endif // EIGEN2_GEOMETRY_MODULE_H diff --git a/Eigen/src/Eigen2Support/Geometry/AngleAxis.h b/Eigen/src/Eigen2Support/Geometry/AngleAxis.h new file mode 100644 index 000000000..20f1fceeb --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/AngleAxis.h @@ -0,0 +1,214 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +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. + * + * The following two typedefs are provided for convenience: + * \li \c AngleAxisf for \c float + * \li \c AngleAxisd for \c double + * + * \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles + * + * 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() + */ + +template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> > +{ + typedef _Scalar Scalar; +}; + +template<typename _Scalar> +class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> +{ + typedef RotationBase<AngleAxis<_Scalar>,3> Base; + +public: + + using Base::operator*; + + enum { Dim = 3 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Quaternion<Scalar> QuaternionType; + +protected: + + Vector3 m_axis; + Scalar m_angle; + +public: + + /** Default constructor without initialization. */ + AngleAxis() {} + /** Constructs and initialize the angle-axis rotation from an \a angle in radian + * and an \a axis which must be normalized. */ + template<typename Derived> + inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} + /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */ + inline AngleAxis(const QuaternionType& q) { *this = q; } + /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ + template<typename Derived> + inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } + + Scalar angle() const { return m_angle; } + Scalar& angle() { return m_angle; } + + const Vector3& axis() const { return m_axis; } + Vector3& axis() { return m_axis; } + + /** Concatenates two rotations */ + inline QuaternionType operator* (const AngleAxis& other) const + { return QuaternionType(*this) * QuaternionType(other); } + + /** Concatenates two rotations */ + inline QuaternionType operator* (const QuaternionType& other) const + { return QuaternionType(*this) * other; } + + /** Concatenates two rotations */ + friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) + { return a * QuaternionType(b); } + + /** Concatenates two rotations */ + inline Matrix3 operator* (const Matrix3& other) const + { return toRotationMatrix() * other; } + + /** Concatenates two rotations */ + inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b) + { return a * b.toRotationMatrix(); } + + /** Applies rotation to vector */ + inline Vector3 operator* (const Vector3& other) const + { return toRotationMatrix() * other; } + + /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ + AngleAxis inverse() const + { return AngleAxis(-m_angle, m_axis); } + + AngleAxis& operator=(const QuaternionType& q); + template<typename Derived> + AngleAxis& operator=(const MatrixBase<Derived>& m); + + template<typename Derived> + AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); + 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<typename NewScalarType> + inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const + { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) + { + m_axis = other.axis().template cast<Scalar>(); + m_angle = Scalar(other.angle()); + } + + /** \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 AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); } +}; + +/** \ingroup Geometry_Module + * single precision angle-axis type */ +typedef AngleAxis<float> AngleAxisf; +/** \ingroup Geometry_Module + * double precision angle-axis type */ +typedef AngleAxis<double> AngleAxisd; + +/** Set \c *this from a quaternion. + * The axis is normalized. + */ +template<typename Scalar> +AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q) +{ + Scalar n2 = q.vec().squaredNorm(); + if (n2 < precision<Scalar>()*precision<Scalar>()) + { + m_angle = 0; + m_axis << 1, 0, 0; + } + else + { + m_angle = 2*std::acos(q.w()); + m_axis = q.vec() / ei_sqrt(n2); + } + return *this; +} + +/** Set \c *this from a 3x3 rotation matrix \a mat. + */ +template<typename Scalar> +template<typename Derived> +AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) +{ + // Since a direct conversion would not be really faster, + // let's use the robust Quaternion implementation: + return *this = QuaternionType(mat); +} + +/** Constructs and \returns an equivalent 3x3 rotation matrix. + */ +template<typename Scalar> +typename AngleAxis<Scalar>::Matrix3 +AngleAxis<Scalar>::toRotationMatrix(void) const +{ + Matrix3 res; + Vector3 sin_axis = ei_sin(m_angle) * m_axis; + Scalar c = ei_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.cwise() * m_axis).cwise() + c; + + return res; +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt b/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt new file mode 100644 index 000000000..c347a8f26 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_Eigen2Support_Geometry_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Eigen2Support_Geometry_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Eigen2Support/Geometry + ) diff --git a/Eigen/src/Eigen2Support/Geometry/Hyperplane.h b/Eigen/src/Eigen2Support/Geometry/Hyperplane.h new file mode 100644 index 000000000..19cc1bfd8 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Hyperplane.h @@ -0,0 +1,254 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +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. + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * \param _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 <typename _Scalar, int _AmbientDim> +class Hyperplane +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1) + enum { AmbientDimAtCompileTime = _AmbientDim }; + typedef _Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; + typedef Matrix<Scalar,int(AmbientDimAtCompileTime)==Dynamic + ? Dynamic + : int(AmbientDimAtCompileTime)+1,1> Coefficients; + typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType; + + /** Default constructor without initialization */ + inline explicit Hyperplane() {} + + /** Constructs a dynamic-size hyperplane with \a _dim the dimension + * of the ambient space */ + inline explicit Hyperplane(int _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. + */ + inline Hyperplane(const VectorType& n, const VectorType& e) + : m_coeffs(n.size()+1) + { + normal() = n; + offset() = -e.eigen2_dot(n); + } + + /** 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. + */ + inline Hyperplane(const VectorType& n, 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. + */ + static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) + { + Hyperplane result(p0.size()); + result.normal() = (p1 - p0).unitOrthogonal(); + result.offset() = -result.normal().eigen2_dot(p0); + return result; + } + + /** Constructs a hyperplane passing through the three points. The dimension of the ambient space + * is required to be exactly 3. + */ + 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()); + result.normal() = (p2 - p0).cross(p1 - p0).normalized(); + result.offset() = -result.normal().eigen2_dot(p0); + 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 consitent with the rest this could be implemented as a static Through function ?? + explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) + { + normal() = parametrized.direction().unitOrthogonal(); + offset() = -normal().eigen2_dot(parametrized.origin()); + } + + ~Hyperplane() {} + + /** \returns the dimension in which the plane holds */ + inline int dim() const { return int(AmbientDimAtCompileTime)==Dynamic ? m_coeffs.size()-1 : int(AmbientDimAtCompileTime); } + + /** normalizes \c *this */ + void normalize(void) + { + m_coeffs /= normal().norm(); + } + + /** \returns the signed distance between the plane \c *this and a point \a p. + * \sa absDistance() + */ + inline Scalar signedDistance(const VectorType& p) const { return p.eigen2_dot(normal()) + offset(); } + + /** \returns the absolute distance between the plane \c *this and a point \a p. + * \sa signedDistance() + */ + inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); } + + /** \returns the projection of a point \a p onto the plane \c *this. + */ + 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. + */ + inline const NormalReturnType normal() const { return NormalReturnType(*const_cast<Coefficients*>(&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. + */ + 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. + */ + 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 */ + 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$ + */ + 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$ + */ + 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. + */ + VectorType intersection(const Hyperplane& other) + { + 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(ei_isMuchSmallerThan(det, Scalar(1))) + { // special case where the two lines are approximately parallel. Pick any point on the first line. + if(ei_abs(coeffs().coeff(1))>ei_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<typename XprType> + inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) + { + if (traits==Affine) + normal() = mat.inverse().transpose() * normal(); + else if (traits==Isometry) + normal() = mat * normal(); + else + { + ei_assert("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. + */ + inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t, + TransformTraits traits = Affine) + { + transform(t.linear(), traits); + offset() -= t.translation().eigen2_dot(normal()); + 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<typename NewScalarType> + inline typename internal::cast_return_type<Hyperplane, + Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const + { + return typename internal::cast_return_type<Hyperplane, + Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other) + { m_coeffs = other.coeffs().template cast<Scalar>(); } + + /** \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 Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_coeffs.isApprox(other.m_coeffs, prec); } + +protected: + + Coefficients m_coeffs; +}; + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h b/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h new file mode 100644 index 000000000..6e4a168a8 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h @@ -0,0 +1,141 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +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$ l \in \mathbf{R} \f$. + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. + */ +template <typename _Scalar, int _AmbientDim> +class ParametrizedLine +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) + enum { AmbientDimAtCompileTime = _AmbientDim }; + typedef _Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; + + /** Default constructor without initialization */ + inline explicit ParametrizedLine() {} + + /** Constructs a dynamic-size line with \a _dim the dimension + * of the ambient space */ + inline explicit ParametrizedLine(int _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. + */ + ParametrizedLine(const VectorType& origin, const VectorType& direction) + : m_origin(origin), m_direction(direction) {} + + explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane); + + /** Constructs a parametrized line going from \a p0 to \a p1. */ + static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) + { return ParametrizedLine(p0, (p1-p0).normalized()); } + + ~ParametrizedLine() {} + + /** \returns the dimension in which the line holds */ + inline int dim() const { return m_direction.size(); } + + const VectorType& origin() const { return m_origin; } + VectorType& origin() { return m_origin; } + + const VectorType& direction() const { return m_direction; } + VectorType& direction() { return m_direction; } + + /** \returns the squared distance of a point \a p to its projection onto the line \c *this. + * \sa distance() + */ + RealScalar squaredDistance(const VectorType& p) const + { + VectorType diff = p-origin(); + return (diff - diff.eigen2_dot(direction())* direction()).squaredNorm(); + } + /** \returns the distance of a point \a p to its projection onto the line \c *this. + * \sa squaredDistance() + */ + RealScalar distance(const VectorType& p) const { return ei_sqrt(squaredDistance(p)); } + + /** \returns the projection of a point \a p onto the line \c *this. */ + VectorType projection(const VectorType& p) const + { return origin() + (p-origin()).eigen2_dot(direction()) * direction(); } + + Scalar intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane); + + /** \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<typename NewScalarType> + inline typename internal::cast_return_type<ParametrizedLine, + ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type cast() const + { + return typename internal::cast_return_type<ParametrizedLine, + ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type(*this); + } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime>& other) + { + m_origin = other.origin().template cast<Scalar>(); + m_direction = other.direction().template cast<Scalar>(); + } + + /** \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 ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) 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 <typename _Scalar, int _AmbientDim> +inline ParametrizedLine<_Scalar, _AmbientDim>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) + direction() = hyperplane.normal().unitOrthogonal(); + origin() = -hyperplane.normal()*hyperplane.offset(); +} + +/** \returns the parameter value of the intersection between \c *this and the given hyperplane + */ +template <typename _Scalar, int _AmbientDim> +inline _Scalar ParametrizedLine<_Scalar, _AmbientDim>::intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane) +{ + return -(hyperplane.offset()+origin().eigen2_dot(hyperplane.normal())) + /(direction().eigen2_dot(hyperplane.normal())); +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/Quaternion.h b/Eigen/src/Eigen2Support/Geometry/Quaternion.h new file mode 100644 index 000000000..ec87da054 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Quaternion.h @@ -0,0 +1,495 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +template<typename Other, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> +struct ei_quaternion_assign_impl; + +/** \geometry_module \ingroup Geometry_Module + * + * \class Quaternion + * + * \brief The quaternion class used to represent 3D orientations and rotations + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * + * 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, quatertions 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 + * + * \sa class AngleAxis, class Transform + */ + +template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> > +{ + typedef _Scalar Scalar; +}; + +template<typename _Scalar> +class Quaternion : public RotationBase<Quaternion<_Scalar>,3> +{ + typedef RotationBase<Quaternion<_Scalar>,3> Base; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4) + + using Base::operator*; + + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + + /** the type of the Coefficients 4-vector */ + typedef Matrix<Scalar, 4, 1> Coefficients; + /** the type of a 3D vector */ + typedef Matrix<Scalar,3,1> Vector3; + /** the equivalent rotation matrix type */ + typedef Matrix<Scalar,3,3> Matrix3; + /** the equivalent angle-axis type */ + typedef AngleAxis<Scalar> AngleAxisType; + + /** \returns the \c x coefficient */ + inline Scalar x() const { return m_coeffs.coeff(0); } + /** \returns the \c y coefficient */ + inline Scalar y() const { return m_coeffs.coeff(1); } + /** \returns the \c z coefficient */ + inline Scalar z() const { return m_coeffs.coeff(2); } + /** \returns the \c w coefficient */ + inline Scalar w() const { return m_coeffs.coeff(3); } + + /** \returns a reference to the \c x coefficient */ + inline Scalar& x() { return m_coeffs.coeffRef(0); } + /** \returns a reference to the \c y coefficient */ + inline Scalar& y() { return m_coeffs.coeffRef(1); } + /** \returns a reference to the \c z coefficient */ + inline Scalar& z() { return m_coeffs.coeffRef(2); } + /** \returns a reference to the \c w coefficient */ + inline Scalar& w() { return m_coeffs.coeffRef(3); } + + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + inline const Block<const Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); } + + /** \returns a vector expression of the imaginary part (x,y,z) */ + inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); } + + /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ + inline const Coefficients& coeffs() const { return m_coeffs; } + + /** \returns a vector expression of the coefficients (x,y,z,w) */ + inline Coefficients& coeffs() { return m_coeffs; } + + /** Default constructor leaving the quaternion uninitialized. */ + 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] + */ + inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) + { m_coeffs << x, y, z, w; } + + /** Copy constructor */ + inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } + + /** Constructs and initializes a quaternion from the angle-axis \a aa */ + 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. + * \sa operator=(MatrixBase<Derived>) + */ + template<typename Derived> + explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } + + Quaternion& operator=(const Quaternion& other); + Quaternion& operator=(const AngleAxisType& aa); + template<typename Derived> + Quaternion& operator=(const MatrixBase<Derived>& m); + + /** \returns a quaternion representing an identity rotation + * \sa MatrixBase::Identity() + */ + static inline Quaternion Identity() { return Quaternion(1, 0, 0, 0); } + + /** \sa Quaternion::Identity(), MatrixBase::setIdentity() + */ + inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; } + + /** \returns the squared norm of the quaternion's coefficients + * \sa Quaternion::norm(), MatrixBase::squaredNorm() + */ + inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); } + + /** \returns the norm of the quaternion's coefficients + * \sa Quaternion::squaredNorm(), MatrixBase::norm() + */ + inline Scalar norm() const { return m_coeffs.norm(); } + + /** Normalizes the quaternion \c *this + * \sa normalized(), MatrixBase::normalize() */ + inline void normalize() { m_coeffs.normalize(); } + /** \returns a normalized version of \c *this + * \sa normalize(), MatrixBase::normalized() */ + inline Quaternion normalized() const { return Quaternion(m_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() + */ + inline Scalar eigen2_dot(const Quaternion& other) const { return m_coeffs.eigen2_dot(other.m_coeffs); } + + inline Scalar angularDistance(const Quaternion& other) const; + + Matrix3 toRotationMatrix(void) const; + + template<typename Derived1, typename Derived2> + Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); + + inline Quaternion operator* (const Quaternion& q) const; + inline Quaternion& operator*= (const Quaternion& q); + + Quaternion inverse(void) const; + Quaternion conjugate(void) const; + + Quaternion slerp(Scalar t, const Quaternion& other) const; + + template<typename Derived> + Vector3 operator* (const MatrixBase<Derived>& vec) 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<typename NewScalarType> + inline typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const + { return typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Quaternion(const Quaternion<OtherScalarType>& other) + { m_coeffs = other.coeffs().template cast<Scalar>(); } + + /** \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 Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_coeffs.isApprox(other.m_coeffs, prec); } + +protected: + Coefficients m_coeffs; +}; + +/** \ingroup Geometry_Module + * single precision quaternion type */ +typedef Quaternion<float> Quaternionf; +/** \ingroup Geometry_Module + * double precision quaternion type */ +typedef Quaternion<double> Quaterniond; + +// Generic Quaternion * Quaternion product +template<typename Scalar> inline Quaternion<Scalar> +ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b) +{ + return Quaternion<Scalar> + ( + 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() + ); +} + +/** \returns the concatenation of two rotations as a quaternion-quaternion product */ +template <typename Scalar> +inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const +{ + return ei_quaternion_product(*this,other); +} + +/** \sa operator*(Quaternion) */ +template <typename Scalar> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other) +{ + return (*this = *this * other); +} + +/** 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: + * - Quaternion: 30n + * - Via a Matrix3: 24 + 15n + */ +template <typename Scalar> +template<typename Derived> +inline typename Quaternion<Scalar>::Vector3 +Quaternion<Scalar>::operator* (const MatrixBase<Derived>& 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 litterature (30 versus 39 flops). It also requires two + // Vector3 as temporaries. + Vector3 uv; + uv = 2 * this->vec().cross(v); + return v + this->w() * uv + this->vec().cross(uv); +} + +template<typename Scalar> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other) +{ + m_coeffs = other.m_coeffs; + return *this; +} + +/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this + */ +template<typename Scalar> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa) +{ + Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings + this->w() = ei_cos(ha); + this->vec() = ei_sin(ha) * aa.axis(); + return *this; +} + +/** 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<typename Scalar> +template<typename Derived> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr) +{ + ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived()); + return *this; +} + +/** Convert the quaternion to a 3x3 rotation matrix */ +template<typename Scalar> +inline typename Quaternion<Scalar>::Matrix3 +Quaternion<Scalar>::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 *this to be a quaternion representing a rotation sending the vector \a a to the vector \a b. + * + * \returns a reference to *this. + * + * Note that the two input vectors do \b not have to be normalized. + */ +template<typename Scalar> +template<typename Derived1, typename Derived2> +inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) +{ + Vector3 v0 = a.normalized(); + Vector3 v1 = b.normalized(); + Scalar c = v0.eigen2_dot(v1); + + // if dot == 1, vectors are the same + if (ei_isApprox(c,Scalar(1))) + { + // set to identity + this->w() = 1; this->vec().setZero(); + return *this; + } + // if dot == -1, vectors are opposites + if (ei_isApprox(c,Scalar(-1))) + { + this->vec() = v0.unitOrthogonal(); + this->w() = 0; + return *this; + } + + Vector3 axis = v0.cross(v1); + Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2)); + Scalar invs = Scalar(1)/s; + this->vec() = axis * invs; + this->w() = s * Scalar(0.5); + + return *this; +} + +/** \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 Quaternion::conjugate() + */ +template <typename Scalar> +inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const +{ + // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? + Scalar n2 = this->squaredNorm(); + if (n2 > 0) + return Quaternion(conjugate().coeffs() / n2); + else + { + // return an invalid result to flag the error + return Quaternion(Coefficients::Zero()); + } +} + +/** \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 Quaternion::inverse() + */ +template <typename Scalar> +inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const +{ + return Quaternion(this->w(),-this->x(),-this->y(),-this->z()); +} + +/** \returns the angle (in radian) between two rotations + * \sa eigen2_dot() + */ +template <typename Scalar> +inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const +{ + double d = ei_abs(this->eigen2_dot(other)); + if (d>=1.0) + return 0; + return Scalar(2) * std::acos(d); +} + +/** \returns the spherical linear interpolation between the two quaternions + * \c *this and \a other at the parameter \a t + */ +template <typename Scalar> +Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const +{ + static const Scalar one = Scalar(1) - machine_epsilon<Scalar>(); + Scalar d = this->eigen2_dot(other); + Scalar absD = ei_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 = std::acos(absD); + Scalar sinTheta = ei_sin(theta); + + scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; + scale1 = ei_sin( ( t * theta) ) / sinTheta; + if (d<0) + scale1 = -scale1; + } + + return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); +} + +// set from a rotation matrix +template<typename Other> +struct ei_quaternion_assign_impl<Other,3,3> +{ + typedef typename Other::Scalar Scalar; + static inline void run(Quaternion<Scalar>& q, const Other& mat) + { + // This algorithm comes from "Quaternion Calculus and Fast Animation", + // Ken Shoemake, 1987 SIGGRAPH course notes + Scalar t = mat.trace(); + if (t > 0) + { + t = ei_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 + { + int i = 0; + if (mat.coeff(1,1) > mat.coeff(0,0)) + i = 1; + if (mat.coeff(2,2) > mat.coeff(i,i)) + i = 2; + int j = (i+1)%3; + int k = (j+1)%3; + + t = ei_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<typename Other> +struct ei_quaternion_assign_impl<Other,4,1> +{ + typedef typename Other::Scalar Scalar; + static inline void run(Quaternion<Scalar>& q, const Other& vec) + { + q.coeffs() = vec; + } +}; + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/Rotation2D.h b/Eigen/src/Eigen2Support/Geometry/Rotation2D.h new file mode 100644 index 000000000..3e02b7a4f --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Rotation2D.h @@ -0,0 +1,145 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Rotation2D + * + * \brief Represents a rotation/orientation in a 2 dimensional space. + * + * \param _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 + */ +template<typename _Scalar> struct ei_traits<Rotation2D<_Scalar> > +{ + typedef _Scalar Scalar; +}; + +template<typename _Scalar> +class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2> +{ + typedef RotationBase<Rotation2D<_Scalar>,2> Base; + +public: + + using Base::operator*; + + enum { Dim = 2 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix<Scalar,2,1> Vector2; + typedef Matrix<Scalar,2,2> Matrix2; + +protected: + + Scalar m_angle; + +public: + + /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ + inline Rotation2D(Scalar a) : m_angle(a) {} + + /** \returns the rotation angle */ + inline Scalar angle() const { return m_angle; } + + /** \returns a read-write reference to the rotation angle */ + inline Scalar& angle() { return m_angle; } + + /** \returns the inverse rotation */ + inline Rotation2D inverse() const { return -m_angle; } + + /** Concatenates two rotations */ + inline Rotation2D operator*(const Rotation2D& other) const + { return m_angle + other.m_angle; } + + /** Concatenates two rotations */ + inline Rotation2D& operator*=(const Rotation2D& other) + { return m_angle += other.m_angle; return *this; } + + /** Applies the rotation to a 2D vector */ + Vector2 operator* (const Vector2& vec) const + { return toRotationMatrix() * vec; } + + template<typename Derived> + Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m); + Matrix2 toRotationMatrix(void) const; + + /** \returns the spherical interpolation between \c *this and \a other using + * parameter \a t. It is in fact equivalent to a linear interpolation. + */ + inline Rotation2D slerp(Scalar t, const Rotation2D& other) const + { return m_angle * (1-t) + other.angle() * 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<typename NewScalarType> + inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const + { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) + { + m_angle = Scalar(other.angle()); + } + + /** \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 Rotation2D& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return ei_isApprox(m_angle,other.m_angle, prec); } +}; + +/** \ingroup Geometry_Module + * single precision 2D rotation type */ +typedef Rotation2D<float> Rotation2Df; +/** \ingroup Geometry_Module + * double precision 2D rotation type */ +typedef Rotation2D<double> 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<typename Scalar> +template<typename Derived> +Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) +{ + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) + m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0)); + return *this; +} + +/** Constructs and \returns an equivalent 2x2 rotation matrix. + */ +template<typename Scalar> +typename Rotation2D<Scalar>::Matrix2 +Rotation2D<Scalar>::toRotationMatrix(void) const +{ + Scalar sinA = ei_sin(m_angle); + Scalar cosA = ei_cos(m_angle); + return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/RotationBase.h b/Eigen/src/Eigen2Support/Geometry/RotationBase.h new file mode 100644 index 000000000..78ad73b60 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/RotationBase.h @@ -0,0 +1,123 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +// this file aims to contains the various representations of rotation/orientation +// in 2D and 3D space excepted Matrix and Quaternion. + +/** \class RotationBase + * + * \brief Common base class for compact rotation representations + * + * \param Derived is the derived type, i.e., a rotation type + * \param _Dim the dimension of the space + */ +template<typename Derived, int _Dim> +class RotationBase +{ + public: + enum { Dim = _Dim }; + /** the scalar type of the coefficients */ + typedef typename ei_traits<Derived>::Scalar Scalar; + + /** corresponding linear transformation matrix type */ + typedef Matrix<Scalar,Dim,Dim> RotationMatrixType; + + inline const Derived& derived() const { return *static_cast<const Derived*>(this); } + inline Derived& derived() { return *static_cast<Derived*>(this); } + + /** \returns an equivalent rotation matrix */ + inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } + + /** \returns the inverse rotation */ + inline Derived inverse() const { return derived().inverse(); } + + /** \returns the concatenation of the rotation \c *this with a translation \a t */ + inline Transform<Scalar,Dim> operator*(const Translation<Scalar,Dim>& t) const + { return toRotationMatrix() * t; } + + /** \returns the concatenation of the rotation \c *this with a scaling \a s */ + inline RotationMatrixType operator*(const Scaling<Scalar,Dim>& s) const + { return toRotationMatrix() * s; } + + /** \returns the concatenation of the rotation \c *this with an affine transformation \a t */ + inline Transform<Scalar,Dim> operator*(const Transform<Scalar,Dim>& t) const + { return toRotationMatrix() * t; } +}; + +/** \geometry_module + * + * Constructs a Dim x Dim rotation matrix from the rotation \a r + */ +template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> +template<typename OtherDerived> +Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> +::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) + *this = r.toRotationMatrix(); +} + +/** \geometry_module + * + * Set a Dim x Dim rotation matrix from the rotation \a r + */ +template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> +template<typename OtherDerived> +Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>& +Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> +::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) + return *this = r.toRotationMatrix(); +} + +/** \internal + * + * Helper function to return an arbitrary rotation object to a rotation matrix. + * + * \param Scalar the numeric type of the matrix coefficients + * \param 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 ei_toRotationMatrix is only used by Transform. + * + * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis + */ +template<typename Scalar, int Dim> +static inline Matrix<Scalar,2,2> ei_toRotationMatrix(const Scalar& s) +{ + EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE) + return Rotation2D<Scalar>(s).toRotationMatrix(); +} + +template<typename Scalar, int Dim, typename OtherDerived> +static inline Matrix<Scalar,Dim,Dim> ei_toRotationMatrix(const RotationBase<OtherDerived,Dim>& r) +{ + return r.toRotationMatrix(); +} + +template<typename Scalar, int Dim, typename OtherDerived> +static inline const MatrixBase<OtherDerived>& ei_toRotationMatrix(const MatrixBase<OtherDerived>& mat) +{ + EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, + YOU_MADE_A_PROGRAMMING_MISTAKE) + return mat; +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/Scaling.h b/Eigen/src/Eigen2Support/Geometry/Scaling.h new file mode 100644 index 000000000..a07c1c7c7 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Scaling.h @@ -0,0 +1,167 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Scaling + * + * \brief Represents a possibly non uniform scaling transformation + * + * \param _Scalar the scalar type, i.e., the type of the coefficients. + * \param _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 scaling transformation, + * but rather to make easier the constructions and updates of Transform objects. + * + * \sa class Translation, class Transform + */ +template<typename _Scalar, int _Dim> +class Scaling +{ +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<Scalar,Dim,1> VectorType; + /** corresponding linear transformation matrix type */ + typedef Matrix<Scalar,Dim,Dim> LinearMatrixType; + /** corresponding translation type */ + typedef Translation<Scalar,Dim> TranslationType; + /** corresponding affine transformation type */ + typedef Transform<Scalar,Dim> TransformType; + +protected: + + VectorType m_coeffs; + +public: + + /** Default constructor without initialization. */ + Scaling() {} + /** Constructs and initialize a uniform scaling transformation */ + explicit inline Scaling(const Scalar& s) { m_coeffs.setConstant(s); } + /** 2D only */ + inline Scaling(const Scalar& sx, const Scalar& sy) + { + ei_assert(Dim==2); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + } + /** 3D only */ + inline Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz) + { + ei_assert(Dim==3); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + m_coeffs.z() = sz; + } + /** Constructs and initialize the scaling transformation from a vector of scaling coefficients */ + explicit inline Scaling(const VectorType& coeffs) : m_coeffs(coeffs) {} + + const VectorType& coeffs() const { return m_coeffs; } + VectorType& coeffs() { return m_coeffs; } + + /** Concatenates two scaling */ + inline Scaling operator* (const Scaling& other) const + { return Scaling(coeffs().cwise() * other.coeffs()); } + + /** Concatenates a scaling and a translation */ + inline TransformType operator* (const TranslationType& t) const; + + /** Concatenates a scaling and an affine transformation */ + inline TransformType operator* (const TransformType& t) const; + + /** Concatenates a scaling and a linear transformation matrix */ + // TODO returns an expression + inline LinearMatrixType operator* (const LinearMatrixType& other) const + { return coeffs().asDiagonal() * other; } + + /** Concatenates a linear transformation matrix and a scaling */ + // TODO returns an expression + friend inline LinearMatrixType operator* (const LinearMatrixType& other, const Scaling& s) + { return other * s.coeffs().asDiagonal(); } + + template<typename Derived> + inline LinearMatrixType operator*(const RotationBase<Derived,Dim>& r) const + { return *this * r.toRotationMatrix(); } + + /** Applies scaling to vector */ + inline VectorType operator* (const VectorType& other) const + { return coeffs().asDiagonal() * other; } + + /** \returns the inverse scaling */ + inline Scaling inverse() const + { return Scaling(coeffs().cwise().inverse()); } + + inline Scaling& operator=(const Scaling& other) + { + m_coeffs = other.m_coeffs; + 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<typename NewScalarType> + inline typename internal::cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type cast() const + { return typename internal::cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Scaling(const Scaling<OtherScalarType,Dim>& other) + { m_coeffs = other.coeffs().template cast<Scalar>(); } + + /** \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 Scaling& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_coeffs.isApprox(other.m_coeffs, prec); } + +}; + +/** \addtogroup Geometry_Module */ +//@{ +typedef Scaling<float, 2> Scaling2f; +typedef Scaling<double,2> Scaling2d; +typedef Scaling<float, 3> Scaling3f; +typedef Scaling<double,3> Scaling3d; +//@} + +template<typename Scalar, int Dim> +inline typename Scaling<Scalar,Dim>::TransformType +Scaling<Scalar,Dim>::operator* (const TranslationType& t) const +{ + TransformType res; + res.matrix().setZero(); + res.linear().diagonal() = coeffs(); + res.translation() = m_coeffs.cwise() * t.vector(); + res(Dim,Dim) = Scalar(1); + return res; +} + +template<typename Scalar, int Dim> +inline typename Scaling<Scalar,Dim>::TransformType +Scaling<Scalar,Dim>::operator* (const TransformType& t) const +{ + TransformType res = t; + res.prescale(m_coeffs); + return res; +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/Transform.h b/Eigen/src/Eigen2Support/Geometry/Transform.h new file mode 100644 index 000000000..dceb80203 --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Transform.h @@ -0,0 +1,786 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +// Note that we have to pass Dim and HDim because it is not allowed to use a template +// parameter to define a template specialization. To be more precise, in the following +// specializations, it is not allowed to use Dim+1 instead of HDim. +template< typename Other, + int Dim, + int HDim, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> +struct ei_transform_product_impl; + +/** \geometry_module \ingroup Geometry_Module + * + * \class Transform + * + * \brief Represents an homogeneous transformation in a N dimensional space + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * \param _Dim the dimension of the space + * + * The homography is internally represented and stored as a (Dim+1)^2 matrix which + * is available through the matrix() method. + * + * Conversion methods from/to Qt's QMatrix and QTransform are available if the + * preprocessor token EIGEN_QT_SUPPORT is defined. + * + * \sa class Matrix, class Quaternion + */ +template<typename _Scalar, int _Dim> +class Transform +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1)) + enum { + Dim = _Dim, ///< space dimension in which the transformation holds + HDim = _Dim+1 ///< size of a respective homogeneous vector + }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + /** type of the matrix used to represent the transformation */ + typedef Matrix<Scalar,HDim,HDim> MatrixType; + /** type of the matrix used to represent the linear part of the transformation */ + typedef Matrix<Scalar,Dim,Dim> LinearMatrixType; + /** type of read/write reference to the linear part of the transformation */ + typedef Block<MatrixType,Dim,Dim> LinearPart; + /** type of read/write reference to the linear part of the transformation */ + typedef const Block<const MatrixType,Dim,Dim> ConstLinearPart; + /** type of a vector */ + typedef Matrix<Scalar,Dim,1> VectorType; + /** type of a read/write reference to the translation part of the rotation */ + typedef Block<MatrixType,Dim,1> TranslationPart; + /** type of a read/write reference to the translation part of the rotation */ + typedef const Block<const MatrixType,Dim,1> ConstTranslationPart; + /** corresponding translation type */ + typedef Translation<Scalar,Dim> TranslationType; + /** corresponding scaling transformation type */ + typedef Scaling<Scalar,Dim> ScalingType; + +protected: + + MatrixType m_matrix; + +public: + + /** Default constructor without initialization of the coefficients. */ + inline Transform() { } + + inline Transform(const Transform& other) + { + m_matrix = other.m_matrix; + } + + inline explicit Transform(const TranslationType& t) { *this = t; } + inline explicit Transform(const ScalingType& s) { *this = s; } + template<typename Derived> + inline explicit Transform(const RotationBase<Derived, Dim>& r) { *this = r; } + + inline Transform& operator=(const Transform& other) + { m_matrix = other.m_matrix; return *this; } + + template<typename OtherDerived, bool BigMatrix> // MSVC 2005 will commit suicide if BigMatrix has a default value + struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other) + { + transform->matrix() = other; + } + }; + + template<typename OtherDerived> struct construct_from_matrix<OtherDerived, true> + { + static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other) + { + transform->linear() = other; + transform->translation().setZero(); + transform->matrix()(Dim,Dim) = Scalar(1); + transform->matrix().template block<1,Dim>(Dim,0).setZero(); + } + }; + + /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */ + template<typename OtherDerived> + inline explicit Transform(const MatrixBase<OtherDerived>& other) + { + construct_from_matrix<OtherDerived, int(OtherDerived::RowsAtCompileTime) == Dim>::run(this, other); + } + + /** Set \c *this from a (Dim+1)^2 matrix. */ + template<typename OtherDerived> + inline Transform& operator=(const MatrixBase<OtherDerived>& other) + { m_matrix = other; return *this; } + + #ifdef EIGEN_QT_SUPPORT + inline Transform(const QMatrix& other); + inline Transform& operator=(const QMatrix& other); + inline QMatrix toQMatrix(void) const; + inline Transform(const QTransform& other); + inline Transform& operator=(const QTransform& other); + inline QTransform toQTransform(void) const; + #endif + + /** shortcut for m_matrix(row,col); + * \sa MatrixBase::operaror(int,int) const */ + inline Scalar operator() (int row, int col) const { return m_matrix(row,col); } + /** shortcut for m_matrix(row,col); + * \sa MatrixBase::operaror(int,int) */ + inline Scalar& operator() (int row, int col) { return m_matrix(row,col); } + + /** \returns a read-only expression of the transformation matrix */ + inline const MatrixType& matrix() const { return m_matrix; } + /** \returns a writable expression of the transformation matrix */ + inline MatrixType& matrix() { return m_matrix; } + + /** \returns a read-only expression of the linear (linear) part of the transformation */ + inline ConstLinearPart linear() const { return m_matrix.template block<Dim,Dim>(0,0); } + /** \returns a writable expression of the linear (linear) part of the transformation */ + inline LinearPart linear() { return m_matrix.template block<Dim,Dim>(0,0); } + + /** \returns a read-only expression of the translation vector of the transformation */ + inline ConstTranslationPart translation() const { return m_matrix.template block<Dim,1>(0,Dim); } + /** \returns a writable expression of the translation vector of the transformation */ + inline TranslationPart translation() { return m_matrix.template block<Dim,1>(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 might be either: + * \li a vector of size Dim, + * \li an homogeneous vector of size Dim+1, + * \li a transformation matrix of size Dim+1 x Dim+1. + */ + // note: this function is defined here because some compilers cannot find the respective declaration + template<typename OtherDerived> + inline const typename ei_transform_product_impl<OtherDerived,_Dim,_Dim+1>::ResultType + operator * (const MatrixBase<OtherDerived> &other) const + { return ei_transform_product_impl<OtherDerived,Dim,HDim>::run(*this,other.derived()); } + + /** \returns the product expression of a transformation matrix \a a times a transform \a b + * The transformation matrix \a a must have a Dim+1 x Dim+1 sizes. */ + template<typename OtherDerived> + friend inline const typename ProductReturnType<OtherDerived,MatrixType>::Type + operator * (const MatrixBase<OtherDerived> &a, const Transform &b) + { return a.derived() * b.matrix(); } + + /** Contatenates two transformations */ + inline const Transform + operator * (const Transform& other) const + { return Transform(m_matrix * other.matrix()); } + + /** \sa MatrixBase::setIdentity() */ + void setIdentity() { m_matrix.setIdentity(); } + static const typename MatrixType::IdentityReturnType Identity() + { + return MatrixType::Identity(); + } + + template<typename OtherDerived> + inline Transform& scale(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + inline Transform& prescale(const MatrixBase<OtherDerived> &other); + + inline Transform& scale(Scalar s); + inline Transform& prescale(Scalar s); + + template<typename OtherDerived> + inline Transform& translate(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + inline Transform& pretranslate(const MatrixBase<OtherDerived> &other); + + template<typename RotationType> + inline Transform& rotate(const RotationType& rotation); + + template<typename RotationType> + inline Transform& prerotate(const RotationType& rotation); + + Transform& shear(Scalar sx, Scalar sy); + Transform& preshear(Scalar sx, Scalar sy); + + inline Transform& operator=(const TranslationType& t); + inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } + inline Transform operator*(const TranslationType& t) const; + + inline Transform& operator=(const ScalingType& t); + inline Transform& operator*=(const ScalingType& s) { return scale(s.coeffs()); } + inline Transform operator*(const ScalingType& s) const; + friend inline Transform operator*(const LinearMatrixType& mat, const Transform& t) + { + Transform res = t; + res.matrix().row(Dim) = t.matrix().row(Dim); + res.matrix().template block<Dim,HDim>(0,0) = (mat * t.matrix().template block<Dim,HDim>(0,0)).lazy(); + return res; + } + + template<typename Derived> + inline Transform& operator=(const RotationBase<Derived,Dim>& r); + template<typename Derived> + inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); } + template<typename Derived> + inline Transform operator*(const RotationBase<Derived,Dim>& r) const; + + LinearMatrixType rotation() const; + template<typename RotationMatrixType, typename ScalingMatrixType> + void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; + template<typename ScalingMatrixType, typename RotationMatrixType> + void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const; + + template<typename PositionDerived, typename OrientationType, typename ScaleDerived> + Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, + const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale); + + inline const MatrixType inverse(TransformTraits traits = Affine) const; + + /** \returns a const pointer to the column major internal matrix */ + const Scalar* data() const { return m_matrix.data(); } + /** \returns a non-const pointer to the column major internal matrix */ + 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<typename NewScalarType> + inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim> >::type cast() const + { return typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Transform(const Transform<OtherScalarType,Dim>& other) + { m_matrix = other.matrix().template cast<Scalar>(); } + + /** \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 Transform& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_matrix.isApprox(other.m_matrix, prec); } + + #ifdef EIGEN_TRANSFORM_PLUGIN + #include EIGEN_TRANSFORM_PLUGIN + #endif + +protected: + +}; + +/** \ingroup Geometry_Module */ +typedef Transform<float,2> Transform2f; +/** \ingroup Geometry_Module */ +typedef Transform<float,3> Transform3f; +/** \ingroup Geometry_Module */ +typedef Transform<double,2> Transform2d; +/** \ingroup Geometry_Module */ +typedef Transform<double,3> Transform3d; + +/************************** +*** Optional QT support *** +**************************/ + +#ifdef EIGEN_QT_SUPPORT +/** Initialises \c *this from a QMatrix assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template<typename Scalar, int Dim> +Transform<Scalar,Dim>::Transform(const QMatrix& other) +{ + *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<typename Scalar, int Dim> +Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QMatrix& other) +{ + EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) + 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 convertion might loss data if \c *this is not affine + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template<typename Scalar, int Dim> +QMatrix Transform<Scalar,Dim>::toQMatrix(void) const +{ + 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)); +} + +/** Initialises \c *this from a QTransform assuming the dimension is 2. + * + * This function is available only if the token EIGEN_QT_SUPPORT is defined. + */ +template<typename Scalar, int Dim> +Transform<Scalar,Dim>::Transform(const QTransform& other) +{ + *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<typename Scalar, int Dim> +Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QTransform& other) +{ + EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) + 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<typename Scalar, int Dim> +QTransform Transform<Scalar,Dim>::toQTransform(void) const +{ + EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) + 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<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) + linear() = (linear() * other.asDiagonal()).lazy(); + 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<typename Scalar, int Dim> +inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::scale(Scalar s) +{ + linear() *= 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<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) + m_matrix.template block<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)).lazy(); + 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<typename Scalar, int Dim> +inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::prescale(Scalar s) +{ + m_matrix.template corner<Dim,HDim>(TopLeft) *= 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<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) + translation() += linear() * 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<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) + 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 ei_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<typename Scalar, int Dim> +template<typename RotationType> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::rotate(const RotationType& rotation) +{ + linear() *= ei_toRotationMatrix<Scalar,Dim>(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<typename Scalar, int Dim> +template<typename RotationType> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::prerotate(const RotationType& rotation) +{ + m_matrix.template block<Dim,HDim>(0,0) = ei_toRotationMatrix<Scalar,Dim>(rotation) + * m_matrix.template block<Dim,HDim>(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<typename Scalar, int Dim> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::shear(Scalar sx, Scalar sy) +{ + EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) + 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<typename Scalar, int Dim> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::preshear(Scalar sx, Scalar sy) +{ + EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) + m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0); + return *this; +} + +/****************************************************** +*** Scaling, Translation and Rotation compatibility *** +******************************************************/ + +template<typename Scalar, int Dim> +inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const TranslationType& t) +{ + linear().setIdentity(); + translation() = t.vector(); + m_matrix.template block<1,Dim>(Dim,0).setZero(); + m_matrix(Dim,Dim) = Scalar(1); + return *this; +} + +template<typename Scalar, int Dim> +inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const TranslationType& t) const +{ + Transform res = *this; + res.translate(t.vector()); + return res; +} + +template<typename Scalar, int Dim> +inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const ScalingType& s) +{ + m_matrix.setZero(); + linear().diagonal() = s.coeffs(); + m_matrix.coeffRef(Dim,Dim) = Scalar(1); + return *this; +} + +template<typename Scalar, int Dim> +inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const ScalingType& s) const +{ + Transform res = *this; + res.scale(s.coeffs()); + return res; +} + +template<typename Scalar, int Dim> +template<typename Derived> +inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const RotationBase<Derived,Dim>& r) +{ + linear() = ei_toRotationMatrix<Scalar,Dim>(r); + translation().setZero(); + m_matrix.template block<1,Dim>(Dim,0).setZero(); + m_matrix.coeffRef(Dim,Dim) = Scalar(1); + return *this; +} + +template<typename Scalar, int Dim> +template<typename Derived> +inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const RotationBase<Derived,Dim>& r) const +{ + Transform res = *this; + res.rotate(r.derived()); + return res; +} + +/************************ +*** Special functions *** +************************/ + +/** \returns the rotation part of the transformation + * \nonstableyet + * + * \svd_module + * + * \sa computeRotationScaling(), computeScalingRotation(), class SVD + */ +template<typename Scalar, int Dim> +typename Transform<Scalar,Dim>::LinearMatrixType +Transform<Scalar,Dim>::rotation() const +{ + LinearMatrixType result; + computeRotationScaling(&result, (LinearMatrixType*)0); + return result; +} + + +/** 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. + * + * \nonstableyet + * + * \svd_module + * + * \sa computeScalingRotation(), rotation(), class SVD + */ +template<typename Scalar, int Dim> +template<typename RotationMatrixType, typename ScalingMatrixType> +void Transform<Scalar,Dim>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const +{ + JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU|ComputeFullV); + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 + Matrix<Scalar, Dim, 1> sv(svd.singularValues()); + sv.coeffRef(0) *= x; + if(scaling) + { + scaling->noalias() = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); + } + if(rotation) + { + LinearMatrixType m(svd.matrixU()); + m.col(0) /= x; + rotation->noalias() = m * svd.matrixV().adjoint(); + } +} + +/** 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. + * + * \nonstableyet + * + * \svd_module + * + * \sa computeRotationScaling(), rotation(), class SVD + */ +template<typename Scalar, int Dim> +template<typename ScalingMatrixType, typename RotationMatrixType> +void Transform<Scalar,Dim>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const +{ + JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU|ComputeFullV); + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 + Matrix<Scalar, Dim, 1> sv(svd.singularValues()); + sv.coeffRef(0) *= x; + if(scaling) + { + scaling->noalias() = svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint(); + } + if(rotation) + { + LinearMatrixType m(svd.matrixU()); + m.col(0) /= x; + rotation->noalias() = m * svd.matrixV().adjoint(); + } +} + +/** Convenient method to set \c *this from a position, orientation and scale + * of a 3D object. + */ +template<typename Scalar, int Dim> +template<typename PositionDerived, typename OrientationType, typename ScaleDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, + const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale) +{ + linear() = ei_toRotationMatrix<Scalar,Dim>(orientation); + linear() *= scale.asDiagonal(); + translation() = position; + m_matrix.template block<1,Dim>(Dim,0).setZero(); + m_matrix(Dim,Dim) = Scalar(1); + return *this; +} + +/** \nonstableyet + * + * \returns the inverse transformation matrix according to some given knowledge + * on \c *this. + * + * \param traits allows to optimize the inversion process when the transformion + * is known to be not a general transformation. 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 is the default, the last row is assumed to be [0 ... 0 1] + * - Isometry if the transformation is only a concatenations of translations + * and rotations. + * + * \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<typename Scalar, int Dim> +inline const typename Transform<Scalar,Dim>::MatrixType +Transform<Scalar,Dim>::inverse(TransformTraits traits) const +{ + if (traits == Projective) + { + return m_matrix.inverse(); + } + else + { + MatrixType res; + if (traits == Affine) + { + res.template corner<Dim,Dim>(TopLeft) = linear().inverse(); + } + else if (traits == Isometry) + { + res.template corner<Dim,Dim>(TopLeft) = linear().transpose(); + } + else + { + ei_assert("invalid traits value in Transform::inverse()"); + } + // translation and remaining parts + res.template corner<Dim,1>(TopRight) = - res.template corner<Dim,Dim>(TopLeft) * translation(); + res.template corner<1,Dim>(BottomLeft).setZero(); + res.coeffRef(Dim,Dim) = Scalar(1); + return res; + } +} + +/***************************************************** +*** Specializations of operator* with a MatrixBase *** +*****************************************************/ + +template<typename Other, int Dim, int HDim> +struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim> +{ + typedef Transform<typename Other::Scalar,Dim> TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return tr.matrix() * other; } +}; + +template<typename Other, int Dim, int HDim> +struct ei_transform_product_impl<Other,Dim,HDim, Dim,Dim> +{ + typedef Transform<typename Other::Scalar,Dim> TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef TransformType ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { + TransformType res; + res.translation() = tr.translation(); + res.matrix().row(Dim) = tr.matrix().row(Dim); + res.linear() = (tr.linear() * other).lazy(); + return res; + } +}; + +template<typename Other, int Dim, int HDim> +struct ei_transform_product_impl<Other,Dim,HDim, HDim,1> +{ + typedef Transform<typename Other::Scalar,Dim> TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return tr.matrix() * other; } +}; + +template<typename Other, int Dim, int HDim> +struct ei_transform_product_impl<Other,Dim,HDim, Dim,1> +{ + typedef typename Other::Scalar Scalar; + typedef Transform<Scalar,Dim> TransformType; + typedef Matrix<Scalar,Dim,1> ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return ((tr.linear() * other) + tr.translation()) + * (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); } +}; + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/Geometry/Translation.h b/Eigen/src/Eigen2Support/Geometry/Translation.h new file mode 100644 index 000000000..0fb9a9f9a --- /dev/null +++ b/Eigen/src/Eigen2Support/Geometry/Translation.h @@ -0,0 +1,184 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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/. + +// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Translation + * + * \brief Represents a translation transformation + * + * \param _Scalar the scalar type, i.e., the type of the coefficients. + * \param _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<typename _Scalar, int _Dim> +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<Scalar,Dim,1> VectorType; + /** corresponding linear transformation matrix type */ + typedef Matrix<Scalar,Dim,Dim> LinearMatrixType; + /** corresponding scaling transformation type */ + typedef Scaling<Scalar,Dim> ScalingType; + /** corresponding affine transformation type */ + typedef Transform<Scalar,Dim> TransformType; + +protected: + + VectorType m_coeffs; + +public: + + /** Default constructor without initialization. */ + Translation() {} + /** */ + inline Translation(const Scalar& sx, const Scalar& sy) + { + ei_assert(Dim==2); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + } + /** */ + inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) + { + ei_assert(Dim==3); + m_coeffs.x() = sx; + m_coeffs.y() = sy; + m_coeffs.z() = sz; + } + /** Constructs and initialize the scaling transformation from a vector of scaling coefficients */ + explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} + + const VectorType& vector() const { return m_coeffs; } + VectorType& vector() { return m_coeffs; } + + /** Concatenates two translation */ + inline Translation operator* (const Translation& other) const + { return Translation(m_coeffs + other.m_coeffs); } + + /** Concatenates a translation and a scaling */ + inline TransformType operator* (const ScalingType& other) const; + + /** Concatenates a translation and a linear transformation */ + inline TransformType operator* (const LinearMatrixType& linear) const; + + template<typename Derived> + inline TransformType operator*(const RotationBase<Derived,Dim>& r) const + { return *this * r.toRotationMatrix(); } + + /** Concatenates a linear transformation and a translation */ + // its a nightmare to define a templated friend function outside its declaration + friend inline TransformType operator* (const LinearMatrixType& linear, const Translation& t) + { + TransformType res; + res.matrix().setZero(); + res.linear() = linear; + res.translation() = linear * t.m_coeffs; + res.matrix().row(Dim).setZero(); + res(Dim,Dim) = Scalar(1); + return res; + } + + /** Concatenates a translation and an affine transformation */ + inline TransformType operator* (const TransformType& t) const; + + /** Applies translation to vector */ + inline VectorType operator* (const VectorType& other) const + { return m_coeffs + other; } + + /** \returns the inverse translation (opposite) */ + Translation inverse() const { return Translation(-m_coeffs); } + + Translation& operator=(const Translation& other) + { + m_coeffs = other.m_coeffs; + 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<typename NewScalarType> + inline typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const + { return typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit Translation(const Translation<OtherScalarType,Dim>& other) + { m_coeffs = other.vector().template cast<Scalar>(); } + + /** \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 Translation& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const + { return m_coeffs.isApprox(other.m_coeffs, prec); } + +}; + +/** \addtogroup Geometry_Module */ +//@{ +typedef Translation<float, 2> Translation2f; +typedef Translation<double,2> Translation2d; +typedef Translation<float, 3> Translation3f; +typedef Translation<double,3> Translation3d; +//@} + + +template<typename Scalar, int Dim> +inline typename Translation<Scalar,Dim>::TransformType +Translation<Scalar,Dim>::operator* (const ScalingType& other) const +{ + TransformType res; + res.matrix().setZero(); + res.linear().diagonal() = other.coeffs(); + res.translation() = m_coeffs; + res(Dim,Dim) = Scalar(1); + return res; +} + +template<typename Scalar, int Dim> +inline typename Translation<Scalar,Dim>::TransformType +Translation<Scalar,Dim>::operator* (const LinearMatrixType& linear) const +{ + TransformType res; + res.matrix().setZero(); + res.linear() = linear; + res.translation() = m_coeffs; + res.matrix().row(Dim).setZero(); + res(Dim,Dim) = Scalar(1); + return res; +} + +template<typename Scalar, int Dim> +inline typename Translation<Scalar,Dim>::TransformType +Translation<Scalar,Dim>::operator* (const TransformType& t) const +{ + TransformType res = t; + res.pretranslate(m_coeffs); + return res; +} + +} // end namespace Eigen diff --git a/Eigen/src/Eigen2Support/LU.h b/Eigen/src/Eigen2Support/LU.h new file mode 100644 index 000000000..49f19ad76 --- /dev/null +++ b/Eigen/src/Eigen2Support/LU.h @@ -0,0 +1,120 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_LU_H +#define EIGEN2_LU_H + +namespace Eigen { + +template<typename MatrixType> +class LU : public FullPivLU<MatrixType> +{ + public: + + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar; + typedef Matrix<int, 1, MatrixType::ColsAtCompileTime, MatrixType::Options, 1, MatrixType::MaxColsAtCompileTime> IntRowVectorType; + typedef Matrix<int, MatrixType::RowsAtCompileTime, 1, MatrixType::Options, MatrixType::MaxRowsAtCompileTime, 1> IntColVectorType; + typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime, MatrixType::Options, 1, MatrixType::MaxColsAtCompileTime> RowVectorType; + typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1, MatrixType::Options, MatrixType::MaxRowsAtCompileTime, 1> ColVectorType; + + typedef Matrix<typename MatrixType::Scalar, + MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" is the number of cols of the original matrix + // so that the product "matrix * kernel = zero" makes sense + Dynamic, // we don't know at compile-time the dimension of the kernel + MatrixType::Options, + MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter + MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, whose dimension is the number + // of columns of the original matrix + > KernelResultType; + + typedef Matrix<typename MatrixType::Scalar, + MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose dimension is the number + // of rows of the original matrix + Dynamic, // we don't know at compile time the dimension of the image (the rank) + MatrixType::Options, + MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix, + MatrixType::MaxColsAtCompileTime // so it has the same number of rows and at most as many columns. + > ImageResultType; + + typedef FullPivLU<MatrixType> Base; + + template<typename T> + explicit LU(const T& t) : Base(t), m_originalMatrix(t) {} + + template<typename OtherDerived, typename ResultType> + bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const + { + *result = static_cast<const Base*>(this)->solve(b); + return true; + } + + template<typename ResultType> + inline void computeInverse(ResultType *result) const + { + solve(MatrixType::Identity(this->rows(), this->cols()), result); + } + + template<typename KernelMatrixType> + void computeKernel(KernelMatrixType *result) const + { + *result = static_cast<const Base*>(this)->kernel(); + } + + template<typename ImageMatrixType> + void computeImage(ImageMatrixType *result) const + { + *result = static_cast<const Base*>(this)->image(m_originalMatrix); + } + + const ImageResultType image() const + { + return static_cast<const Base*>(this)->image(m_originalMatrix); + } + + const MatrixType& m_originalMatrix; +}; + +#if EIGEN2_SUPPORT_STAGE < STAGE20_RESOLVE_API_CONFLICTS +/** \lu_module + * + * Synonym of partialPivLu(). + * + * \return the partial-pivoting LU decomposition of \c *this. + * + * \sa class PartialPivLU + */ +template<typename Derived> +inline const LU<typename MatrixBase<Derived>::PlainObject> +MatrixBase<Derived>::lu() const +{ + return LU<PlainObject>(eval()); +} +#endif + +#ifdef EIGEN2_SUPPORT +/** \lu_module + * + * Synonym of partialPivLu(). + * + * \return the partial-pivoting LU decomposition of \c *this. + * + * \sa class PartialPivLU + */ +template<typename Derived> +inline const LU<typename MatrixBase<Derived>::PlainObject> +MatrixBase<Derived>::eigen2_lu() const +{ + return LU<PlainObject>(eval()); +} +#endif + +} // end namespace Eigen + +#endif // EIGEN2_LU_H diff --git a/Eigen/src/Eigen2Support/Lazy.h b/Eigen/src/Eigen2Support/Lazy.h new file mode 100644 index 000000000..593fc78e6 --- /dev/null +++ b/Eigen/src/Eigen2Support/Lazy.h @@ -0,0 +1,71 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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_LAZY_H +#define EIGEN_LAZY_H + +namespace Eigen { + +/** \deprecated it is only used by lazy() which is deprecated + * + * \returns an expression of *this with added flags + * + * Example: \include MatrixBase_marked.cpp + * Output: \verbinclude MatrixBase_marked.out + * + * \sa class Flagged, extract(), part() + */ +template<typename Derived> +template<unsigned int Added> +inline const Flagged<Derived, Added, 0> +MatrixBase<Derived>::marked() const +{ + return derived(); +} + +/** \deprecated use MatrixBase::noalias() + * + * \returns an expression of *this with the EvalBeforeAssigningBit flag removed. + * + * Example: \include MatrixBase_lazy.cpp + * Output: \verbinclude MatrixBase_lazy.out + * + * \sa class Flagged, marked() + */ +template<typename Derived> +inline const Flagged<Derived, 0, EvalBeforeAssigningBit> +MatrixBase<Derived>::lazy() const +{ + return derived(); +} + + +/** \internal + * Overloaded to perform an efficient C += (A*B).lazy() */ +template<typename Derived> +template<typename ProductDerived, typename Lhs, typename Rhs> +Derived& MatrixBase<Derived>::operator+=(const Flagged<ProductBase<ProductDerived, Lhs,Rhs>, 0, + EvalBeforeAssigningBit>& other) +{ + other._expression().derived().addTo(derived()); return derived(); +} + +/** \internal + * Overloaded to perform an efficient C -= (A*B).lazy() */ +template<typename Derived> +template<typename ProductDerived, typename Lhs, typename Rhs> +Derived& MatrixBase<Derived>::operator-=(const Flagged<ProductBase<ProductDerived, Lhs,Rhs>, 0, + EvalBeforeAssigningBit>& other) +{ + other._expression().derived().subTo(derived()); return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_LAZY_H diff --git a/Eigen/src/Eigen2Support/LeastSquares.h b/Eigen/src/Eigen2Support/LeastSquares.h new file mode 100644 index 000000000..7aff428dc --- /dev/null +++ b/Eigen/src/Eigen2Support/LeastSquares.h @@ -0,0 +1,170 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_LEASTSQUARES_H +#define EIGEN2_LEASTSQUARES_H + +namespace Eigen { + +/** \ingroup LeastSquares_Module + * + * \leastsquares_module + * + * For a set of points, this function tries to express + * one of the coords as a linear (affine) function of the other coords. + * + * This is best explained by an example. This function works in full + * generality, for points in a space of arbitrary dimension, and also over + * the complex numbers, but for this example we will work in dimension 3 + * over the real numbers (doubles). + * + * So let us work with the following set of 5 points given by their + * \f$(x,y,z)\f$ coordinates: + * @code + Vector3d points[5]; + points[0] = Vector3d( 3.02, 6.89, -4.32 ); + points[1] = Vector3d( 2.01, 5.39, -3.79 ); + points[2] = Vector3d( 2.41, 6.01, -4.01 ); + points[3] = Vector3d( 2.09, 5.55, -3.86 ); + points[4] = Vector3d( 2.58, 6.32, -4.10 ); + * @endcode + * Suppose that we want to express the second coordinate (\f$y\f$) as a linear + * expression in \f$x\f$ and \f$z\f$, that is, + * \f[ y=ax+bz+c \f] + * for some constants \f$a,b,c\f$. Thus, we want to find the best possible + * constants \f$a,b,c\f$ so that the plane of equation \f$y=ax+bz+c\f$ fits + * best the five above points. To do that, call this function as follows: + * @code + Vector3d coeffs; // will store the coefficients a, b, c + linearRegression( + 5, + &points, + &coeffs, + 1 // the coord to express as a function of + // the other ones. 0 means x, 1 means y, 2 means z. + ); + * @endcode + * Now the vector \a coeffs is approximately + * \f$( 0.495 , -1.927 , -2.906 )\f$. + * Thus, we get \f$a=0.495, b = -1.927, c = -2.906\f$. Let us check for + * instance how near points[0] is from the plane of equation \f$y=ax+bz+c\f$. + * Looking at the coords of points[0], we see that: + * \f[ax+bz+c = 0.495 * 3.02 + (-1.927) * (-4.32) + (-2.906) = 6.91.\f] + * On the other hand, we have \f$y=6.89\f$. We see that the values + * \f$6.91\f$ and \f$6.89\f$ + * are near, so points[0] is very near the plane of equation \f$y=ax+bz+c\f$. + * + * Let's now describe precisely the parameters: + * @param numPoints the number of points + * @param points the array of pointers to the points on which to perform the linear regression + * @param result pointer to the vector in which to store the result. + This vector must be of the same type and size as the + data points. The meaning of its coords is as follows. + For brevity, let \f$n=Size\f$, + \f$r_i=result[i]\f$, + and \f$f=funcOfOthers\f$. Denote by + \f$x_0,\ldots,x_{n-1}\f$ + the n coordinates in the n-dimensional space. + Then the resulting equation is: + \f[ x_f = r_0 x_0 + \cdots + r_{f-1}x_{f-1} + + r_{f+1}x_{f+1} + \cdots + r_{n-1}x_{n-1} + r_n. \f] + * @param funcOfOthers Determines which coord to express as a function of the + others. Coords are numbered starting from 0, so that a + value of 0 means \f$x\f$, 1 means \f$y\f$, + 2 means \f$z\f$, ... + * + * \sa fitHyperplane() + */ +template<typename VectorType> +void linearRegression(int numPoints, + VectorType **points, + VectorType *result, + int funcOfOthers ) +{ + typedef typename VectorType::Scalar Scalar; + typedef Hyperplane<Scalar, VectorType::SizeAtCompileTime> HyperplaneType; + const int size = points[0]->size(); + result->resize(size); + HyperplaneType h(size); + fitHyperplane(numPoints, points, &h); + for(int i = 0; i < funcOfOthers; i++) + result->coeffRef(i) = - h.coeffs()[i] / h.coeffs()[funcOfOthers]; + for(int i = funcOfOthers; i < size; i++) + result->coeffRef(i) = - h.coeffs()[i+1] / h.coeffs()[funcOfOthers]; +} + +/** \ingroup LeastSquares_Module + * + * \leastsquares_module + * + * This function is quite similar to linearRegression(), so we refer to the + * documentation of this function and only list here the differences. + * + * The main difference from linearRegression() is that this function doesn't + * take a \a funcOfOthers argument. Instead, it finds a general equation + * of the form + * \f[ r_0 x_0 + \cdots + r_{n-1}x_{n-1} + r_n = 0, \f] + * where \f$n=Size\f$, \f$r_i=retCoefficients[i]\f$, and we denote by + * \f$x_0,\ldots,x_{n-1}\f$ the n coordinates in the n-dimensional space. + * + * Thus, the vector \a retCoefficients has size \f$n+1\f$, which is another + * difference from linearRegression(). + * + * In practice, this function performs an hyper-plane fit in a total least square sense + * via the following steps: + * 1 - center the data to the mean + * 2 - compute the covariance matrix + * 3 - pick the eigenvector corresponding to the smallest eigenvalue of the covariance matrix + * The ratio of the smallest eigenvalue and the second one gives us a hint about the relevance + * of the solution. This value is optionally returned in \a soundness. + * + * \sa linearRegression() + */ +template<typename VectorType, typename HyperplaneType> +void fitHyperplane(int numPoints, + VectorType **points, + HyperplaneType *result, + typename NumTraits<typename VectorType::Scalar>::Real* soundness = 0) +{ + typedef typename VectorType::Scalar Scalar; + typedef Matrix<Scalar,VectorType::SizeAtCompileTime,VectorType::SizeAtCompileTime> CovMatrixType; + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType) + ei_assert(numPoints >= 1); + int size = points[0]->size(); + ei_assert(size+1 == result->coeffs().size()); + + // compute the mean of the data + VectorType mean = VectorType::Zero(size); + for(int i = 0; i < numPoints; ++i) + mean += *(points[i]); + mean /= numPoints; + + // compute the covariance matrix + CovMatrixType covMat = CovMatrixType::Zero(size, size); + VectorType remean = VectorType::Zero(size); + for(int i = 0; i < numPoints; ++i) + { + VectorType diff = (*(points[i]) - mean).conjugate(); + covMat += diff * diff.adjoint(); + } + + // now we just have to pick the eigen vector with smallest eigen value + SelfAdjointEigenSolver<CovMatrixType> eig(covMat); + result->normal() = eig.eigenvectors().col(0); + if (soundness) + *soundness = eig.eigenvalues().coeff(0)/eig.eigenvalues().coeff(1); + + // let's compute the constant coefficient such that the + // plane pass trough the mean point: + result->offset() = - (result->normal().cwise()* mean).sum(); +} + +} // end namespace Eigen + +#endif // EIGEN2_LEASTSQUARES_H diff --git a/Eigen/src/Eigen2Support/Macros.h b/Eigen/src/Eigen2Support/Macros.h new file mode 100644 index 000000000..351c32afb --- /dev/null +++ b/Eigen/src/Eigen2Support/Macros.h @@ -0,0 +1,20 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_MACROS_H +#define EIGEN2_MACROS_H + +#define ei_assert eigen_assert +#define ei_internal_assert eigen_internal_assert + +#define EIGEN_ALIGN_128 EIGEN_ALIGN16 + +#define EIGEN_ARCH_WANTS_ALIGNMENT EIGEN_ALIGN_STATICALLY + +#endif // EIGEN2_MACROS_H diff --git a/Eigen/src/Eigen2Support/MathFunctions.h b/Eigen/src/Eigen2Support/MathFunctions.h new file mode 100644 index 000000000..3a8a9ca81 --- /dev/null +++ b/Eigen/src/Eigen2Support/MathFunctions.h @@ -0,0 +1,57 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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 EIGEN2_MATH_FUNCTIONS_H +#define EIGEN2_MATH_FUNCTIONS_H + +namespace Eigen { + +template<typename T> inline typename NumTraits<T>::Real ei_real(const T& x) { return internal::real(x); } +template<typename T> inline typename NumTraits<T>::Real ei_imag(const T& x) { return internal::imag(x); } +template<typename T> inline T ei_conj(const T& x) { return internal::conj(x); } +template<typename T> inline typename NumTraits<T>::Real ei_abs (const T& x) { return internal::abs(x); } +template<typename T> inline typename NumTraits<T>::Real ei_abs2(const T& x) { return internal::abs2(x); } +template<typename T> inline T ei_sqrt(const T& x) { return internal::sqrt(x); } +template<typename T> inline T ei_exp (const T& x) { return internal::exp(x); } +template<typename T> inline T ei_log (const T& x) { return internal::log(x); } +template<typename T> inline T ei_sin (const T& x) { return internal::sin(x); } +template<typename T> inline T ei_cos (const T& x) { return internal::cos(x); } +template<typename T> inline T ei_atan2(const T& x,const T& y) { return internal::atan2(x,y); } +template<typename T> inline T ei_pow (const T& x,const T& y) { return internal::pow(x,y); } +template<typename T> inline T ei_random () { return internal::random<T>(); } +template<typename T> inline T ei_random (const T& x, const T& y) { return internal::random(x, y); } + +template<typename T> inline T precision () { return NumTraits<T>::dummy_precision(); } +template<typename T> inline T machine_epsilon () { return NumTraits<T>::epsilon(); } + + +template<typename Scalar, typename OtherScalar> +inline bool ei_isMuchSmallerThan(const Scalar& x, const OtherScalar& y, + typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision()) +{ + return internal::isMuchSmallerThan(x, y, precision); +} + +template<typename Scalar> +inline bool ei_isApprox(const Scalar& x, const Scalar& y, + typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision()) +{ + return internal::isApprox(x, y, precision); +} + +template<typename Scalar> +inline bool ei_isApproxOrLessThan(const Scalar& x, const Scalar& y, + typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision()) +{ + return internal::isApproxOrLessThan(x, y, precision); +} + +} // end namespace Eigen + +#endif // EIGEN2_MATH_FUNCTIONS_H diff --git a/Eigen/src/Eigen2Support/Memory.h b/Eigen/src/Eigen2Support/Memory.h new file mode 100644 index 000000000..f86372b6b --- /dev/null +++ b/Eigen/src/Eigen2Support/Memory.h @@ -0,0 +1,45 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_MEMORY_H +#define EIGEN2_MEMORY_H + +namespace Eigen { + +inline void* ei_aligned_malloc(size_t size) { return internal::aligned_malloc(size); } +inline void ei_aligned_free(void *ptr) { internal::aligned_free(ptr); } +inline void* ei_aligned_realloc(void *ptr, size_t new_size, size_t old_size) { return internal::aligned_realloc(ptr, new_size, old_size); } +inline void* ei_handmade_aligned_malloc(size_t size) { return internal::handmade_aligned_malloc(size); } +inline void ei_handmade_aligned_free(void *ptr) { internal::handmade_aligned_free(ptr); } + +template<bool Align> inline void* ei_conditional_aligned_malloc(size_t size) +{ + return internal::conditional_aligned_malloc<Align>(size); +} +template<bool Align> inline void ei_conditional_aligned_free(void *ptr) +{ + internal::conditional_aligned_free<Align>(ptr); +} +template<bool Align> inline void* ei_conditional_aligned_realloc(void* ptr, size_t new_size, size_t old_size) +{ + return internal::conditional_aligned_realloc<Align>(ptr, new_size, old_size); +} + +template<typename T> inline T* ei_aligned_new(size_t size) +{ + return internal::aligned_new<T>(size); +} +template<typename T> inline void ei_aligned_delete(T *ptr, size_t size) +{ + return internal::aligned_delete(ptr, size); +} + +} // end namespace Eigen + +#endif // EIGEN2_MACROS_H diff --git a/Eigen/src/Eigen2Support/Meta.h b/Eigen/src/Eigen2Support/Meta.h new file mode 100644 index 000000000..fa37cfc96 --- /dev/null +++ b/Eigen/src/Eigen2Support/Meta.h @@ -0,0 +1,75 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_META_H +#define EIGEN2_META_H + +namespace Eigen { + +template<typename T> +struct ei_traits : internal::traits<T> +{}; + +struct ei_meta_true { enum { ret = 1 }; }; +struct ei_meta_false { enum { ret = 0 }; }; + +template<bool Condition, typename Then, typename Else> +struct ei_meta_if { typedef Then ret; }; + +template<typename Then, typename Else> +struct ei_meta_if <false, Then, Else> { typedef Else ret; }; + +template<typename T, typename U> struct ei_is_same_type { enum { ret = 0 }; }; +template<typename T> struct ei_is_same_type<T,T> { enum { ret = 1 }; }; + +template<typename T> struct ei_unref { typedef T type; }; +template<typename T> struct ei_unref<T&> { typedef T type; }; + +template<typename T> struct ei_unpointer { typedef T type; }; +template<typename T> struct ei_unpointer<T*> { typedef T type; }; +template<typename T> struct ei_unpointer<T*const> { typedef T type; }; + +template<typename T> struct ei_unconst { typedef T type; }; +template<typename T> struct ei_unconst<const T> { typedef T type; }; +template<typename T> struct ei_unconst<T const &> { typedef T & type; }; +template<typename T> struct ei_unconst<T const *> { typedef T * type; }; + +template<typename T> struct ei_cleantype { typedef T type; }; +template<typename T> struct ei_cleantype<const T> { typedef typename ei_cleantype<T>::type type; }; +template<typename T> struct ei_cleantype<const T&> { typedef typename ei_cleantype<T>::type type; }; +template<typename T> struct ei_cleantype<T&> { typedef typename ei_cleantype<T>::type type; }; +template<typename T> struct ei_cleantype<const T*> { typedef typename ei_cleantype<T>::type type; }; +template<typename T> struct ei_cleantype<T*> { typedef typename ei_cleantype<T>::type type; }; + +/** \internal In short, it computes int(sqrt(\a Y)) with \a Y an integer. + * Usage example: \code ei_meta_sqrt<1023>::ret \endcode + */ +template<int Y, + int InfX = 0, + int SupX = ((Y==1) ? 1 : Y/2), + bool Done = ((SupX-InfX)<=1 ? true : ((SupX*SupX <= Y) && ((SupX+1)*(SupX+1) > Y))) > + // use ?: instead of || just to shut up a stupid gcc 4.3 warning +class ei_meta_sqrt +{ + enum { + MidX = (InfX+SupX)/2, + TakeInf = MidX*MidX > Y ? 1 : 0, + NewInf = int(TakeInf) ? InfX : int(MidX), + NewSup = int(TakeInf) ? int(MidX) : SupX + }; + public: + enum { ret = ei_meta_sqrt<Y,NewInf,NewSup>::ret }; +}; + +template<int Y, int InfX, int SupX> +class ei_meta_sqrt<Y, InfX, SupX, true> { public: enum { ret = (SupX*SupX <= Y) ? SupX : InfX }; }; + +} // end namespace Eigen + +#endif // EIGEN2_META_H diff --git a/Eigen/src/Eigen2Support/Minor.h b/Eigen/src/Eigen2Support/Minor.h new file mode 100644 index 000000000..4cded5734 --- /dev/null +++ b/Eigen/src/Eigen2Support/Minor.h @@ -0,0 +1,117 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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_MINOR_H +#define EIGEN_MINOR_H + +namespace Eigen { + +/** + * \class Minor + * + * \brief Expression of a minor + * + * \param MatrixType the type of the object in which we are taking a minor + * + * This class represents an expression of a minor. It is the return + * type of MatrixBase::minor() and most of the time this is the only way it + * is used. + * + * \sa MatrixBase::minor() + */ + +namespace internal { +template<typename MatrixType> +struct traits<Minor<MatrixType> > + : traits<MatrixType> +{ + typedef typename nested<MatrixType>::type MatrixTypeNested; + typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested; + typedef typename MatrixType::StorageKind StorageKind; + enum { + RowsAtCompileTime = (MatrixType::RowsAtCompileTime != Dynamic) ? + int(MatrixType::RowsAtCompileTime) - 1 : Dynamic, + ColsAtCompileTime = (MatrixType::ColsAtCompileTime != Dynamic) ? + int(MatrixType::ColsAtCompileTime) - 1 : Dynamic, + MaxRowsAtCompileTime = (MatrixType::MaxRowsAtCompileTime != Dynamic) ? + int(MatrixType::MaxRowsAtCompileTime) - 1 : Dynamic, + MaxColsAtCompileTime = (MatrixType::MaxColsAtCompileTime != Dynamic) ? + int(MatrixType::MaxColsAtCompileTime) - 1 : Dynamic, + Flags = _MatrixTypeNested::Flags & (HereditaryBits | LvalueBit), + CoeffReadCost = _MatrixTypeNested::CoeffReadCost // minor is used typically on tiny matrices, + // where loops are unrolled and the 'if' evaluates at compile time + }; +}; +} + +template<typename MatrixType> class Minor + : public MatrixBase<Minor<MatrixType> > +{ + public: + + typedef MatrixBase<Minor> Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Minor) + + inline Minor(const MatrixType& matrix, + Index row, Index col) + : m_matrix(matrix), m_row(row), m_col(col) + { + eigen_assert(row >= 0 && row < matrix.rows() + && col >= 0 && col < matrix.cols()); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Minor) + + inline Index rows() const { return m_matrix.rows() - 1; } + inline Index cols() const { return m_matrix.cols() - 1; } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_matrix.const_cast_derived().coeffRef(row + (row >= m_row), col + (col >= m_col)); + } + + inline const Scalar coeff(Index row, Index col) const + { + return m_matrix.coeff(row + (row >= m_row), col + (col >= m_col)); + } + + protected: + const typename MatrixType::Nested m_matrix; + const Index m_row, m_col; +}; + +/** + * \return an expression of the (\a row, \a col)-minor of *this, + * i.e. an expression constructed from *this by removing the specified + * row and column. + * + * Example: \include MatrixBase_minor.cpp + * Output: \verbinclude MatrixBase_minor.out + * + * \sa class Minor + */ +template<typename Derived> +inline Minor<Derived> +MatrixBase<Derived>::minor(Index row, Index col) +{ + return Minor<Derived>(derived(), row, col); +} + +/** + * This is the const version of minor(). */ +template<typename Derived> +inline const Minor<Derived> +MatrixBase<Derived>::minor(Index row, Index col) const +{ + return Minor<Derived>(derived(), row, col); +} + +} // end namespace Eigen + +#endif // EIGEN_MINOR_H diff --git a/Eigen/src/Eigen2Support/QR.h b/Eigen/src/Eigen2Support/QR.h new file mode 100644 index 000000000..2042c9851 --- /dev/null +++ b/Eigen/src/Eigen2Support/QR.h @@ -0,0 +1,67 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_QR_H +#define EIGEN2_QR_H + +namespace Eigen { + +template<typename MatrixType> +class QR : public HouseholderQR<MatrixType> +{ + public: + + typedef HouseholderQR<MatrixType> Base; + typedef Block<const MatrixType, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixRBlockType; + + QR() : Base() {} + + template<typename T> + explicit QR(const T& t) : Base(t) {} + + template<typename OtherDerived, typename ResultType> + bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const + { + *result = static_cast<const Base*>(this)->solve(b); + return true; + } + + MatrixType matrixQ(void) const { + MatrixType ret = MatrixType::Identity(this->rows(), this->cols()); + ret = this->householderQ() * ret; + return ret; + } + + bool isFullRank() const { + return true; + } + + const TriangularView<MatrixRBlockType, UpperTriangular> + matrixR(void) const + { + int cols = this->cols(); + return MatrixRBlockType(this->matrixQR(), 0, 0, cols, cols).template triangularView<UpperTriangular>(); + } +}; + +/** \return the QR decomposition of \c *this. + * + * \sa class QR + */ +template<typename Derived> +const QR<typename MatrixBase<Derived>::PlainObject> +MatrixBase<Derived>::qr() const +{ + return QR<PlainObject>(eval()); +} + +} // end namespace Eigen + +#endif // EIGEN2_QR_H diff --git a/Eigen/src/Eigen2Support/SVD.h b/Eigen/src/Eigen2Support/SVD.h new file mode 100644 index 000000000..3d2eeb445 --- /dev/null +++ b/Eigen/src/Eigen2Support/SVD.h @@ -0,0 +1,638 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// 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 EIGEN2_SVD_H +#define EIGEN2_SVD_H + +namespace Eigen { + +/** \ingroup SVD_Module + * \nonstableyet + * + * \class SVD + * + * \brief Standard SVD decomposition of a matrix and associated features + * + * \param MatrixType the type of the matrix of which we are computing the SVD decomposition + * + * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N + * with \c M \>= \c N. + * + * + * \sa MatrixBase::SVD() + */ +template<typename MatrixType> class SVD +{ + private: + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar; + + enum { + PacketSize = internal::packet_traits<Scalar>::size, + AlignmentMask = int(PacketSize)-1, + MinSize = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime) + }; + + typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector; + typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector; + + typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType; + typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType; + typedef Matrix<Scalar, MinSize, 1> SingularValuesType; + + public: + + SVD() {} // a user who relied on compiler-generated default compiler reported problems with MSVC in 2.0.7 + + SVD(const MatrixType& matrix) + : m_matU(matrix.rows(), (std::min)(matrix.rows(), matrix.cols())), + m_matV(matrix.cols(),matrix.cols()), + m_sigma((std::min)(matrix.rows(),matrix.cols())) + { + compute(matrix); + } + + template<typename OtherDerived, typename ResultType> + bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const; + + const MatrixUType& matrixU() const { return m_matU; } + const SingularValuesType& singularValues() const { return m_sigma; } + const MatrixVType& matrixV() const { return m_matV; } + + void compute(const MatrixType& matrix); + SVD& sort(); + + template<typename UnitaryType, typename PositiveType> + void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const; + template<typename PositiveType, typename UnitaryType> + void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const; + template<typename RotationType, typename ScalingType> + void computeRotationScaling(RotationType *unitary, ScalingType *positive) const; + template<typename ScalingType, typename RotationType> + void computeScalingRotation(ScalingType *positive, RotationType *unitary) const; + + protected: + /** \internal */ + MatrixUType m_matU; + /** \internal */ + MatrixVType m_matV; + /** \internal */ + SingularValuesType m_sigma; +}; + +/** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix + * + * \note this code has been adapted from JAMA (public domain) + */ +template<typename MatrixType> +void SVD<MatrixType>::compute(const MatrixType& matrix) +{ + const int m = matrix.rows(); + const int n = matrix.cols(); + const int nu = (std::min)(m,n); + ei_assert(m>=n && "In Eigen 2.0, SVD only works for MxN matrices with M>=N. Sorry!"); + ei_assert(m>1 && "In Eigen 2.0, SVD doesn't work on 1x1 matrices"); + + m_matU.resize(m, nu); + m_matU.setZero(); + m_sigma.resize((std::min)(m,n)); + m_matV.resize(n,n); + + RowVector e(n); + ColVector work(m); + MatrixType matA(matrix); + const bool wantu = true; + const bool wantv = true; + int i=0, j=0, k=0; + + // Reduce A to bidiagonal form, storing the diagonal elements + // in s and the super-diagonal elements in e. + int nct = (std::min)(m-1,n); + int nrt = (std::max)(0,(std::min)(n-2,m)); + for (k = 0; k < (std::max)(nct,nrt); ++k) + { + if (k < nct) + { + // Compute the transformation for the k-th column and + // place the k-th diagonal in m_sigma[k]. + m_sigma[k] = matA.col(k).end(m-k).norm(); + if (m_sigma[k] != 0.0) // FIXME + { + if (matA(k,k) < 0.0) + m_sigma[k] = -m_sigma[k]; + matA.col(k).end(m-k) /= m_sigma[k]; + matA(k,k) += 1.0; + } + m_sigma[k] = -m_sigma[k]; + } + + for (j = k+1; j < n; ++j) + { + if ((k < nct) && (m_sigma[k] != 0.0)) + { + // Apply the transformation. + Scalar t = matA.col(k).end(m-k).eigen2_dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ?? + t = -t/matA(k,k); + matA.col(j).end(m-k) += t * matA.col(k).end(m-k); + } + + // Place the k-th row of A into e for the + // subsequent calculation of the row transformation. + e[j] = matA(k,j); + } + + // Place the transformation in U for subsequent back multiplication. + if (wantu & (k < nct)) + m_matU.col(k).end(m-k) = matA.col(k).end(m-k); + + if (k < nrt) + { + // Compute the k-th row transformation and place the + // k-th super-diagonal in e[k]. + e[k] = e.end(n-k-1).norm(); + if (e[k] != 0.0) + { + if (e[k+1] < 0.0) + e[k] = -e[k]; + e.end(n-k-1) /= e[k]; + e[k+1] += 1.0; + } + e[k] = -e[k]; + if ((k+1 < m) & (e[k] != 0.0)) + { + // Apply the transformation. + work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1); + for (j = k+1; j < n; ++j) + matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1); + } + + // Place the transformation in V for subsequent back multiplication. + if (wantv) + m_matV.col(k).end(n-k-1) = e.end(n-k-1); + } + } + + + // Set up the final bidiagonal matrix or order p. + int p = (std::min)(n,m+1); + if (nct < n) + m_sigma[nct] = matA(nct,nct); + if (m < p) + m_sigma[p-1] = 0.0; + if (nrt+1 < p) + e[nrt] = matA(nrt,p-1); + e[p-1] = 0.0; + + // If required, generate U. + if (wantu) + { + for (j = nct; j < nu; ++j) + { + m_matU.col(j).setZero(); + m_matU(j,j) = 1.0; + } + for (k = nct-1; k >= 0; k--) + { + if (m_sigma[k] != 0.0) + { + for (j = k+1; j < nu; ++j) + { + Scalar t = m_matU.col(k).end(m-k).eigen2_dot(m_matU.col(j).end(m-k)); // FIXME is it really a dot product we want ? + t = -t/m_matU(k,k); + m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k); + } + m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k); + m_matU(k,k) = Scalar(1) + m_matU(k,k); + if (k-1>0) + m_matU.col(k).start(k-1).setZero(); + } + else + { + m_matU.col(k).setZero(); + m_matU(k,k) = 1.0; + } + } + } + + // If required, generate V. + if (wantv) + { + for (k = n-1; k >= 0; k--) + { + if ((k < nrt) & (e[k] != 0.0)) + { + for (j = k+1; j < nu; ++j) + { + Scalar t = m_matV.col(k).end(n-k-1).eigen2_dot(m_matV.col(j).end(n-k-1)); // FIXME is it really a dot product we want ? + t = -t/m_matV(k+1,k); + m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1); + } + } + m_matV.col(k).setZero(); + m_matV(k,k) = 1.0; + } + } + + // Main iteration loop for the singular values. + int pp = p-1; + int iter = 0; + Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52)); + while (p > 0) + { + int k=0; + int kase=0; + + // Here is where a test for too many iterations would go. + + // This section of the program inspects for + // negligible elements in the s and e arrays. On + // completion the variables kase and k are set as follows. + + // kase = 1 if s(p) and e[k-1] are negligible and k<p + // kase = 2 if s(k) is negligible and k<p + // kase = 3 if e[k-1] is negligible, k<p, and + // s(k), ..., s(p) are not negligible (qr step). + // kase = 4 if e(p-1) is negligible (convergence). + + for (k = p-2; k >= -1; --k) + { + if (k == -1) + break; + if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1]))) + { + e[k] = 0.0; + break; + } + } + if (k == p-2) + { + kase = 4; + } + else + { + int ks; + for (ks = p-1; ks >= k; --ks) + { + if (ks == k) + break; + Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0)); + if (ei_abs(m_sigma[ks]) <= eps*t) + { + m_sigma[ks] = 0.0; + break; + } + } + if (ks == k) + { + kase = 3; + } + else if (ks == p-1) + { + kase = 1; + } + else + { + kase = 2; + k = ks; + } + } + ++k; + + // Perform the task indicated by kase. + switch (kase) + { + + // Deflate negligible s(p). + case 1: + { + Scalar f(e[p-2]); + e[p-2] = 0.0; + for (j = p-2; j >= k; --j) + { + Scalar t(internal::hypot(m_sigma[j],f)); + Scalar cs(m_sigma[j]/t); + Scalar sn(f/t); + m_sigma[j] = t; + if (j != k) + { + f = -sn*e[j-1]; + e[j-1] = cs*e[j-1]; + } + if (wantv) + { + for (i = 0; i < n; ++i) + { + t = cs*m_matV(i,j) + sn*m_matV(i,p-1); + m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1); + m_matV(i,j) = t; + } + } + } + } + break; + + // Split at negligible s(k). + case 2: + { + Scalar f(e[k-1]); + e[k-1] = 0.0; + for (j = k; j < p; ++j) + { + Scalar t(internal::hypot(m_sigma[j],f)); + Scalar cs( m_sigma[j]/t); + Scalar sn(f/t); + m_sigma[j] = t; + f = -sn*e[j]; + e[j] = cs*e[j]; + if (wantu) + { + for (i = 0; i < m; ++i) + { + t = cs*m_matU(i,j) + sn*m_matU(i,k-1); + m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1); + m_matU(i,j) = t; + } + } + } + } + break; + + // Perform one qr step. + case 3: + { + // Calculate the shift. + Scalar scale = (std::max)((std::max)((std::max)((std::max)( + ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])), + ei_abs(m_sigma[k])),ei_abs(e[k])); + Scalar sp = m_sigma[p-1]/scale; + Scalar spm1 = m_sigma[p-2]/scale; + Scalar epm1 = e[p-2]/scale; + Scalar sk = m_sigma[k]/scale; + Scalar ek = e[k]/scale; + Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2); + Scalar c = (sp*epm1)*(sp*epm1); + Scalar shift(0); + if ((b != 0.0) || (c != 0.0)) + { + shift = ei_sqrt(b*b + c); + if (b < 0.0) + shift = -shift; + shift = c/(b + shift); + } + Scalar f = (sk + sp)*(sk - sp) + shift; + Scalar g = sk*ek; + + // Chase zeros. + + for (j = k; j < p-1; ++j) + { + Scalar t = internal::hypot(f,g); + Scalar cs = f/t; + Scalar sn = g/t; + if (j != k) + e[j-1] = t; + f = cs*m_sigma[j] + sn*e[j]; + e[j] = cs*e[j] - sn*m_sigma[j]; + g = sn*m_sigma[j+1]; + m_sigma[j+1] = cs*m_sigma[j+1]; + if (wantv) + { + for (i = 0; i < n; ++i) + { + t = cs*m_matV(i,j) + sn*m_matV(i,j+1); + m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1); + m_matV(i,j) = t; + } + } + t = internal::hypot(f,g); + cs = f/t; + sn = g/t; + m_sigma[j] = t; + f = cs*e[j] + sn*m_sigma[j+1]; + m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1]; + g = sn*e[j+1]; + e[j+1] = cs*e[j+1]; + if (wantu && (j < m-1)) + { + for (i = 0; i < m; ++i) + { + t = cs*m_matU(i,j) + sn*m_matU(i,j+1); + m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1); + m_matU(i,j) = t; + } + } + } + e[p-2] = f; + iter = iter + 1; + } + break; + + // Convergence. + case 4: + { + // Make the singular values positive. + if (m_sigma[k] <= 0.0) + { + m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0); + if (wantv) + m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1); + } + + // Order the singular values. + while (k < pp) + { + if (m_sigma[k] >= m_sigma[k+1]) + break; + Scalar t = m_sigma[k]; + m_sigma[k] = m_sigma[k+1]; + m_sigma[k+1] = t; + if (wantv && (k < n-1)) + m_matV.col(k).swap(m_matV.col(k+1)); + if (wantu && (k < m-1)) + m_matU.col(k).swap(m_matU.col(k+1)); + ++k; + } + iter = 0; + p--; + } + break; + } // end big switch + } // end iterations +} + +template<typename MatrixType> +SVD<MatrixType>& SVD<MatrixType>::sort() +{ + int mu = m_matU.rows(); + int mv = m_matV.rows(); + int n = m_matU.cols(); + + for (int i=0; i<n; ++i) + { + int k = i; + Scalar p = m_sigma.coeff(i); + + for (int j=i+1; j<n; ++j) + { + if (m_sigma.coeff(j) > p) + { + k = j; + p = m_sigma.coeff(j); + } + } + if (k != i) + { + m_sigma.coeffRef(k) = m_sigma.coeff(i); // i.e. + m_sigma.coeffRef(i) = p; // swaps the i-th and the k-th elements + + int j = mu; + for(int s=0; j!=0; ++s, --j) + std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k)); + + j = mv; + for (int s=0; j!=0; ++s, --j) + std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k)); + } + } + return *this; +} + +/** \returns the solution of \f$ A x = b \f$ using the current SVD decomposition of A. + * The parts of the solution corresponding to zero singular values are ignored. + * + * \sa MatrixBase::svd(), LU::solve(), LLT::solve() + */ +template<typename MatrixType> +template<typename OtherDerived, typename ResultType> +bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const +{ + const int rows = m_matU.rows(); + ei_assert(b.rows() == rows); + + Scalar maxVal = m_sigma.cwise().abs().maxCoeff(); + for (int j=0; j<b.cols(); ++j) + { + Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j); + + for (int i = 0; i <m_matU.cols(); ++i) + { + Scalar si = m_sigma.coeff(i); + if (ei_isMuchSmallerThan(ei_abs(si),maxVal)) + aux.coeffRef(i) = 0; + else + aux.coeffRef(i) /= si; + } + + result->col(j) = m_matV * aux; + } + return true; +} + +/** Computes the polar decomposition of the matrix, as a product unitary x positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * Only for square matrices. + * + * \sa computePositiveUnitary(), computeRotationScaling() + */ +template<typename MatrixType> +template<typename UnitaryType, typename PositiveType> +void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary, + PositiveType *positive) const +{ + ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices"); + if(unitary) *unitary = m_matU * m_matV.adjoint(); + if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint(); +} + +/** Computes the polar decomposition of the matrix, as a product positive x unitary. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * Only for square matrices. + * + * \sa computeUnitaryPositive(), computeRotationScaling() + */ +template<typename MatrixType> +template<typename UnitaryType, typename PositiveType> +void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive, + PositiveType *unitary) const +{ + ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); + if(unitary) *unitary = m_matU * m_matV.adjoint(); + if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint(); +} + +/** decomposes the matrix as a product rotation x scaling, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * This method requires the Geometry module. + * + * \sa computeScalingRotation(), computeUnitaryPositive() + */ +template<typename MatrixType> +template<typename RotationType, typename ScalingType> +void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const +{ + ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); + Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1 + Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma); + sv.coeffRef(0) *= x; + if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint()); + if(rotation) + { + MatrixType m(m_matU); + m.col(0) /= x; + rotation->lazyAssign(m * m_matV.adjoint()); + } +} + +/** decomposes the matrix as a product scaling x rotation, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * This method requires the Geometry module. + * + * \sa computeRotationScaling(), computeUnitaryPositive() + */ +template<typename MatrixType> +template<typename ScalingType, typename RotationType> +void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const +{ + ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); + Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1 + Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma); + sv.coeffRef(0) *= x; + if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint()); + if(rotation) + { + MatrixType m(m_matU); + m.col(0) /= x; + rotation->lazyAssign(m * m_matV.adjoint()); + } +} + + +/** \svd_module + * \returns the SVD decomposition of \c *this + */ +template<typename Derived> +inline SVD<typename MatrixBase<Derived>::PlainObject> +MatrixBase<Derived>::svd() const +{ + return SVD<PlainObject>(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN2_SVD_H diff --git a/Eigen/src/Eigen2Support/TriangularSolver.h b/Eigen/src/Eigen2Support/TriangularSolver.h new file mode 100644 index 000000000..ebbeb3b49 --- /dev/null +++ b/Eigen/src/Eigen2Support/TriangularSolver.h @@ -0,0 +1,42 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_TRIANGULAR_SOLVER2_H +#define EIGEN_TRIANGULAR_SOLVER2_H + +namespace Eigen { + +const unsigned int UnitDiagBit = UnitDiag; +const unsigned int SelfAdjointBit = SelfAdjoint; +const unsigned int UpperTriangularBit = Upper; +const unsigned int LowerTriangularBit = Lower; + +const unsigned int UpperTriangular = Upper; +const unsigned int LowerTriangular = Lower; +const unsigned int UnitUpperTriangular = UnitUpper; +const unsigned int UnitLowerTriangular = UnitLower; + +template<typename ExpressionType, unsigned int Added, unsigned int Removed> +template<typename OtherDerived> +typename ExpressionType::PlainObject +Flagged<ExpressionType,Added,Removed>::solveTriangular(const MatrixBase<OtherDerived>& other) const +{ + return m_matrix.template triangularView<Added>().solve(other.derived()); +} + +template<typename ExpressionType, unsigned int Added, unsigned int Removed> +template<typename OtherDerived> +void Flagged<ExpressionType,Added,Removed>::solveTriangularInPlace(const MatrixBase<OtherDerived>& other) const +{ + m_matrix.template triangularView<Added>().solveInPlace(other.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_TRIANGULAR_SOLVER2_H diff --git a/Eigen/src/Eigen2Support/VectorBlock.h b/Eigen/src/Eigen2Support/VectorBlock.h new file mode 100644 index 000000000..71a8080a9 --- /dev/null +++ b/Eigen/src/Eigen2Support/VectorBlock.h @@ -0,0 +1,94 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com> +// +// 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 EIGEN2_VECTORBLOCK_H +#define EIGEN2_VECTORBLOCK_H + +namespace Eigen { + +/** \deprecated use DenseMase::head(Index) */ +template<typename Derived> +inline VectorBlock<Derived> +MatrixBase<Derived>::start(Index size) +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<Derived>(derived(), 0, size); +} + +/** \deprecated use DenseMase::head(Index) */ +template<typename Derived> +inline const VectorBlock<const Derived> +MatrixBase<Derived>::start(Index size) const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<const Derived>(derived(), 0, size); +} + +/** \deprecated use DenseMase::tail(Index) */ +template<typename Derived> +inline VectorBlock<Derived> +MatrixBase<Derived>::end(Index size) +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<Derived>(derived(), this->size() - size, size); +} + +/** \deprecated use DenseMase::tail(Index) */ +template<typename Derived> +inline const VectorBlock<const Derived> +MatrixBase<Derived>::end(Index size) const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<const Derived>(derived(), this->size() - size, size); +} + +/** \deprecated use DenseMase::head() */ +template<typename Derived> +template<int Size> +inline VectorBlock<Derived,Size> +MatrixBase<Derived>::start() +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<Derived,Size>(derived(), 0); +} + +/** \deprecated use DenseMase::head() */ +template<typename Derived> +template<int Size> +inline const VectorBlock<const Derived,Size> +MatrixBase<Derived>::start() const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<const Derived,Size>(derived(), 0); +} + +/** \deprecated use DenseMase::tail() */ +template<typename Derived> +template<int Size> +inline VectorBlock<Derived,Size> +MatrixBase<Derived>::end() +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<Derived, Size>(derived(), size() - Size); +} + +/** \deprecated use DenseMase::tail() */ +template<typename Derived> +template<int Size> +inline const VectorBlock<const Derived,Size> +MatrixBase<Derived>::end() const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return VectorBlock<const Derived, Size>(derived(), size() - Size); +} + +} // end namespace Eigen + +#endif // EIGEN2_VECTORBLOCK_H |