162 :
public Eigen::ReturnByValue<TridiagonalSymmetricMatrixTpl<_Scalar, _Options>>
172 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> CoeffVectorType;
179 , m_sub_diagonal(size - 1)
181 assert(size > 0 &&
"size should be greater than 0.");
188 return diagonal() ==
other.diagonal() && subDiagonal() ==
other.subDiagonal();
191 bool operator!=(
const TridiagonalSymmetricMatrixTpl & other)
const
193 return !(*
this == other);
196 TridiagonalSymmetricMatrixInverse<Self> inverse()
const
198 return TridiagonalSymmetricMatrixInverse<Self>(*
this);
201 CoeffVectorType & diagonal()
205 const CoeffVectorType & diagonal()
const
209 CoeffVectorType & subDiagonal()
211 return m_sub_diagonal;
213 const CoeffVectorType & subDiagonal()
const
215 return m_sub_diagonal;
220 diagonal().setOnes();
221 subDiagonal().setZero();
224 bool isIdentity(
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
226 return subDiagonal().isZero(prec) && diagonal().isOnes(prec);
231 diagonal().setZero();
232 subDiagonal().setZero();
235 bool isZero(
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
237 return subDiagonal().isZero(prec) && diagonal().isZero(prec);
242 diagonal().setRandom();
243 subDiagonal().setRandom();
246 bool isDiagonal(
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
248 return subDiagonal().isZero(prec);
251 template<
typename VectorCoeffType>
252 void setDiagonal(
const Eigen::MatrixBase<VectorCoeffType> & diagonal_coefficients)
254 PINOCCHIO_CHECK_ARGUMENT_SIZE(diagonal_coefficients.size(), cols());
256 VectorCoeffType::IsVectorAtCompileTime,
257 "VectorCoeffType should be a vector type at compile time");
259 diagonal() = diagonal_coefficients;
260 subDiagonal().setZero();
263 EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT
267 EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT
272 PlainMatrixType matrix()
const
274 return PlainMatrixType(*
this);
277 template<
typename ResultType>
278 inline void evalTo(ResultType & result)
const
281 result.template diagonal<1>() = subDiagonal().conjugate();
282 result.diagonal() = diagonal();
283 result.template diagonal<-1>() = subDiagonal();
286 template<
typename MatrixDerived>
287 TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived>
288 applyOnTheRight(
const Eigen::MatrixBase<MatrixDerived> & mat)
const
290 typedef TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived> ReturnType;
291 return ReturnType(*
this, mat.derived());
294 template<
typename MatrixDerived>
295 TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<MatrixDerived, Self>
296 applyOnTheLeft(
const Eigen::MatrixBase<MatrixDerived> & mat)
const
298 typedef TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<MatrixDerived, Self> ReturnType;
299 return ReturnType(mat.derived(), *
this);
302 template<
typename MatrixDerived>
303 inline TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived>
304 operator*(
const Eigen::MatrixBase<MatrixDerived> & mat)
const
306 return this->applyOnTheRight(mat.derived());
310 Eigen::DenseIndex m_size;
311 CoeffVectorType m_diagonal;
312 CoeffVectorType m_sub_diagonal;
320 const Eigen::MatrixBase<LhsMatrixType> & lhs,
const TridiagonalSymmetricMatrixTpl<S, O> & rhs)
327 :
public Eigen::ReturnByValue<TridiagonalSymmetricMatrixApplyOnTheRightReturnType<
328 TridiagonalSymmetricMatrix,
341 template<
typename ResultType>
344 PINOCCHIO_CHECK_ARGUMENT_SIZE(
result.rows(), rows());
345 PINOCCHIO_CHECK_ARGUMENT_SIZE(
result.cols(), cols());
352 result.noalias() = m_lhs.diagonal().asDiagonal() * m_rhs;
355 m_lhs.subDiagonal().conjugate().asDiagonal() * m_rhs.bottomRows(
reduced_size);
358 m_lhs.subDiagonal().asDiagonal() * m_rhs.topRows(
reduced_size);
371 const TridiagonalSymmetricMatrix & m_lhs;
426 :
public Eigen::ReturnByValue<TridiagonalSymmetricMatrixInverse<_TridiagonalSymmetricMatrix>>
433 Options = TridiagonalSymmetricMatrix::Options
436 typedef typename TridiagonalSymmetricMatrix::CoeffVectorType CoeffVectorType;
440 const TridiagonalSymmetricMatrix & tridiagonal_matrix)
441 : tridiagonal_matrix(tridiagonal_matrix)
442 , m_size(tridiagonal_matrix.rows())
444 , m_sub_diagonal(m_size - 1)
449 const TridiagonalSymmetricMatrix & inverse()
const
451 return tridiagonal_matrix;
454 template<
typename MatrixDerived>
456 applyOnTheRight(
const Eigen::MatrixBase<MatrixDerived> &
mat)
const
460 return ReturnType(*
this,
mat.derived());
463 template<
typename MatrixDerived>
465 operator*(
const Eigen::MatrixBase<MatrixDerived> &
mat)
const
467 return this->applyOnTheRight(
mat.derived());
470 template<
typename ResultType>
473 PINOCCHIO_CHECK_ARGUMENT_SIZE(
result.rows(), rows());
474 PINOCCHIO_CHECK_ARGUMENT_SIZE(
result.cols(), cols());
479 const auto & b = m_diagonal;
480 const auto & c = tridiagonal_matrix.subDiagonal();
481 const auto & w = m_sub_diagonal;
485 for (Eigen::DenseIndex
i = 1;
i < m_size; ++
i)
491 result.row(m_size - 1) /= b[m_size - 1];
492 for (Eigen::DenseIndex
i = m_size - 2;
i >= 0; --
i)
509 template<
typename T,
typename MatrixDerived>
515 m_diagonal = tridiagonal_matrix.diagonal();
516 m_sub_diagonal = tridiagonal_matrix.subDiagonal();
517 auto & w = m_sub_diagonal;
518 auto & b = m_diagonal;
519 const auto & c = tridiagonal_matrix.subDiagonal();
520 for (Eigen::DenseIndex
i = 1;
i < m_size; ++
i)
522 w.coeffRef(
i - 1) /= b[
i - 1];
523 m_diagonal.coeffRef(
i) -= w[
i - 1] * c[
i - 1];
527 const TridiagonalSymmetricMatrix & tridiagonal_matrix;
528 Eigen::DenseIndex m_size;
529 CoeffVectorType m_diagonal;
530 CoeffVectorType m_sub_diagonal;