diff --git a/Eigen/Core b/Eigen/Core index 206f41679..623d735d6 100644 --- a/Eigen/Core +++ b/Eigen/Core @@ -321,6 +321,7 @@ using std::ptrdiff_t; #include "src/Core/DiagonalMatrix.h" #include "src/Core/Diagonal.h" #include "src/Core/DiagonalProduct.h" +#include "src/Core/SkewSymmetricMatrix3.h" #include "src/Core/Redux.h" #include "src/Core/Visitor.h" #include "src/Core/Fuzzy.h" diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index 521280b32..f4f1c5530 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -186,6 +186,11 @@ template class MatrixBase const Product operator*(const DiagonalBase &diagonal) const; + template + EIGEN_DEVICE_FUNC + const Product + operator*(const SkewSymmetricBase &skew) const; + template EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR typename ScalarBinaryOpTraits::Scalar,typename internal::traits::Scalar>::ReturnType @@ -259,6 +264,7 @@ template class MatrixBase EIGEN_DEVICE_FUNC const DiagonalWrapper asDiagonal() const; const PermutationWrapper asPermutation() const; + const SkewSymmetricWrapper asSkewSymmetric() const; EIGEN_DEVICE_FUNC Derived& setIdentity(); @@ -273,6 +279,8 @@ template class MatrixBase bool isUpperTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; bool isLowerTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isSkewSymmetric(const RealScalar& prec = NumTraits::dummy_precision()) const; + template bool isOrthogonal(const MatrixBase& other, const RealScalar& prec = NumTraits::dummy_precision()) const; diff --git a/Eigen/src/Core/ProductEvaluators.h b/Eigen/src/Core/ProductEvaluators.h index 2ea6d0a64..c496e7c0e 100644 --- a/Eigen/src/Core/ProductEvaluators.h +++ b/Eigen/src/Core/ProductEvaluators.h @@ -1174,6 +1174,40 @@ struct generic_product_impl, MatrixShape, TranspositionsShap } }; +/*************************************************************************** +* skew symmetric products +* for now we just call the generic implementation +***************************************************************************/ +template +struct generic_product_impl +{ + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + generic_product_impl::evalTo(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + generic_product_impl::evalTo(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + generic_product_impl::evalTo(dst, lhs, rhs); + } +}; + } // end namespace internal } // end namespace Eigen diff --git a/Eigen/src/Core/SkewSymmetricMatrix3.h b/Eigen/src/Core/SkewSymmetricMatrix3.h new file mode 100644 index 000000000..7f6b5fdfa --- /dev/null +++ b/Eigen/src/Core/SkewSymmetricMatrix3.h @@ -0,0 +1,412 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2007-2009 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SKEWSYMMETRICMATRIX3_H +#define EIGEN_SKEWSYMMETRICMATRIX3_H + +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class SkewSymmetricBase + * \ingroup Core_Module + * + * \brief Base class for skew symmetric matrices and expressions + * + * This is the base class that is inherited by SkewSymmetricMatrix3 and related expression + * types, which internally use a three vector for storing the entries. SkewSymmetric + * types always represent square three times three matrices. + * + * This implementations follows class DiagonalMatrix + * + * \tparam Derived is the derived type, a SkewSymmetricMatrix3 or SkewSymmetricWrapper. + * + * \sa class SkewSymmetricMatrix3, class SkewSymmetricWrapper + */ +template +class SkewSymmetricBase : public EigenBase +{ + public: + typedef typename internal::traits::SkewSymmetricVectorType SkewSymmetricVectorType; + typedef typename SkewSymmetricVectorType::Scalar Scalar; + typedef typename SkewSymmetricVectorType::RealScalar RealScalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + + enum { + RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime, + ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime, + MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime, + IsVectorAtCompileTime = 0, + Flags = NoPreferredStorageOrderBit + }; + + typedef Matrix DenseMatrixType; + typedef DenseMatrixType DenseType; + typedef SkewSymmetricMatrix3 PlainObject; + + /** \returns a reference to the derived object. */ + EIGEN_DEVICE_FUNC + inline const Derived& derived() const { return *static_cast(this); } + /** \returns a const reference to the derived object. */ + EIGEN_DEVICE_FUNC + inline Derived& derived() { return *static_cast(this); } + + /** + * Constructs a dense matrix from \c *this. Note, this directly returns a dense matrix type, + * not an expression. + * \returns A dense matrix, with its entries set from the the derived object. */ + EIGEN_DEVICE_FUNC + DenseMatrixType toDenseMatrix() const { return derived(); } + + /** Determinant vanishes */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Scalar determinant() const { return 0; } + + /** A.transpose() = -A */ + EIGEN_DEVICE_FUNC + PlainObject transpose() const { return (-vector()).asSkewSymmetric(); } + + /** \returns the exponential of this matrix using Rodrigues’ formula */ + EIGEN_DEVICE_FUNC + DenseMatrixType exponential() const { + DenseMatrixType retVal = DenseMatrixType::Identity(); + const SkewSymmetricVectorType& v = vector(); + if (v.isZero()) { + return retVal; + } + const Scalar norm2 = v.squaredNorm(); + const Scalar norm = numext::sqrt(norm2); + retVal += ((((1 - numext::cos(norm))/norm2)*derived())*derived()) + (numext::sin(norm)/norm)*derived().toDenseMatrix(); + return retVal; + } + + /** \returns a reference to the derived object's vector of coefficients. */ + EIGEN_DEVICE_FUNC + inline const SkewSymmetricVectorType& vector() const { return derived().vector(); } + /** \returns a const reference to the derived object's vector of coefficients. */ + EIGEN_DEVICE_FUNC + inline SkewSymmetricVectorType& vector() { return derived().vector(); } + + /** \returns the number of rows. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Index rows() const { return 3; } + /** \returns the number of columns. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Index cols() const { return 3; } + + /** \returns the matrix product of \c *this by the dense matrix, \a matrix */ + template + EIGEN_DEVICE_FUNC + Product + operator*(const MatrixBase &matrix) const + { + return Product(derived(), matrix.derived()); + } + + /** \returns the matrix product of \c *this by the skew symmetric matrix, \a matrix */ + template + EIGEN_DEVICE_FUNC + Product + operator*(const SkewSymmetricBase &matrix) const + { + return Product(derived(), matrix.derived()); + } + + template + using SkewSymmetricProductReturnType = SkewSymmetricWrapper; + + /** \returns the wedge product of \c *this by the skew symmetric matrix \a other + * A wedge B = AB - BA */ + template + EIGEN_DEVICE_FUNC SkewSymmetricProductReturnType wedge( + const SkewSymmetricBase& other) const { + return vector().cross(other.vector()).asSkewSymmetric(); + } + + using SkewSymmetricScaleReturnType = + SkewSymmetricWrapper; + + /** \returns the product of \c *this by the scalar \a scalar */ + EIGEN_DEVICE_FUNC + inline SkewSymmetricScaleReturnType operator*(const Scalar& scalar) const { + return (vector() * scalar).asSkewSymmetric(); + } + + using ScaleSkewSymmetricReturnType = + SkewSymmetricWrapper; + + /** \returns the product of a scalar and the skew symmetric matrix \a other */ + EIGEN_DEVICE_FUNC + friend inline ScaleSkewSymmetricReturnType operator*(const Scalar& scalar, const SkewSymmetricBase& other) { + return (scalar * other.vector()).asSkewSymmetric(); + } + + template + using SkewSymmetricSumReturnType = SkewSymmetricWrapper; + + /** \returns the sum of \c *this and the skew symmetric matrix \a other */ + template + EIGEN_DEVICE_FUNC inline SkewSymmetricSumReturnType operator+( + const SkewSymmetricBase& other) const { + return (vector() + other.vector()).asSkewSymmetric(); + } + + template + using SkewSymmetricDifferenceReturnType = SkewSymmetricWrapper; + + /** \returns the difference of \c *this and the skew symmetric matrix \a other */ + template + EIGEN_DEVICE_FUNC inline SkewSymmetricDifferenceReturnType operator-( + const SkewSymmetricBase& other) const { + return (vector() - other.vector()).asSkewSymmetric(); + } +}; + +/** \class SkewSymmetricMatrix3 + * \ingroup Core_Module + * + * \brief Represents a 3x3 skew symmetric matrix with its storage + * + * \tparam Scalar_ the type of coefficients + * + * \sa class SkewSymmetricBase, class SkewSymmetricWrapper + */ + +namespace internal { +template +struct traits > + : traits > +{ + typedef Matrix SkewSymmetricVectorType; + typedef SkewSymmetricShape StorageKind; + enum { + Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit + }; +}; +} +template +class SkewSymmetricMatrix3 + : public SkewSymmetricBase > +{ + public: + #ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename internal::traits::SkewSymmetricVectorType SkewSymmetricVectorType; + typedef const SkewSymmetricMatrix3& Nested; + typedef Scalar_ Scalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + #endif + + protected: + + SkewSymmetricVectorType m_vector; + + public: + + /** const version of vector(). */ + EIGEN_DEVICE_FUNC + inline const SkewSymmetricVectorType& vector() const { return m_vector; } + /** \returns a reference to the stored vector of coefficients. */ + EIGEN_DEVICE_FUNC + inline SkewSymmetricVectorType& vector() { return m_vector; } + + /** Default constructor without initialization */ + EIGEN_DEVICE_FUNC + inline SkewSymmetricMatrix3() {} + + /** Constructor from three scalars */ + EIGEN_DEVICE_FUNC + inline SkewSymmetricMatrix3(const Scalar& x, const Scalar& y, const Scalar& z) : m_vector(x,y,z) {} + + /** \brief Constructs a SkewSymmetricMatrix3 from an r-value vector type */ + EIGEN_DEVICE_FUNC + explicit inline SkewSymmetricMatrix3(SkewSymmetricVectorType&& vec) : m_vector(std::move(vec)) {} + + /** generic constructor from expression of the coefficients */ + template + EIGEN_DEVICE_FUNC + explicit inline SkewSymmetricMatrix3(const MatrixBase& other) : m_vector(other) + {} + + /** Copy constructor. */ + template + EIGEN_DEVICE_FUNC + inline SkewSymmetricMatrix3(const SkewSymmetricBase& other) : m_vector(other.vector()) {} + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */ + inline SkewSymmetricMatrix3(const SkewSymmetricMatrix3& other) : m_vector(other.vector()) {} + #endif + + /** Copy operator. */ + template + EIGEN_DEVICE_FUNC + SkewSymmetricMatrix3& operator=(const SkewSymmetricBase& other) + { + m_vector = other.vector(); + return *this; + } + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + EIGEN_DEVICE_FUNC + SkewSymmetricMatrix3& operator=(const SkewSymmetricMatrix3& other) + { + m_vector = other.vector(); + return *this; + } + #endif + + typedef SkewSymmetricWrapper, SkewSymmetricVectorType>> + InitializeReturnType; + + /** Initializes a skew symmetric matrix with coefficients set to zero */ + EIGEN_DEVICE_FUNC + static InitializeReturnType Zero() { return SkewSymmetricVectorType::Zero().asSkewSymmetric(); } + + /** Sets all coefficients to zero. */ + EIGEN_DEVICE_FUNC + inline void setZero() { m_vector.setZero(); } +}; + +/** \class SkewSymmetricWrapper + * \ingroup Core_Module + * + * \brief Expression of a skew symmetric matrix + * + * \tparam SkewSymmetricVectorType_ the type of the vector of coefficients + * + * This class is an expression of a skew symmetric matrix, but not storing its own vector of coefficients, + * instead wrapping an existing vector expression. It is the return type of MatrixBase::asSkewSymmetric() + * and most of the time this is the only way that it is used. + * + * \sa class SkewSymmetricMatrix3, class SkewSymmetricBase, MatrixBase::asSkewSymmetric() + */ + +namespace internal { +template +struct traits > +{ + typedef SkewSymmetricVectorType_ SkewSymmetricVectorType; + typedef typename SkewSymmetricVectorType::Scalar Scalar; + typedef typename SkewSymmetricVectorType::StorageIndex StorageIndex; + typedef SkewSymmetricShape StorageKind; + typedef typename traits::XprKind XprKind; + enum { + RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime, + ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime, + MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime, + Flags = (traits::Flags & LvalueBit) | NoPreferredStorageOrderBit + }; +}; +} + +template +class SkewSymmetricWrapper + : public SkewSymmetricBase >, internal::no_assignment_operator +{ + public: + #ifndef EIGEN_PARSED_BY_DOXYGEN + typedef SkewSymmetricVectorType_ SkewSymmetricVectorType; + typedef SkewSymmetricWrapper Nested; + #endif + + /** Constructor from expression of coefficients to wrap. */ + EIGEN_DEVICE_FUNC + explicit inline SkewSymmetricWrapper(SkewSymmetricVectorType& a_vector) : m_vector(a_vector) {} + + /** \returns a const reference to the wrapped expression of coefficients. */ + EIGEN_DEVICE_FUNC + const SkewSymmetricVectorType& vector() const { return m_vector; } + + protected: + typename SkewSymmetricVectorType::Nested m_vector; +}; + +/** \returns a pseudo-expression of a skew symmetric matrix with *this as vector of coefficients + * + * \only_for_vectors + * + * \sa class SkewSymmetricWrapper, class SkewSymmetricMatrix3, vector(), isSkewSymmetric() + **/ +template +EIGEN_DEVICE_FUNC inline const SkewSymmetricWrapper +MatrixBase::asSkewSymmetric() const +{ + return SkewSymmetricWrapper(derived()); +} + +/** \returns true if *this is approximately equal to a skew symmetric matrix, + * within the precision given by \a prec. + */ +template +bool MatrixBase::isSkewSymmetric(const RealScalar& prec) const +{ + if(cols() != rows()) return false; + return (this->transpose() + *this).isZero(prec); +} + +/** \returns the matrix product of \c *this by the skew symmetric matrix \skew. + */ +template +template +EIGEN_DEVICE_FUNC inline const Product +MatrixBase::operator*(const SkewSymmetricBase &skew) const +{ + return Product(derived(), skew.derived()); +} + +namespace internal { + +template<> struct storage_kind_to_shape { typedef SkewSymmetricShape Shape; }; + +struct SkewSymmetric2Dense {}; + +template<> struct AssignmentKind { typedef SkewSymmetric2Dense Kind; }; + +// SkewSymmetric matrix to Dense assignment +template< typename DstXprType, typename SrcXprType, typename Functor> +struct Assignment +{ + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + if((dst.rows()!=3) || (dst.cols()!=3)) { + dst.resize(3, 3); + } + dst.diagonal().setZero(); + const typename SrcXprType::SkewSymmetricVectorType v = src.vector(); + dst(0, 1) = -v(2); + dst(1, 0) = v(2); + dst(0, 2) = v(1); + dst(2, 0) = -v(1); + dst(1, 2) = -v(0); + dst(2, 1) = v(0); + } + + static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op &/*func*/) + { dst.vector() += src.vector(); } + + static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op &/*func*/) + { dst.vector() -= src.vector(); } +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SKEWSYMMETRICMATRIX3_H diff --git a/Eigen/src/Core/util/Constants.h b/Eigen/src/Core/util/Constants.h index 4457f4f69..017508779 100644 --- a/Eigen/src/Core/util/Constants.h +++ b/Eigen/src/Core/util/Constants.h @@ -533,6 +533,7 @@ struct DenseShape { static std::string debugName() { return "DenseSh struct SolverShape { static std::string debugName() { return "SolverShape"; } }; struct HomogeneousShape { static std::string debugName() { return "HomogeneousShape"; } }; struct DiagonalShape { static std::string debugName() { return "DiagonalShape"; } }; +struct SkewSymmetricShape { static std::string debugName() { return "SkewSymmetricShape"; } }; struct BandShape { static std::string debugName() { return "BandShape"; } }; struct TriangularShape { static std::string debugName() { return "TriangularShape"; } }; struct SelfAdjointShape { static std::string debugName() { return "SelfAdjointShape"; } }; diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index 5c55639cd..503d6511d 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -90,6 +90,9 @@ template class DiagonalWrapper; template class DiagonalMatrix; template class DiagonalProduct; template class Diagonal; +template class SkewSymmetricBase; +template class SkewSymmetricWrapper; +template class SkewSymmetricMatrix3; template class PermutationMatrix; template class Transpositions; template class PermutationBase; diff --git a/Eigen/src/Core/util/XprHelper.h b/Eigen/src/Core/util/XprHelper.h index b680e6193..1aa41abb4 100644 --- a/Eigen/src/Core/util/XprHelper.h +++ b/Eigen/src/Core/util/XprHelper.h @@ -270,6 +270,11 @@ template struct plain_matrix_type typedef typename T::PlainObject type; }; +template struct plain_matrix_type +{ + typedef typename T::PlainObject type; +}; + template struct plain_matrix_type_dense { typedef Matrix::Scalar, @@ -316,6 +321,11 @@ template struct eval typedef typename plain_matrix_type::type type; }; +template struct eval +{ + typedef typename plain_matrix_type::type type; +}; + // for matrices, no need to evaluate, just use a const reference to avoid a useless copy template struct eval, Dense> @@ -554,6 +564,12 @@ template struct product_promote_storage_type struct product_promote_storage_type { typedef Dense ret; }; template struct product_promote_storage_type { typedef Dense ret; }; +template struct product_promote_storage_type { typedef A ret; }; +template struct product_promote_storage_type { typedef B ret; }; +template struct product_promote_storage_type { typedef Dense ret; }; +template struct product_promote_storage_type { typedef Dense ret; }; +template struct product_promote_storage_type { typedef Dense ret; }; + template struct product_promote_storage_type { typedef A ret; }; template struct product_promote_storage_type { typedef B ret; }; template struct product_promote_storage_type { typedef Dense ret; }; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6e805d0fa..7d6f9784e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -207,6 +207,7 @@ ei_add_test(product_small) ei_add_test(product_large) ei_add_test(product_extra) ei_add_test(diagonalmatrices) +ei_add_test(skew_symmetric_matrix3) ei_add_test(adjoint) ei_add_test(diagonal) ei_add_test(miscmatrices) diff --git a/test/skew_symmetric_matrix3.cpp b/test/skew_symmetric_matrix3.cpp new file mode 100644 index 000000000..e99aebab2 --- /dev/null +++ b/test/skew_symmetric_matrix3.cpp @@ -0,0 +1,221 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" +#include + +namespace { +template +void constructors() { + typedef Matrix Vector; + const Vector v = Vector::Random(); + // l-value + const SkewSymmetricMatrix3 s1(v); + const Vector& v1 = s1.vector(); + VERIFY_IS_APPROX(v1, v); + VERIFY(s1.cols() == 3); + VERIFY(s1.rows() == 3); + + // r-value + const SkewSymmetricMatrix3 s2(std::move(v)); + VERIFY_IS_APPROX(v1, s2.vector()); + VERIFY_IS_APPROX(s1.toDenseMatrix(), s2.toDenseMatrix()); + + // default constructor leaves the matrix uninitialised + SkewSymmetricMatrix3 s3; + VERIFY_IS_NOT_APPROX(v1, s3.vector()); + + // from scalars + SkewSymmetricMatrix3 s4(v1(0), v1(1), v1(2)); + VERIFY_IS_APPROX(v1, s4.vector()); + + // constructors with four vectors do not compile + // Matrix vector4 = Matrix::Random(); + // SkewSymmetricMatrix3 s5(vector4); +} + +template +void assignments() { + typedef Matrix Vector; + typedef Matrix SquareMatrix; + + const Vector v = Vector::Random(); + + // assign to square matrix + SquareMatrix sq; + sq = v.asSkewSymmetric(); + VERIFY(sq.isSkewSymmetric()); + + // assign to skew symmetric matrix + SkewSymmetricMatrix3 sk; + sk = v.asSkewSymmetric(); + VERIFY_IS_APPROX(v, sk.vector()); +} + +template +void plusMinus() { + typedef Matrix Vector; + typedef Matrix SquareMatrix; + + const Vector v1 = Vector::Random(); + const Vector v2 = Vector::Random(); + + SquareMatrix sq1; + sq1 = v1.asSkewSymmetric(); + SquareMatrix sq2; + sq2 = v2.asSkewSymmetric(); + + SkewSymmetricMatrix3 sk1; + sk1 = v1.asSkewSymmetric(); + SkewSymmetricMatrix3 sk2; + sk2 = v2.asSkewSymmetric(); + + VERIFY_IS_APPROX((sk1 + sk2).toDenseMatrix(), sq1 + sq2); + VERIFY_IS_APPROX((sk1 - sk2).toDenseMatrix(), sq1 - sq2); + + SquareMatrix sq3 = v1.asSkewSymmetric(); + VERIFY_IS_APPROX( sq3 = v1.asSkewSymmetric() + v2.asSkewSymmetric(), sq1 + sq2); + VERIFY_IS_APPROX( sq3 = v1.asSkewSymmetric() - v2.asSkewSymmetric(), sq1 - sq2); + VERIFY_IS_APPROX( sq3 = v1.asSkewSymmetric() - 2*v2.asSkewSymmetric() + v1.asSkewSymmetric(), sq1 - 2*sq2 + sq1); + + VERIFY_IS_APPROX((sk1 + sk1).vector(), 2*v1); + VERIFY((sk1 - sk1).vector().isZero()); + VERIFY((sk1 - sk1).toDenseMatrix().isZero()); +} + + +template +void multiplyScale() { + typedef Matrix Vector; + typedef Matrix SquareMatrix; + + const Vector v1 = Vector::Random(); + SquareMatrix sq1; + sq1 = v1.asSkewSymmetric(); + SkewSymmetricMatrix3 sk1; + sk1 = v1.asSkewSymmetric(); + + const Scalar s1 = internal::random(); + VERIFY_IS_APPROX(SkewSymmetricMatrix3(sk1*s1).vector(), sk1.vector() * s1); + VERIFY_IS_APPROX(SkewSymmetricMatrix3(s1*sk1).vector(), s1 * sk1.vector()); + VERIFY_IS_APPROX(sq1 * (sk1 * s1), (sq1 * sk1) * s1); + + const Vector v2 = Vector::Random(); + SquareMatrix sq2; + sq2 = v2.asSkewSymmetric(); + SkewSymmetricMatrix3 sk2; + sk2 = v2.asSkewSymmetric(); + VERIFY_IS_APPROX(sk1*sk2, sq1*sq2); + + // null space + VERIFY((sk1*v1).isZero()); + VERIFY((sk2*v2).isZero()); +} + +template +void skewSymmetricMultiplication(const Matrix& m) { + typedef Eigen::Matrix Vector; + const Vector v = Vector::Random(); + const Matrix m1 = Matrix::Random(m.rows(), m.cols()); + const SkewSymmetricMatrix3 sk = v.asSkewSymmetric(); + VERIFY_IS_APPROX(m1.transpose() * (sk * m1), (m1.transpose() * sk) * m1); + VERIFY((m1.transpose() * (sk * m1)).isSkewSymmetric()); +} + +template +void traceAndDet() { + typedef Matrix Vector; + const Vector v = Vector::Random(); + // this does not work, values larger than 1.e-08 can be seen + //VERIFY_IS_APPROX(sq.determinant(), static_cast(0)); + VERIFY_IS_APPROX(v.asSkewSymmetric().determinant(), static_cast(0)); + VERIFY_IS_APPROX(v.asSkewSymmetric().toDenseMatrix().trace(), static_cast(0)); +} + +template +void transpose() { + typedef Matrix Vector; + const Vector v = Vector::Random(); + // By definition of a skew symmetric matrix: A^T = -A + VERIFY_IS_APPROX(v.asSkewSymmetric().toDenseMatrix().transpose(), v.asSkewSymmetric().transpose().toDenseMatrix()); + VERIFY_IS_APPROX(v.asSkewSymmetric().transpose().vector(), (-v).asSkewSymmetric().vector()); +} + +template +void exponentialIdentity() { + typedef Matrix Vector; + const Vector v1 = Vector::Zero(); + VERIFY(v1.asSkewSymmetric().exponential().isIdentity()); + + Vector v2 = Vector::Random(); + v2.normalize(); + VERIFY((2*EIGEN_PI*v2).asSkewSymmetric().exponential().isIdentity()); + + Vector v3; + const auto precision = static_cast(1.1)*NumTraits::dummy_precision(); + v3 << 0, 0, precision; + VERIFY(v3.asSkewSymmetric().exponential().isIdentity(precision)); +} + +template +void exponentialOrthogonality() { + typedef Matrix Vector; + typedef Matrix SquareMatrix; + const Vector v = Vector::Random(); + SquareMatrix sq = v.asSkewSymmetric().exponential(); + VERIFY(sq.isUnitary()); +} + +template +void exponentialRotation() { + typedef Matrix Vector; + typedef Matrix SquareMatrix; + + // rotation axis is invariant + const Vector v1 = Vector::Random(); + const SquareMatrix r1 = v1.asSkewSymmetric().exponential(); + VERIFY_IS_APPROX(r1*v1, v1); + + // rotate around z-axis + Vector v2; + v2 << 0, 0, EIGEN_PI; + const SquareMatrix r2 = v2.asSkewSymmetric().exponential(); + VERIFY_IS_APPROX(r2*(Vector() << 1,0,0).finished(), (Vector() << -1,0,0).finished()); + VERIFY_IS_APPROX(r2*(Vector() << 0,1,0).finished(), (Vector() << 0,-1,0).finished()); +} + + +} // namespace + + +EIGEN_DECLARE_TEST(skew_symmetric_matrix3) +{ + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1(constructors()); + CALL_SUBTEST_1(constructors()); + CALL_SUBTEST_1(assignments()); + CALL_SUBTEST_1(assignments()); + + CALL_SUBTEST_2(plusMinus()); + CALL_SUBTEST_2(plusMinus()); + CALL_SUBTEST_2(multiplyScale()); + CALL_SUBTEST_2(multiplyScale()); + CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXf(3,internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXd(3,internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_2(traceAndDet()); + CALL_SUBTEST_2(traceAndDet()); + CALL_SUBTEST_2(transpose()); + CALL_SUBTEST_2(transpose()); + + CALL_SUBTEST_3(exponentialIdentity()); + CALL_SUBTEST_3(exponentialIdentity()); + CALL_SUBTEST_3(exponentialOrthogonality()); + CALL_SUBTEST_3(exponentialOrthogonality()); + CALL_SUBTEST_3(exponentialRotation()); + CALL_SUBTEST_3(exponentialRotation()); + } +}