blob: 6812236084a1a78d3b5440189a9985192a6e0ad4 [file] [log] [blame]
// 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
// IWYU pragma: private
#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>
{
EIGEN_DEVICE_FUNC
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);
}
EIGEN_DEVICE_FUNC
static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
{ dst.vector() += src.vector(); }
EIGEN_DEVICE_FUNC
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