devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

merge with default branch

Browse Source
This commit is contained in:
Gael Guennebaud
2009-12-22 22:51:08 +01:00
parent e182e9c616 eeec7d842e
commit eaaba30cac
200 changed files with 3649 additions and 1636 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Eigen/src/LU/CMakeLists.txt
View File

@@ -4,3 +4,5 @@ INSTALL(FILES
${Eigen_LU_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LU COMPONENT Devel
)
ADD_SUBDIRECTORY(arch)

4
Eigen/src/LU/Determinant.h
View File

@@ -118,7 +118,9 @@ template<typename Derived>
inline typename ei_traits<Derived>::Scalar MatrixBase<Derived>::determinant() const
{
assert(rows() == cols());
return ei_determinant_impl<Derived>::run(derived());
typedef typename ei_nested<Derived,RowsAtCompileTime>::type Nested;
Nested nested(derived());
return ei_determinant_impl<typename ei_cleantype<Nested>::type>::run(nested);
}
#endif // EIGEN_DETERMINANT_H

73
Eigen/src/LU/FullPivLU.h
View File

@@ -14,7 +14,7 @@
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
@@ -63,8 +63,8 @@ template<typename _MatrixType> class FullPivLU
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef Matrix<int, 1, MatrixType::ColsAtCompileTime> IntRowVectorType;
typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> IntColVectorType;
typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime> RowVectorType;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVectorType;
typedef PermutationMatrix<MatrixType::ColsAtCompileTime> PermutationQType;
typedef PermutationMatrix<MatrixType::RowsAtCompileTime> PermutationPType;
/**
* \brief Default Constructor.
@@ -119,26 +119,22 @@ template<typename _MatrixType> class FullPivLU
* diagonal coefficient of U.
*/
RealScalar maxPivot() const { return m_maxpivot; }
/** \returns a vector of integers, whose size is the number of rows of the matrix being decomposed,
* representing the P permutation i.e. the permutation of the rows. For its precise meaning,
* see the examples given in the documentation of class FullPivLU.
/** \returns the permutation matrix P
*
* \sa permutationQ()
*/
inline const IntColVectorType& permutationP() const
inline const PermutationPType& permutationP() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
return m_p;
}
/** \returns a vector of integers, whose size is the number of columns of the matrix being
* decomposed, representing the Q permutation i.e. the permutation of the columns.
* For its precise meaning, see the examples given in the documentation of class FullPivLU.
/** \returns the permutation matrix Q
*
* \sa permutationP()
*/
inline const IntRowVectorType& permutationQ() const
inline const PermutationQType& permutationQ() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
return m_q;
@@ -238,8 +234,9 @@ template<typename _MatrixType> class FullPivLU
* who need to determine when pivots are to be considered nonzero. This is not used for the
* LU decomposition itself.
*
* When it needs to get the threshold value, Eigen calls threshold(). By default, this calls
* defaultThreshold(). Once you have called the present method setThreshold(const RealScalar&),
* When it needs to get the threshold value, Eigen calls threshold(). By default, this
* uses a formula to automatically determine a reasonable threshold.
* Once you have called the present method setThreshold(const RealScalar&),
* your value is used instead.
*
* \param threshold The new value to use as the threshold.
@@ -307,7 +304,7 @@ template<typename _MatrixType> class FullPivLU
inline int dimensionOfKernel() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
return m_lu.cols() - rank();
return cols() - rank();
}
/** \returns true if the matrix of which *this is the LU decomposition represents an injective
@@ -320,7 +317,7 @@ template<typename _MatrixType> class FullPivLU
inline bool isInjective() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
return rank() == m_lu.cols();
return rank() == cols();
}
/** \returns true if the matrix of which *this is the LU decomposition represents a surjective
@@ -333,7 +330,7 @@ template<typename _MatrixType> class FullPivLU
inline bool isSurjective() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
return rank() == m_lu.rows();
return rank() == rows();
}
/** \returns true if the matrix of which *this is the LU decomposition is invertible.
@@ -355,12 +352,12 @@ template<typename _MatrixType> class FullPivLU
*
* \sa MatrixBase::inverse()
*/
inline const ei_solve_retval<FullPivLU,NestByValue<typename MatrixType::IdentityReturnType> > inverse() const
inline const ei_solve_retval<FullPivLU,typename MatrixType::IdentityReturnType> inverse() const
{
ei_assert(m_isInitialized && "LU is not initialized.");
ei_assert(m_lu.rows() == m_lu.cols() && "You can't take the inverse of a non-square matrix!");
return ei_solve_retval<FullPivLU,NestByValue<typename MatrixType::IdentityReturnType> >
(*this, MatrixType::Identity(m_lu.rows(), m_lu.cols()).nestByValue());
return ei_solve_retval<FullPivLU,typename MatrixType::IdentityReturnType>
(*this, MatrixType::Identity(m_lu.rows(), m_lu.cols()));
}
inline int rows() const { return m_lu.rows(); }
@@ -368,8 +365,8 @@ template<typename _MatrixType> class FullPivLU
protected:
MatrixType m_lu;
IntColVectorType m_p;
IntRowVectorType m_q;
PermutationPType m_p;
PermutationQType m_q;
int m_det_pq, m_nonzero_pivots;
RealScalar m_maxpivot, m_prescribedThreshold;
bool m_isInitialized, m_usePrescribedThreshold;
@@ -393,8 +390,6 @@ FullPivLU<MatrixType>& FullPivLU<MatrixType>::compute(const MatrixType& matrix)
{
m_isInitialized = true;
m_lu = matrix;
m_p.resize(matrix.rows());
m_q.resize(matrix.cols());
const int size = matrix.diagonalSize();
const int rows = matrix.rows();
@@ -408,6 +403,7 @@ FullPivLU<MatrixType>& FullPivLU<MatrixType>::compute(const MatrixType& matrix)
m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
m_maxpivot = RealScalar(0);
for(int k = 0; k < size; ++k)
{
// First, we need to find the pivot.
@@ -424,10 +420,10 @@ FullPivLU<MatrixType>& FullPivLU<MatrixType>::compute(const MatrixType& matrix)
// if the pivot (hence the corner) is exactly zero, terminate to avoid generating nan/inf values
if(biggest_in_corner == RealScalar(0))
{
// before exiting, make sure to initialize the still uninitialized row_transpositions
// before exiting, make sure to initialize the still uninitialized transpositions
// in a sane state without destroying what we already have.
m_nonzero_pivots = k;
for(int i = k; i < size; i++)
for(int i = k; i < size; ++i)
{
rows_transpositions.coeffRef(i) = i;
cols_transpositions.coeffRef(i) = i;
@@ -463,13 +459,13 @@ FullPivLU<MatrixType>& FullPivLU<MatrixType>::compute(const MatrixType& matrix)
// the main loop is over, we still have to accumulate the transpositions to find the
// permutations P and Q
for(int k = 0; k < matrix.rows(); ++k) m_p.coeffRef(k) = k;
m_p.setIdentity(rows);
for(int k = size-1; k >= 0; --k)
std::swap(m_p.coeffRef(k), m_p.coeffRef(rows_transpositions.coeff(k)));
m_p.applyTranspositionOnTheRight(k, rows_transpositions.coeff(k));
for(int k = 0; k < matrix.cols(); ++k) m_q.coeffRef(k) = k;
m_q.setIdentity(cols);
for(int k = 0; k < size; ++k)
std::swap(m_q.coeffRef(k), m_q.coeffRef(cols_transpositions.coeff(k)));
m_q.applyTranspositionOnTheRight(k, cols_transpositions.coeff(k));
m_det_pq = (number_of_transpositions%2) ? -1 : 1;
return *this;
@@ -562,9 +558,9 @@ struct ei_kernel_retval<FullPivLU<_MatrixType> >
m.col(i).swap(m.col(pivots.coeff(i)));
// see the negative sign in the next line, that's what we were talking about above.
for(int i = 0; i < rank(); ++i) dst.row(dec().permutationQ().coeff(i)) = -m.row(i).end(dimker);
for(int i = rank(); i < cols; ++i) dst.row(dec().permutationQ().coeff(i)).setZero();
for(int k = 0; k < dimker; ++k) dst.coeffRef(dec().permutationQ().coeff(rank()+k), k) = Scalar(1);
for(int i = 0; i < rank(); ++i) dst.row(dec().permutationQ().indices().coeff(i)) = -m.row(i).end(dimker);
for(int i = rank(); i < cols; ++i) dst.row(dec().permutationQ().indices().coeff(i)).setZero();
for(int k = 0; k < dimker; ++k) dst.coeffRef(dec().permutationQ().indices().coeff(rank()+k), k) = Scalar(1);
}
};
@@ -601,7 +597,7 @@ struct ei_image_retval<FullPivLU<_MatrixType> >
ei_internal_assert(p == rank());
for(int i = 0; i < rank(); ++i)
dst.col(i) = originalMatrix().col(dec().permutationQ().coeff(pivots.coeff(i)));
dst.col(i) = originalMatrix().col(dec().permutationQ().indices().coeff(pivots.coeff(i)));
}
};
@@ -623,7 +619,7 @@ struct ei_solve_retval<FullPivLU<_MatrixType>, Rhs>
* Step 4: result = Q * c;
*/
const int rows = dec().matrixLU().rows(), cols = dec().matrixLU().cols(),
const int rows = dec().rows(), cols = dec().cols(),
nonzero_pivots = dec().nonzeroPivots();
ei_assert(rhs().rows() == rows);
const int smalldim = std::min(rows, cols);
@@ -637,8 +633,7 @@ struct ei_solve_retval<FullPivLU<_MatrixType>, Rhs>
typename Rhs::PlainMatrixType c(rhs().rows(), rhs().cols());
// Step 1
for(int i = 0; i < rows; ++i)
c.row(dec().permutationP().coeff(i)) = rhs().row(i);
c = dec().permutationP() * rhs();
// Step 2
dec().matrixLU()
@@ -660,9 +655,9 @@ struct ei_solve_retval<FullPivLU<_MatrixType>, Rhs>
// Step 4
for(int i = 0; i < nonzero_pivots; ++i)
dst.row(dec().permutationQ().coeff(i)) = c.row(i);
dst.row(dec().permutationQ().indices().coeff(i)) = c.row(i);
for(int i = nonzero_pivots; i < dec().matrixLU().cols(); ++i)
dst.row(dec().permutationQ().coeff(i)).setZero();
dst.row(dec().permutationQ().indices().coeff(i)).setZero();
}
};

111
Eigen/src/LU/Inverse.h
View File

@@ -182,91 +182,37 @@ struct ei_compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
*** Size 4 implementation ***
****************************/
template<typename MatrixType, typename ResultType>
void ei_compute_inverse_size4_helper(const MatrixType& matrix, ResultType& result)
template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
struct ei_compute_inverse_size4
{
/* Let's split M into four 2x2 blocks:
* (P Q)
* (R S)
* If P is invertible, with inverse denoted by P_inverse, and if
* (S - R*P_inverse*Q) is also invertible, then the inverse of M is
* (P' Q')
* (R' S')
* where
* S' = (S - R*P_inverse*Q)^(-1)
* P' = P1 + (P1*Q) * S' *(R*P_inverse)
* Q' = -(P_inverse*Q) * S'
* R' = -S' * (R*P_inverse)
*/
typedef Block<ResultType,2,2> XprBlock22;
typedef typename MatrixBase<XprBlock22>::PlainMatrixType Block22;
Block22 P_inverse;
ei_compute_inverse<XprBlock22, Block22>::run(matrix.template block<2,2>(0,0), P_inverse);
const Block22 Q = matrix.template block<2,2>(0,2);
const Block22 P_inverse_times_Q = P_inverse * Q;
const XprBlock22 R = matrix.template block<2,2>(2,0);
const Block22 R_times_P_inverse = R * P_inverse;
const Block22 R_times_P_inverse_times_Q = R_times_P_inverse * Q;
const XprBlock22 S = matrix.template block<2,2>(2,2);
const Block22 X = S - R_times_P_inverse_times_Q;
Block22 Y;
ei_compute_inverse<Block22, Block22>::run(X, Y);
result.template block<2,2>(2,2) = Y;
result.template block<2,2>(2,0) = - Y * R_times_P_inverse;
const Block22 Z = P_inverse_times_Q * Y;
result.template block<2,2>(0,2) = - Z;
result.template block<2,2>(0,0) = P_inverse + Z * R_times_P_inverse;
}
static void run(const MatrixType& matrix, ResultType& result)
{
result.coeffRef(0,0) = matrix.minor(0,0).determinant();
result.coeffRef(1,0) = -matrix.minor(0,1).determinant();
result.coeffRef(2,0) = matrix.minor(0,2).determinant();
result.coeffRef(3,0) = -matrix.minor(0,3).determinant();
result.coeffRef(0,2) = matrix.minor(2,0).determinant();
result.coeffRef(1,2) = -matrix.minor(2,1).determinant();
result.coeffRef(2,2) = matrix.minor(2,2).determinant();
result.coeffRef(3,2) = -matrix.minor(2,3).determinant();
result.coeffRef(0,1) = -matrix.minor(1,0).determinant();
result.coeffRef(1,1) = matrix.minor(1,1).determinant();
result.coeffRef(2,1) = -matrix.minor(1,2).determinant();
result.coeffRef(3,1) = matrix.minor(1,3).determinant();
result.coeffRef(0,3) = -matrix.minor(3,0).determinant();
result.coeffRef(1,3) = matrix.minor(3,1).determinant();
result.coeffRef(2,3) = -matrix.minor(3,2).determinant();
result.coeffRef(3,3) = matrix.minor(3,3).determinant();
result /= (matrix.col(0).cwise()*result.row(0).transpose()).sum();
}
};
template<typename MatrixType, typename ResultType>
struct ei_compute_inverse<MatrixType, ResultType, 4>
: ei_compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
MatrixType, ResultType>
{
static inline void run(const MatrixType& _matrix, ResultType& result)
{
typedef typename ResultType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
// we will do row permutations on the matrix. This copy should have negligible cost.
// if not, consider working in-place on the matrix (const-cast it, but then undo the permutations
// to nevertheless honor constness)
typename MatrixType::PlainMatrixType matrix(_matrix);
// let's extract from the 2 first colums a 2x2 block whose determinant is as big as possible.
int good_row0, good_row1, good_i;
Matrix<RealScalar,6,1> absdet;
// any 2x2 block with determinant above this threshold will be considered good enough
RealScalar d = (matrix.col(0).squaredNorm()+matrix.col(1).squaredNorm()) * RealScalar(1e-2);
#define ei_inv_size4_helper_macro(i,row0,row1) \
absdet[i] = ei_abs(matrix.coeff(row0,0)*matrix.coeff(row1,1) \
- matrix.coeff(row0,1)*matrix.coeff(row1,0)); \
if(absdet[i] > d) { good_row0=row0; good_row1=row1; goto good; }
ei_inv_size4_helper_macro(0,0,1)
ei_inv_size4_helper_macro(1,0,2)
ei_inv_size4_helper_macro(2,0,3)
ei_inv_size4_helper_macro(3,1,2)
ei_inv_size4_helper_macro(4,1,3)
ei_inv_size4_helper_macro(5,2,3)
// no 2x2 block has determinant bigger than the threshold. So just take the one that
// has the biggest determinant
absdet.maxCoeff(&good_i);
good_row0 = good_i <= 2 ? 0 : good_i <= 4 ? 1 : 2;
good_row1 = good_i <= 2 ? good_i+1 : good_i <= 4 ? good_i-1 : 3;
// now good_row0 and good_row1 are correctly set
good:
// do row permutations to move this 2x2 block to the top
matrix.row(0).swap(matrix.row(good_row0));
matrix.row(1).swap(matrix.row(good_row1));
// now applying our helper function is numerically stable
ei_compute_inverse_size4_helper(matrix, result);
// Since we did row permutations on the original matrix, we need to do column permutations
// in the reverse order on the inverse
result.col(1).swap(result.col(good_row1));
result.col(0).swap(result.col(good_row0));
}
// FIXME empty?
};
template<typename MatrixType, typename ResultType>
@@ -300,7 +246,8 @@ template<typename MatrixType>
struct ei_inverse_impl : public ReturnByValue<ei_inverse_impl<MatrixType> >
{
// for 2x2, it's worth giving a chance to avoid evaluating.
// for larger sizes, evaluating has negligible cost and limits code size.
// for larger sizes, evaluating has negligible cost, limits code size,
// and allows for vectorized paths.
typedef typename ei_meta_if<
MatrixType::RowsAtCompileTime == 2,
typename ei_nested<MatrixType,2>::type,
@@ -326,7 +273,7 @@ struct ei_inverse_impl : public ReturnByValue<ei_inverse_impl<MatrixType> >
*
* \returns the matrix inverse of this matrix.
*
* For small fixed sizes up to 4x4, this method uses ad-hoc methods (cofactors up to 3x3, Euler's trick for 4x4).
* For small fixed sizes up to 4x4, this method uses cofactors.
* In the general case, this method uses class PartialPivLU.
*
* \note This matrix must be invertible, otherwise the result is undefined. If you need an

29
Eigen/src/LU/PartialPivLU.h
View File

@@ -64,10 +64,8 @@ template<typename _MatrixType> class PartialPivLU
typedef _MatrixType MatrixType;
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef Matrix<int, 1, MatrixType::ColsAtCompileTime> IntRowVectorType;
typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> IntColVectorType;
typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime> RowVectorType;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVectorType;
typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> PermutationVectorType;
typedef PermutationMatrix<MatrixType::RowsAtCompileTime> PermutationType;
enum { MaxSmallDimAtCompileTime = EIGEN_ENUM_MIN(
MatrixType::MaxColsAtCompileTime,
@@ -105,11 +103,9 @@ template<typename _MatrixType> class PartialPivLU
return m_lu;
}
/** \returns a vector of integers, whose size is the number of rows of the matrix being decomposed,
* representing the P permutation i.e. the permutation of the rows. For its precise meaning,
* see the examples given in the documentation of class FullPivLU.
/** \returns the permutation matrix P.
*/
inline const IntColVectorType& permutationP() const
inline const PermutationType& permutationP() const
{
ei_assert(m_isInitialized && "PartialPivLU is not initialized.");
return m_p;
@@ -147,11 +143,11 @@ template<typename _MatrixType> class PartialPivLU
*
* \sa MatrixBase::inverse(), LU::inverse()
*/
inline const ei_solve_retval<PartialPivLU,NestByValue<typename MatrixType::IdentityReturnType> > inverse() const
inline const ei_solve_retval<PartialPivLU,typename MatrixType::IdentityReturnType> inverse() const
{
ei_assert(m_isInitialized && "PartialPivLU is not initialized.");
return ei_solve_retval<PartialPivLU,NestByValue<typename MatrixType::IdentityReturnType> >
(*this, MatrixType::Identity(m_lu.rows(), m_lu.cols()).nestByValue());
return ei_solve_retval<PartialPivLU,typename MatrixType::IdentityReturnType>
(*this, MatrixType::Identity(m_lu.rows(), m_lu.cols()));
}
/** \returns the determinant of the matrix of which
@@ -174,7 +170,7 @@ template<typename _MatrixType> class PartialPivLU
protected:
MatrixType m_lu;
IntColVectorType m_p;
PermutationType m_p;
int m_det_p;
bool m_isInitialized;
};
@@ -379,20 +375,19 @@ template<typename MatrixType>
PartialPivLU<MatrixType>& PartialPivLU<MatrixType>::compute(const MatrixType& matrix)
{
m_lu = matrix;
m_p.resize(matrix.rows());
ei_assert(matrix.rows() == matrix.cols() && "PartialPivLU is only for square (and moreover invertible) matrices");
const int size = matrix.rows();
IntColVectorType rows_transpositions(size);
PermutationVectorType rows_transpositions(size);
int nb_transpositions;
ei_partial_lu_inplace(m_lu, rows_transpositions, nb_transpositions);
m_det_p = (nb_transpositions%2) ? -1 : 1;
for(int k = 0; k < size; ++k) m_p.coeffRef(k) = k;
m_p.setIdentity(size);
for(int k = size-1; k >= 0; --k)
std::swap(m_p.coeffRef(k), m_p.coeffRef(rows_transpositions.coeff(k)));
m_p.applyTranspositionOnTheRight(k, rows_transpositions.coeff(k));
m_isInitialized = true;
return *this;
@@ -428,7 +423,7 @@ struct ei_solve_retval<PartialPivLU<_MatrixType>, Rhs>
dst.resize(size, rhs().cols());
// Step 1
for(int i = 0; i < size; ++i) dst.row(dec().permutationP().coeff(i)) = rhs().row(i);
dst = dec().permutationP() * rhs();
// Step 2
dec().matrixLU().template triangularView<UnitLowerTriangular>().solveInPlace(dst);

6
Eigen/src/LU/arch/CMakeLists.txt Normal file
View File

@@ -0,0 +1,6 @@
FILE(GLOB Eigen_LU_arch_SRCS "*.h")
INSTALL(FILES
${Eigen_LU_arch_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LU/arch COMPONENT Devel
)

151
Eigen/src/LU/arch/Inverse_SSE.h Normal file
View File

@@ -0,0 +1,151 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 1999 Intel Corporation
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
// The SSE code for the 4x4 float matrix inverse in this file comes from the file
// ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf
// See page ii of that document for legal stuff. Not being lawyers, we just assume
// here that if Intel makes this document publically available, with source code
// and detailed explanations, it's because they want their CPUs to be fed with
// good code, and therefore they presumably don't mind us using it in Eigen.
#ifndef EIGEN_INVERSE_SSE_H
#define EIGEN_INVERSE_SSE_H
template<typename MatrixType, typename ResultType>
struct ei_compute_inverse_size4<Architecture::SSE, float, MatrixType, ResultType>
{
static void run(const MatrixType& matrix, ResultType& result)
{
// Variables (Streaming SIMD Extensions registers) which will contain cofactors and, later, the
// lines of the inverted matrix.
__m128 minor0, minor1, minor2, minor3;
// Variables which will contain the lines of the reference matrix and, later (after the transposition),
// the columns of the original matrix.
__m128 row0, row1, row2, row3;
// Temporary variables and the variable that will contain the matrix determinant.
__m128 det, tmp1;
// Matrix transposition
const float *src = matrix.data();
tmp1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+8))), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+ 2))), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+10))), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
// Cofactors calculation. Because in the process of cofactor computation some pairs in three-
// element products are repeated, it is not reasonable to load these pairs anew every time. The
// values in the registers with these pairs are formed using shuffle instruction. Cofactors are
// calculated row by row (4 elements are placed in 1 SP FP SIMD floating point register).
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
// Evaluation of determinant and its reciprocal value. In the original Intel document,
// 1/det was evaluated using a fast rcpps command with subsequent approximation using
// the Newton-Raphson algorithm. Here, we go for a IEEE-compliant division instead,
// so as to not compromise precision at all.
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
// tmp1= _mm_rcp_ss(det);
// det= _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_div_ss(_mm_set_ss(1.0f), det); // <--- yay, one original line not copied from Intel
det = _mm_shuffle_ps(det, det, 0x00);
// warning, Intel's variable naming is very confusing: now 'det' is 1/det !
// Multiplication of cofactors by 1/det. Storing the inverse matrix to the address in pointer src.
minor0 = _mm_mul_ps(det, minor0);
float *dst = result.data();
_mm_storel_pi((__m64*)(dst), minor0);
_mm_storeh_pi((__m64*)(dst+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(dst+4), minor1);
_mm_storeh_pi((__m64*)(dst+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(dst+ 8), minor2);
_mm_storeh_pi((__m64*)(dst+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(dst+12), minor3);
_mm_storeh_pi((__m64*)(dst+14), minor3);
}
};
#endif // EIGEN_INVERSE_SSE_H