Yoji KURODA / Eigen

Eigne Matrix Class Library

Dependents:   Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more

Eigen Matrix Class Library for mbed.

Finally, you can use Eigen on your mbed!!!

src/Core/DiagonalProduct.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:13a5d365ba16 1 // This file is part of Eigen, a lightweight C++ template library
ykuroda 0:13a5d365ba16 2 // for linear algebra.
ykuroda 0:13a5d365ba16 3 //
ykuroda 0:13a5d365ba16 4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
ykuroda 0:13a5d365ba16 5 // Copyright (C) 2007-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
ykuroda 0:13a5d365ba16 6 //
ykuroda 0:13a5d365ba16 7 // This Source Code Form is subject to the terms of the Mozilla
ykuroda 0:13a5d365ba16 8 // Public License v. 2.0. If a copy of the MPL was not distributed
ykuroda 0:13a5d365ba16 9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
ykuroda 0:13a5d365ba16 10
ykuroda 0:13a5d365ba16 11 #ifndef EIGEN_DIAGONALPRODUCT_H
ykuroda 0:13a5d365ba16 12 #define EIGEN_DIAGONALPRODUCT_H
ykuroda 0:13a5d365ba16 13
ykuroda 0:13a5d365ba16 14 namespace Eigen {
ykuroda 0:13a5d365ba16 15
ykuroda 0:13a5d365ba16 16 namespace internal {
ykuroda 0:13a5d365ba16 17 template<typename MatrixType, typename DiagonalType, int ProductOrder>
ykuroda 0:13a5d365ba16 18 struct traits<DiagonalProduct<MatrixType, DiagonalType, ProductOrder> >
ykuroda 0:13a5d365ba16 19 : traits<MatrixType>
ykuroda 0:13a5d365ba16 20 {
ykuroda 0:13a5d365ba16 21 typedef typename scalar_product_traits<typename MatrixType::Scalar, typename DiagonalType::Scalar>::ReturnType Scalar;
ykuroda 0:13a5d365ba16 22 enum {
ykuroda 0:13a5d365ba16 23 RowsAtCompileTime = MatrixType::RowsAtCompileTime,
ykuroda 0:13a5d365ba16 24 ColsAtCompileTime = MatrixType::ColsAtCompileTime,
ykuroda 0:13a5d365ba16 25 MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
ykuroda 0:13a5d365ba16 26 MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
ykuroda 0:13a5d365ba16 27
ykuroda 0:13a5d365ba16 28 _StorageOrder = MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor,
ykuroda 0:13a5d365ba16 29 _ScalarAccessOnDiag = !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft)
ykuroda 0:13a5d365ba16 30 ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)),
ykuroda 0:13a5d365ba16 31 _SameTypes = is_same<typename MatrixType::Scalar, typename DiagonalType::Scalar>::value,
ykuroda 0:13a5d365ba16 32 // FIXME currently we need same types, but in the future the next rule should be the one
ykuroda 0:13a5d365ba16 33 //_Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))),
ykuroda 0:13a5d365ba16 34 _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))),
ykuroda 0:13a5d365ba16 35 _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0,
ykuroda 0:13a5d365ba16 36
ykuroda 0:13a5d365ba16 37 Flags = ((HereditaryBits|_LinearAccessMask|AlignedBit) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0),//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit),
ykuroda 0:13a5d365ba16 38 Cost0 = EIGEN_ADD_COST(NumTraits<Scalar>::MulCost, MatrixType::CoeffReadCost),
ykuroda 0:13a5d365ba16 39 CoeffReadCost = EIGEN_ADD_COST(Cost0,DiagonalType::DiagonalVectorType::CoeffReadCost)
ykuroda 0:13a5d365ba16 40 };
ykuroda 0:13a5d365ba16 41 };
ykuroda 0:13a5d365ba16 42 }
ykuroda 0:13a5d365ba16 43
ykuroda 0:13a5d365ba16 44 template<typename MatrixType, typename DiagonalType, int ProductOrder>
ykuroda 0:13a5d365ba16 45 class DiagonalProduct : internal::no_assignment_operator,
ykuroda 0:13a5d365ba16 46 public MatrixBase<DiagonalProduct<MatrixType, DiagonalType, ProductOrder> >
ykuroda 0:13a5d365ba16 47 {
ykuroda 0:13a5d365ba16 48 public:
ykuroda 0:13a5d365ba16 49
ykuroda 0:13a5d365ba16 50 typedef MatrixBase<DiagonalProduct> Base;
ykuroda 0:13a5d365ba16 51 EIGEN_DENSE_PUBLIC_INTERFACE(DiagonalProduct)
ykuroda 0:13a5d365ba16 52
ykuroda 0:13a5d365ba16 53 inline DiagonalProduct(const MatrixType& matrix, const DiagonalType& diagonal)
ykuroda 0:13a5d365ba16 54 : m_matrix(matrix), m_diagonal(diagonal)
ykuroda 0:13a5d365ba16 55 {
ykuroda 0:13a5d365ba16 56 eigen_assert(diagonal.diagonal().size() == (ProductOrder == OnTheLeft ? matrix.rows() : matrix.cols()));
ykuroda 0:13a5d365ba16 57 }
ykuroda 0:13a5d365ba16 58
ykuroda 0:13a5d365ba16 59 EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); }
ykuroda 0:13a5d365ba16 60 EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); }
ykuroda 0:13a5d365ba16 61
ykuroda 0:13a5d365ba16 62 EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const
ykuroda 0:13a5d365ba16 63 {
ykuroda 0:13a5d365ba16 64 return m_diagonal.diagonal().coeff(ProductOrder == OnTheLeft ? row : col) * m_matrix.coeff(row, col);
ykuroda 0:13a5d365ba16 65 }
ykuroda 0:13a5d365ba16 66
ykuroda 0:13a5d365ba16 67 EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const
ykuroda 0:13a5d365ba16 68 {
ykuroda 0:13a5d365ba16 69 enum {
ykuroda 0:13a5d365ba16 70 StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor
ykuroda 0:13a5d365ba16 71 };
ykuroda 0:13a5d365ba16 72 return coeff(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx);
ykuroda 0:13a5d365ba16 73 }
ykuroda 0:13a5d365ba16 74
ykuroda 0:13a5d365ba16 75 template<int LoadMode>
ykuroda 0:13a5d365ba16 76 EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const
ykuroda 0:13a5d365ba16 77 {
ykuroda 0:13a5d365ba16 78 enum {
ykuroda 0:13a5d365ba16 79 StorageOrder = Flags & RowMajorBit ? RowMajor : ColMajor
ykuroda 0:13a5d365ba16 80 };
ykuroda 0:13a5d365ba16 81 const Index indexInDiagonalVector = ProductOrder == OnTheLeft ? row : col;
ykuroda 0:13a5d365ba16 82 return packet_impl<LoadMode>(row,col,indexInDiagonalVector,typename internal::conditional<
ykuroda 0:13a5d365ba16 83 ((int(StorageOrder) == RowMajor && int(ProductOrder) == OnTheLeft)
ykuroda 0:13a5d365ba16 84 ||(int(StorageOrder) == ColMajor && int(ProductOrder) == OnTheRight)), internal::true_type, internal::false_type>::type());
ykuroda 0:13a5d365ba16 85 }
ykuroda 0:13a5d365ba16 86
ykuroda 0:13a5d365ba16 87 template<int LoadMode>
ykuroda 0:13a5d365ba16 88 EIGEN_STRONG_INLINE PacketScalar packet(Index idx) const
ykuroda 0:13a5d365ba16 89 {
ykuroda 0:13a5d365ba16 90 enum {
ykuroda 0:13a5d365ba16 91 StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor
ykuroda 0:13a5d365ba16 92 };
ykuroda 0:13a5d365ba16 93 return packet<LoadMode>(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx);
ykuroda 0:13a5d365ba16 94 }
ykuroda 0:13a5d365ba16 95
ykuroda 0:13a5d365ba16 96 protected:
ykuroda 0:13a5d365ba16 97 template<int LoadMode>
ykuroda 0:13a5d365ba16 98 EIGEN_STRONG_INLINE PacketScalar packet_impl(Index row, Index col, Index id, internal::true_type) const
ykuroda 0:13a5d365ba16 99 {
ykuroda 0:13a5d365ba16 100 return internal::pmul(m_matrix.template packet<LoadMode>(row, col),
ykuroda 0:13a5d365ba16 101 internal::pset1<PacketScalar>(m_diagonal.diagonal().coeff(id)));
ykuroda 0:13a5d365ba16 102 }
ykuroda 0:13a5d365ba16 103
ykuroda 0:13a5d365ba16 104 template<int LoadMode>
ykuroda 0:13a5d365ba16 105 EIGEN_STRONG_INLINE PacketScalar packet_impl(Index row, Index col, Index id, internal::false_type) const
ykuroda 0:13a5d365ba16 106 {
ykuroda 0:13a5d365ba16 107 enum {
ykuroda 0:13a5d365ba16 108 InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime,
ykuroda 0:13a5d365ba16 109 DiagonalVectorPacketLoadMode = (LoadMode == Aligned && (((InnerSize%16) == 0) || (int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit)==AlignedBit) ? Aligned : Unaligned)
ykuroda 0:13a5d365ba16 110 };
ykuroda 0:13a5d365ba16 111 return internal::pmul(m_matrix.template packet<LoadMode>(row, col),
ykuroda 0:13a5d365ba16 112 m_diagonal.diagonal().template packet<DiagonalVectorPacketLoadMode>(id));
ykuroda 0:13a5d365ba16 113 }
ykuroda 0:13a5d365ba16 114
ykuroda 0:13a5d365ba16 115 typename MatrixType::Nested m_matrix;
ykuroda 0:13a5d365ba16 116 typename DiagonalType::Nested m_diagonal;
ykuroda 0:13a5d365ba16 117 };
ykuroda 0:13a5d365ba16 118
ykuroda 0:13a5d365ba16 119 /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal.
ykuroda 0:13a5d365ba16 120 */
ykuroda 0:13a5d365ba16 121 template<typename Derived>
ykuroda 0:13a5d365ba16 122 template<typename DiagonalDerived>
ykuroda 0:13a5d365ba16 123 inline const DiagonalProduct<Derived, DiagonalDerived, OnTheRight>
ykuroda 0:13a5d365ba16 124 MatrixBase<Derived>::operator*(const DiagonalBase<DiagonalDerived> &a_diagonal) const
ykuroda 0:13a5d365ba16 125 {
ykuroda 0:13a5d365ba16 126 return DiagonalProduct<Derived, DiagonalDerived, OnTheRight>(derived(), a_diagonal.derived());
ykuroda 0:13a5d365ba16 127 }
ykuroda 0:13a5d365ba16 128
ykuroda 0:13a5d365ba16 129 } // end namespace Eigen
ykuroda 0:13a5d365ba16 130
ykuroda 0:13a5d365ba16 131 #endif // EIGEN_DIAGONALPRODUCT_H