mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-04-19 08:09:36 +08:00
Feature/skew symmetric matrix3
This commit is contained in:
Thomas Gloor
Rasmus Munk Larsen
parent
311ba66f7c
commit
ec9c7163a3
@ -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"
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
412
Eigen/src/Core/SkewSymmetricMatrix3.h
Normal file
412
Eigen/src/Core/SkewSymmetricMatrix3.h
Normal file
@ -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
|
||||
@ -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"; } };
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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; };
|
||||
|
||||
@ -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)
|
||||
|
||||
221
test/skew_symmetric_matrix3.cpp
Normal file
221
test/skew_symmetric_matrix3.cpp
Normal file
@ -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>());
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user