14using Eigen::internal::LDLT_Traits;
18template <
typename Scalar>
19void update_sign_matrix(SignMatrix &sign,
const Scalar &akk) {
21 case SignMatrix::PositiveSemiDef:
23 sign = SignMatrix::Indefinite;
25 case SignMatrix::NegativeSemiDef:
27 sign = SignMatrix::Indefinite;
29 case SignMatrix::ZeroSign: {
31 sign = SignMatrix::PositiveSemiDef;
33 sign = SignMatrix::NegativeSemiDef;
44template <
typename Derived>
47 using Scalar =
typename Derived::Scalar;
48 const isize n = a.rows();
51 sign = SignMatrix::ZeroSign;
53 sign = SignMatrix::PositiveSemiDef;
55 sign = SignMatrix::NegativeSemiDef;
57 sign = SignMatrix::ZeroSign;
63 auto l10 = a.row(j).head(j);
64 auto d0 = a.diagonal().head(j);
65 auto work = a.col(n - 1).head(j);
67 work = l10.transpose().cwiseProduct(d0);
69 Scalar &akk = a.coeffRef(j, j);
72 update_sign_matrix(sign, akk);
78 const isize rem = n - j - 1;
80 auto l20 = a.bottomLeftCorner(rem, j);
81 auto l21 = a.col(j).tail(rem);
83 l21.noalias() -= l20 * work;
89static constexpr isize UNBLK_THRESHOLD = 128;
93template <
typename Derived>
96 using PlainObject =
typename Derived::PlainObject;
97 using MatrixRef = Eigen::Ref<PlainObject>;
98 const isize n = a.rows();
99 if (n <= UNBLK_THRESHOLD) {
102 const isize bs = (n + 1) / 2;
103 const isize rem = n - bs;
105 MatrixRef l00 = a.block(0, 0, bs, bs);
106 Eigen::Block<Derived> l10 = a.block(bs, 0, rem, bs);
107 MatrixRef l11 = a.block(bs, bs, rem, rem);
110 auto d0 = l00.diagonal();
113 .template triangularView<Eigen::UnitUpper>()
114 .template solveInPlace<Eigen::OnTheRight>(l10);
116 auto work = a.block(0, bs, bs, rem).transpose();
118 l10 = l10 * d0.asDiagonal().inverse();
120 l11.template triangularView<Eigen::Lower>() -= l10 * work.transpose();
128template <
typename MatDerived,
typename Rhs>
130 using Scalar =
typename MatDerived::Scalar;
131 using Traits = LDLT_Traits<MatDerived, Eigen::Lower>;
132 Traits::getL(mat).solveInPlace(b);
135 auto vecD(mat.diagonal());
136 const Scalar tol = std::numeric_limits<Scalar>::min();
137 for (isize i = 0; i < vecD.size(); ++i) {
138 if (abs(vecD(i)) > tol)
144 Traits::getU(mat).solveInPlace(b);
148template <
typename Scalar>
153 using Traits = LDLT_Traits<ConstMatrixRef, Eigen::Lower>;
154 res = Traits::getU(mat) * res;
156 auto vecD = mat.diagonal();
157 res = vecD.asDiagonal() * res;
159 res = Traits::getL(mat) * res;
164template <
typename _Scalar>
struct DenseLDLT :
ldlt_base<_Scalar> {
165 using Scalar = _Scalar;
168 using DView =
typename Base::DView;
169 using MatrixType = MatrixXs;
171 DenseLDLT() =
default;
172 explicit DenseLDLT(isize size) : Base(), m_matrix(size, size) {
176 explicit DenseLDLT(MatrixRef a) : Base(), m_matrix(a) {
179 : Eigen::NumericalIssue;
182 DenseLDLT &compute(
const ConstMatrixRef &mat) {
186 : Eigen::NumericalIssue;
190 const MatrixXs &matrixLDLT()
const {
return m_matrix; }
192 template <
typename Derived>
193 bool solveInPlace(Eigen::MatrixBase<Derived> &b)
const {
197 template <
typename Rhs>
198 typename Rhs::PlainObject solve(
const Eigen::MatrixBase<Rhs> &rhs)
const {
199 typename Rhs::PlainObject out = rhs;
204 MatrixXs reconstructedMatrix()
const {
205 MatrixXs res(m_matrix.rows(), m_matrix.cols());
207 backend::dense_ldlt_reconstruct<Scalar>(m_matrix, res);
211 inline DView vectorD()
const {
return Base::diag_view_impl(m_matrix); }
bool dense_ldlt_solve_in_place(MatDerived &mat, Rhs &b)
bool ldlt_in_place_unblocked(Eigen::MatrixBase< Derived > &a, SignMatrix &sign)
bool dense_ldlt_in_place(Eigen::MatrixBase< Derived > &a, SignMatrix &sign)
#define PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar)
Specific linear algebra routines.
Base interface for LDLT solvers.
Typedefs for math (Eigen vectors, matrices) depending on scalar type.