Feature/skew symmetric matrix3

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"

Eigen/src/Core/MatrixBase.h

@@ -186,6 +186,11 @@ template<typename Derived> class MatrixBase
     const Product<Derived, DiagonalDerived, LazyProduct>
     operator*(const DiagonalBase<DiagonalDerived> &diagonal) const;
 
+    template<typename SkewDerived>
+    EIGEN_DEVICE_FUNC
+    const Product<Derived, SkewDerived, LazyProduct>
+    operator*(const SkewSymmetricBase<SkewDerived> &skew) const;
+
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
@@ -259,6 +264,7 @@ template<typename Derived> class MatrixBase
     EIGEN_DEVICE_FUNC
     const DiagonalWrapper<const Derived> asDiagonal() const;
     const PermutationWrapper<const Derived> asPermutation() const;
+    const SkewSymmetricWrapper<const Derived> asSkewSymmetric() const;
 
     EIGEN_DEVICE_FUNC
     Derived& setIdentity();
@@ -273,6 +279,8 @@ template<typename Derived> class MatrixBase
     bool isUpperTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
     bool isLowerTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
 
+    bool isSkewSymmetric(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
     template<typename OtherDerived>
     bool isOrthogonal(const MatrixBase<OtherDerived>& other,
                       const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;

Eigen/src/Core/ProductEvaluators.h

@@ -1174,6 +1174,40 @@ struct generic_product_impl<Lhs, Transpose<Rhs>, MatrixShape, TranspositionsShap
   }
 };
 
+/***************************************************************************
+* skew symmetric products
+* for now we just call the generic implementation
+***************************************************************************/
+template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, MatrixShape, ProductTag>
+{
+  template<typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  {
+    generic_product_impl<typename Lhs::DenseMatrixType , Rhs, DenseShape, MatrixShape, ProductTag>::evalTo(dst, lhs, rhs);
+  }
+};
+
+template<typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
+struct generic_product_impl<Lhs, Rhs, MatrixShape, SkewSymmetricShape, ProductTag>
+{
+  template<typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  {
+    generic_product_impl<Lhs, typename Rhs::DenseMatrixType, MatrixShape, DenseShape, ProductTag>::evalTo(dst, lhs, rhs);
+  }
+};
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, SkewSymmetricShape, ProductTag>
+{
+  template<typename Dest>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  {
+    generic_product_impl<typename Lhs::DenseMatrixType, typename Rhs::DenseMatrixType, DenseShape, DenseShape, ProductTag>::evalTo(dst, lhs, rhs);
+  }
+};
+
 } // end namespace internal
 
 } // end namespace Eigen

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 <gael.guennebaud@inria.fr>
+// Copyright (C) 2007-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_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<typename Derived>
+class SkewSymmetricBase : public EigenBase<Derived>
+{
+  public:
+    typedef typename internal::traits<Derived>::SkewSymmetricVectorType SkewSymmetricVectorType;
+    typedef typename SkewSymmetricVectorType::Scalar Scalar;
+    typedef typename SkewSymmetricVectorType::RealScalar RealScalar;
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+
+    enum {
+      RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+      ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+      MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+      MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+      IsVectorAtCompileTime = 0,
+      Flags = NoPreferredStorageOrderBit
+    };
+
+    typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime> DenseMatrixType;
+    typedef DenseMatrixType DenseType;
+    typedef SkewSymmetricMatrix3<Scalar> PlainObject;
+
+    /** \returns a reference to the derived object. */
+    EIGEN_DEVICE_FUNC
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    /** \returns a const reference to the derived object. */
+    EIGEN_DEVICE_FUNC
+    inline Derived& derived() { return *static_cast<Derived*>(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<typename MatrixDerived>
+    EIGEN_DEVICE_FUNC
+    Product<Derived,MatrixDerived,LazyProduct>
+    operator*(const MatrixBase<MatrixDerived> &matrix) const
+    {
+      return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
+    }
+
+    /** \returns the matrix product of \c *this by the skew symmetric matrix, \a matrix */
+    template<typename MatrixDerived>
+    EIGEN_DEVICE_FUNC
+    Product<Derived,MatrixDerived,LazyProduct>
+    operator*(const SkewSymmetricBase<MatrixDerived> &matrix) const
+    {
+      return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
+    }
+
+    template <typename OtherDerived>
+    using SkewSymmetricProductReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+        SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, product)>;
+
+    /** \returns the wedge product of \c *this by the skew symmetric matrix \a other
+     *  A wedge B = AB - BA */
+    template <typename OtherDerived>
+    EIGEN_DEVICE_FUNC SkewSymmetricProductReturnType<OtherDerived> wedge(
+        const SkewSymmetricBase<OtherDerived>& other) const {
+      return vector().cross(other.vector()).asSkewSymmetric();
+    }
+
+    using SkewSymmetricScaleReturnType =
+        SkewSymmetricWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(SkewSymmetricVectorType, Scalar, product)>;
+
+    /** \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<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar, SkewSymmetricVectorType, product)>;
+
+    /** \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 <typename OtherDerived>
+    using SkewSymmetricSumReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+        SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, sum)>;
+
+    /** \returns the sum of \c *this and the skew symmetric matrix \a other */
+    template <typename OtherDerived>
+    EIGEN_DEVICE_FUNC inline SkewSymmetricSumReturnType<OtherDerived> operator+(
+        const SkewSymmetricBase<OtherDerived>& other) const {
+      return (vector() + other.vector()).asSkewSymmetric();
+    }
+
+    template <typename OtherDerived>
+    using SkewSymmetricDifferenceReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
+        SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, difference)>;
+
+    /** \returns the difference of \c *this and the skew symmetric matrix \a other */
+    template <typename OtherDerived>
+    EIGEN_DEVICE_FUNC inline SkewSymmetricDifferenceReturnType<OtherDerived> operator-(
+        const SkewSymmetricBase<OtherDerived>& 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<typename Scalar_>
+struct traits<SkewSymmetricMatrix3<Scalar_> >
+ : traits<Matrix<Scalar_,3,3,0,3,3> >
+{
+  typedef Matrix<Scalar_,3,1,0,3,1> SkewSymmetricVectorType;
+  typedef SkewSymmetricShape StorageKind;
+  enum {
+    Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit
+  };
+};
+}
+template<typename Scalar_>
+class SkewSymmetricMatrix3
+  : public SkewSymmetricBase<SkewSymmetricMatrix3<Scalar_> >
+{
+  public:
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    typedef typename internal::traits<SkewSymmetricMatrix3>::SkewSymmetricVectorType SkewSymmetricVectorType;
+    typedef const SkewSymmetricMatrix3& Nested;
+    typedef Scalar_ Scalar;
+    typedef typename internal::traits<SkewSymmetricMatrix3>::StorageKind StorageKind;
+    typedef typename internal::traits<SkewSymmetricMatrix3>::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<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+        explicit inline SkewSymmetricMatrix3(const MatrixBase<OtherDerived>& other) : m_vector(other)
+    {}
+
+    /** Copy constructor. */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    inline SkewSymmetricMatrix3(const SkewSymmetricBase<OtherDerived>& 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<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    SkewSymmetricMatrix3& operator=(const SkewSymmetricBase<OtherDerived>& 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<const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, 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<typename SkewSymmetricVectorType_>
+struct traits<SkewSymmetricWrapper<SkewSymmetricVectorType_> >
+{
+  typedef SkewSymmetricVectorType_ SkewSymmetricVectorType;
+  typedef typename SkewSymmetricVectorType::Scalar Scalar;
+  typedef typename SkewSymmetricVectorType::StorageIndex StorageIndex;
+  typedef SkewSymmetricShape StorageKind;
+  typedef typename traits<SkewSymmetricVectorType>::XprKind XprKind;
+  enum {
+    RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
+    MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
+    Flags =  (traits<SkewSymmetricVectorType>::Flags & LvalueBit) | NoPreferredStorageOrderBit
+  };
+};
+}
+
+template<typename SkewSymmetricVectorType_>
+class SkewSymmetricWrapper
+  : public SkewSymmetricBase<SkewSymmetricWrapper<SkewSymmetricVectorType_> >, 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<typename Derived>
+EIGEN_DEVICE_FUNC inline const SkewSymmetricWrapper<const Derived>
+MatrixBase<Derived>::asSkewSymmetric() const
+{
+  return SkewSymmetricWrapper<const Derived>(derived());
+}
+
+/** \returns true if *this is approximately equal to a skew symmetric matrix,
+  *          within the precision given by \a prec.
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::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<typename Derived>
+template<typename SkewDerived>
+EIGEN_DEVICE_FUNC inline const Product<Derived, SkewDerived, LazyProduct>
+MatrixBase<Derived>::operator*(const SkewSymmetricBase<SkewDerived> &skew) const
+{
+  return Product<Derived, SkewDerived, LazyProduct>(derived(), skew.derived());
+}
+
+namespace internal {
+
+template<> struct storage_kind_to_shape<SkewSymmetricShape> { typedef SkewSymmetricShape Shape; };
+
+struct SkewSymmetric2Dense {};
+
+template<> struct AssignmentKind<DenseShape,SkewSymmetricShape> { typedef SkewSymmetric2Dense Kind; };
+
+// SkewSymmetric matrix to Dense assignment
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, SkewSymmetric2Dense>
+{
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*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<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  { dst.vector() += src.vector(); }
+  
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  { dst.vector() -= src.vector(); }
+};
+
+} // namespace internal
+
+}  // end namespace Eigen
+
+#endif // EIGEN_SKEWSYMMETRICMATRIX3_H

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"; } };

Eigen/src/Core/util/ForwardDeclarations.h

@@ -90,6 +90,9 @@ template<typename DiagonalVectorType_> class DiagonalWrapper;
 template<typename Scalar_, int SizeAtCompileTime, int MaxSizeAtCompileTime=SizeAtCompileTime> class DiagonalMatrix;
 template<typename MatrixType, typename DiagonalType, int ProductOrder> class DiagonalProduct;
 template<typename MatrixType, int Index = 0> class Diagonal;
+template<typename Derived> class SkewSymmetricBase;
+template<typename VectorType_> class SkewSymmetricWrapper;
+template<typename Scalar_> class SkewSymmetricMatrix3;
 template<int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType=int> class PermutationMatrix;
 template<int SizeAtCompileTime, int MaxSizeAtCompileTime = SizeAtCompileTime, typename IndexType=int> class Transpositions;
 template<typename Derived> class PermutationBase;

Eigen/src/Core/util/XprHelper.h

@@ -270,6 +270,11 @@ template<typename T> struct plain_matrix_type<T,DiagonalShape>
   typedef typename T::PlainObject type;
 };
 
+template<typename T> struct plain_matrix_type<T,SkewSymmetricShape>
+{
+  typedef typename T::PlainObject type;
+};
+
 template<typename T, int Flags> struct plain_matrix_type_dense<T,MatrixXpr,Flags>
 {
   typedef Matrix<typename traits<T>::Scalar,
@@ -316,6 +321,11 @@ template<typename T> struct eval<T,DiagonalShape>
   typedef typename plain_matrix_type<T>::type type;
 };
 
+template<typename T> struct eval<T,SkewSymmetricShape>
+{
+  typedef typename plain_matrix_type<T>::type type;
+};
+
 // for matrices, no need to evaluate, just use a const reference to avoid a useless copy
 template<typename Scalar_, int Rows_, int Cols_, int Options_, int MaxRows_, int MaxCols_>
 struct eval<Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>, Dense>
@@ -554,6 +564,12 @@ template <typename B, int ProductTag> struct product_promote_storage_type<Diagon
 template <int ProductTag>             struct product_promote_storage_type<Dense,              DiagonalShape,      ProductTag> { typedef Dense ret; };
 template <int ProductTag>             struct product_promote_storage_type<DiagonalShape,      Dense,              ProductTag> { typedef Dense ret; };
 
+template <typename A, int ProductTag> struct product_promote_storage_type<A,                  SkewSymmetricShape, ProductTag> { typedef A ret; };
+template <typename B, int ProductTag> struct product_promote_storage_type<SkewSymmetricShape, B,                  ProductTag> { typedef B ret; };
+template <int ProductTag>             struct product_promote_storage_type<Dense,              SkewSymmetricShape, ProductTag> { typedef Dense ret; };
+template <int ProductTag>             struct product_promote_storage_type<SkewSymmetricShape, Dense,              ProductTag> { typedef Dense ret; };
+template <int ProductTag>             struct product_promote_storage_type<SkewSymmetricShape, SkewSymmetricShape, ProductTag> { typedef Dense ret; };
+
 template <typename A, int ProductTag> struct product_promote_storage_type<A,                  PermutationStorage, ProductTag> { typedef A ret; };
 template <typename B, int ProductTag> struct product_promote_storage_type<PermutationStorage, B,                  ProductTag> { typedef B ret; };
 template <int ProductTag>             struct product_promote_storage_type<Dense,              PermutationStorage, ProductTag> { typedef Dense ret; };

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)

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 <Eigen/LU>
+
+namespace {
+template <typename Scalar>
+void constructors() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  const Vector v = Vector::Random();
+  // l-value
+  const SkewSymmetricMatrix3<Scalar> s1(v);
+  const Vector& v1 = s1.vector();
+  VERIFY_IS_APPROX(v1, v);
+  VERIFY(s1.cols() == 3);
+  VERIFY(s1.rows() == 3);
+
+  // r-value
+  const SkewSymmetricMatrix3<Scalar> s2(std::move(v));
+  VERIFY_IS_APPROX(v1, s2.vector());
+  VERIFY_IS_APPROX(s1.toDenseMatrix(), s2.toDenseMatrix());
+
+  // default constructor leaves the matrix uninitialised
+  SkewSymmetricMatrix3<Scalar> s3;
+  VERIFY_IS_NOT_APPROX(v1, s3.vector());
+
+  // from scalars
+  SkewSymmetricMatrix3<Scalar> s4(v1(0), v1(1), v1(2));
+  VERIFY_IS_APPROX(v1, s4.vector());
+
+  // constructors with four vectors do not compile
+  // Matrix<Scalar, 4, 1> vector4 = Matrix<Scalar, 4, 1>::Random();
+  // SkewSymmetricMatrix3<Scalar> s5(vector4);
+}
+
+template <typename Scalar>
+void assignments() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  typedef Matrix<Scalar, 3, 3> SquareMatrix;
+
+  const Vector v = Vector::Random();
+
+  // assign to square matrix
+  SquareMatrix sq;
+  sq = v.asSkewSymmetric();
+  VERIFY(sq.isSkewSymmetric());
+
+  // assign to skew symmetric matrix
+  SkewSymmetricMatrix3<Scalar> sk;
+  sk = v.asSkewSymmetric();
+  VERIFY_IS_APPROX(v, sk.vector());
+}
+
+template <typename Scalar>
+void plusMinus() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  typedef Matrix<Scalar, 3, 3> SquareMatrix;
+
+  const Vector v1 = Vector::Random();
+  const Vector v2 = Vector::Random();
+
+  SquareMatrix sq1;
+  sq1 = v1.asSkewSymmetric();
+  SquareMatrix sq2;
+  sq2 = v2.asSkewSymmetric();
+
+  SkewSymmetricMatrix3<Scalar> sk1;
+  sk1 = v1.asSkewSymmetric();
+  SkewSymmetricMatrix3<Scalar> 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 <typename Scalar>
+void multiplyScale() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  typedef Matrix<Scalar, 3, 3> SquareMatrix;
+
+  const Vector v1 = Vector::Random();
+  SquareMatrix sq1;
+  sq1 = v1.asSkewSymmetric();
+  SkewSymmetricMatrix3<Scalar> sk1;
+  sk1 = v1.asSkewSymmetric();
+
+  const Scalar s1 = internal::random<Scalar>();
+  VERIFY_IS_APPROX(SkewSymmetricMatrix3<Scalar>(sk1*s1).vector(), sk1.vector() * s1);
+  VERIFY_IS_APPROX(SkewSymmetricMatrix3<Scalar>(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<Scalar> sk2;
+  sk2 = v2.asSkewSymmetric();
+  VERIFY_IS_APPROX(sk1*sk2, sq1*sq2);
+
+  // null space
+  VERIFY((sk1*v1).isZero());
+  VERIFY((sk2*v2).isZero());
+}
+
+template<typename Matrix>
+void skewSymmetricMultiplication(const Matrix& m) {
+  typedef Eigen::Matrix<typename Matrix::Scalar, 3, 1> Vector;
+  const Vector v = Vector::Random();
+  const Matrix m1 = Matrix::Random(m.rows(), m.cols());
+  const SkewSymmetricMatrix3<typename Matrix::Scalar> sk = v.asSkewSymmetric();
+  VERIFY_IS_APPROX(m1.transpose() * (sk * m1), (m1.transpose() * sk) * m1);
+  VERIFY((m1.transpose() * (sk * m1)).isSkewSymmetric());
+}
+
+template <typename Scalar>
+void traceAndDet() {
+  typedef Matrix<Scalar, 3, 1> 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<Scalar>(0));
+  VERIFY_IS_APPROX(v.asSkewSymmetric().determinant(), static_cast<Scalar>(0));
+  VERIFY_IS_APPROX(v.asSkewSymmetric().toDenseMatrix().trace(), static_cast<Scalar>(0));
+}
+
+template <typename Scalar>
+void transpose() {
+  typedef Matrix<Scalar, 3, 1> 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 <typename Scalar>
+void exponentialIdentity() {
+  typedef Matrix<Scalar, 3, 1> 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<Scalar>(1.1)*NumTraits<Scalar>::dummy_precision();
+  v3 << 0, 0, precision;
+  VERIFY(v3.asSkewSymmetric().exponential().isIdentity(precision));
+}
+
+template <typename Scalar>
+void exponentialOrthogonality() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  typedef Matrix<Scalar, 3, 3> SquareMatrix;
+  const Vector v = Vector::Random();
+  SquareMatrix sq = v.asSkewSymmetric().exponential();
+  VERIFY(sq.isUnitary());
+}
+
+template <typename Scalar>
+void exponentialRotation() {
+  typedef Matrix<Scalar, 3, 1> Vector;
+  typedef Matrix<Scalar, 3, 3> 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<float>());
+    CALL_SUBTEST_1(constructors<double>());
+    CALL_SUBTEST_1(assignments<float>());
+    CALL_SUBTEST_1(assignments<double>());
+
+    CALL_SUBTEST_2(plusMinus<float>());
+    CALL_SUBTEST_2(plusMinus<double>());
+    CALL_SUBTEST_2(multiplyScale<float>());
+    CALL_SUBTEST_2(multiplyScale<double>());
+    CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXf(3,internal::random<int>(1,EIGEN_TEST_MAX_SIZE))));
+    CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXd(3,internal::random<int>(1,EIGEN_TEST_MAX_SIZE))));
+    CALL_SUBTEST_2(traceAndDet<float>());
+    CALL_SUBTEST_2(traceAndDet<double>());
+    CALL_SUBTEST_2(transpose<float>());
+    CALL_SUBTEST_2(transpose<double>());
+
+    CALL_SUBTEST_3(exponentialIdentity<float>());
+    CALL_SUBTEST_3(exponentialIdentity<double>());
+    CALL_SUBTEST_3(exponentialOrthogonality<float>());
+    CALL_SUBTEST_3(exponentialOrthogonality<double>());
+    CALL_SUBTEST_3(exponentialRotation<float>());
+    CALL_SUBTEST_3(exponentialRotation<double>());
+  }
+}