21#ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
22#define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
25#include "./InternalHeaderCheck.h"
31template <
typename Scalar,
typename StorageIndex>
32struct simpl_chol_helper {
33 using CholMatrixType = SparseMatrix<Scalar, ColMajor, StorageIndex>;
34 using InnerIterator =
typename CholMatrixType::InnerIterator;
35 using VectorI = Matrix<StorageIndex, Dynamic, 1>;
36 static constexpr StorageIndex kEmpty = -1;
43 const Index m_maxSize;
44 Stack(StorageIndex* data, StorageIndex size, StorageIndex maxSize)
45 : m_data(data), m_size(convert_index<Index>(size)), m_maxSize(convert_index<Index>(maxSize)) {
46 eigen_assert(size >= 0);
47 eigen_assert(maxSize >= size);
50 Stack(StorageIndex* data, StorageIndex size, StorageIndex )
51 : m_data(data), m_size(convert_index<Index>(size)) {}
53 bool empty()
const {
return m_size == 0; }
54 Index size()
const {
return m_size; }
55 StorageIndex back()
const {
56 eigen_assert(m_size > 0);
57 return m_data[m_size - 1];
59 void push(
const StorageIndex& value) {
61 eigen_assert(m_size < m_maxSize);
63 m_data[m_size] = value;
67 eigen_assert(m_size > 0);
75 DisjointSet(StorageIndex* set, StorageIndex size) : m_set(set) { std::iota(set, set + size, 0); }
77 StorageIndex find(StorageIndex u)
const {
78 eigen_assert(u != kEmpty);
79 while (m_set[u] != u) {
86 void compress(StorageIndex u, StorageIndex v) {
87 eigen_assert(u != kEmpty);
88 eigen_assert(v != kEmpty);
89 while (m_set[u] != v) {
90 StorageIndex next = m_set[u];
102 static void calc_hadj_outer(
const StorageIndex size,
const CholMatrixType& ap, StorageIndex* outerIndex) {
103 for (StorageIndex j = 1; j < size; j++) {
104 for (InnerIterator it(ap, convert_index<Index>(j)); it; ++it) {
105 StorageIndex i = it.index();
106 if (i < j) outerIndex[i + 1]++;
109 std::partial_sum(outerIndex, outerIndex + size + 1, outerIndex);
113 static void calc_hadj_inner(
const StorageIndex size,
const CholMatrixType& ap,
const StorageIndex* outerIndex,
114 StorageIndex* innerIndex, StorageIndex* tmp) {
115 std::fill_n(tmp, size, 0);
117 for (StorageIndex j = 1; j < size; j++) {
118 for (InnerIterator it(ap, convert_index<Index>(j)); it; ++it) {
119 StorageIndex i = it.index();
121 StorageIndex b = outerIndex[i] + tmp[i];
137 static void calc_etree(
const StorageIndex size,
const CholMatrixType& ap, StorageIndex* parent, StorageIndex* tmp) {
138 std::fill_n(parent, size, kEmpty);
140 DisjointSet ancestor(tmp, size);
142 for (StorageIndex j = 1; j < size; j++) {
143 for (InnerIterator it(ap, convert_index<Index>(j)); it; ++it) {
144 StorageIndex i = it.index();
146 StorageIndex r = ancestor.find(i);
147 if (r != j) parent[r] = j;
148 ancestor.compress(i, j);
155 static void calc_lineage(
const StorageIndex size,
const StorageIndex* parent, StorageIndex* firstChild,
156 StorageIndex* firstSibling) {
157 std::fill_n(firstChild, size, kEmpty);
158 std::fill_n(firstSibling, size, kEmpty);
160 for (StorageIndex j = 0; j < size; j++) {
161 StorageIndex p = parent[j];
162 if (p == kEmpty)
continue;
163 StorageIndex c = firstChild[p];
167 while (firstSibling[c] != kEmpty) c = firstSibling[c];
174 static void calc_post(
const StorageIndex size,
const StorageIndex* parent, StorageIndex* firstChild,
175 const StorageIndex* firstSibling, StorageIndex* post, StorageIndex* dfs) {
176 Stack post_stack(post, 0, size);
177 for (StorageIndex j = 0; j < size; j++) {
178 if (parent[j] != kEmpty)
continue;
180 Stack dfs_stack(dfs, 0, size);
182 while (!dfs_stack.empty()) {
183 StorageIndex i = dfs_stack.back();
184 StorageIndex c = firstChild[i];
191 firstChild[i] = firstSibling[c];
195 eigen_assert(post_stack.size() == size);
196 eigen_assert(std::all_of(firstChild, firstChild + size, [](StorageIndex a) {
return a == kEmpty; }));
205 static void calc_colcount(
const StorageIndex size,
const StorageIndex* hadjOuter,
const StorageIndex* hadjInner,
206 const StorageIndex* parent, StorageIndex* prevLeaf, StorageIndex* tmp,
207 const StorageIndex* post, StorageIndex* nonZerosPerCol,
bool doLDLT) {
209 std::fill_n(nonZerosPerCol, size, 1);
210 for (StorageIndex j = 0; j < size; j++) {
211 StorageIndex p = parent[j];
213 if (p != kEmpty) nonZerosPerCol[p] = 0;
216 DisjointSet parentSet(tmp, size);
218 eigen_assert(std::all_of(prevLeaf, prevLeaf + size, [](StorageIndex a) {
return a == kEmpty; }));
220 for (StorageIndex j_ = 0; j_ < size; j_++) {
221 StorageIndex j = post[j_];
222 nonZerosPerCol[j] += hadjOuter[j + 1] - hadjOuter[j];
223 for (StorageIndex k = hadjOuter[j]; k < hadjOuter[j + 1]; k++) {
224 StorageIndex i = hadjInner[k];
226 StorageIndex prev = prevLeaf[i];
227 if (prev != kEmpty) {
228 StorageIndex q = parentSet.find(prev);
229 parentSet.compress(prev, q);
234 StorageIndex p = parent[j];
235 if (p != kEmpty) parentSet.compress(j, p);
238 for (StorageIndex j = 0; j < size; j++) {
239 StorageIndex p = parent[j];
240 if (p != kEmpty) nonZerosPerCol[p] += nonZerosPerCol[j] - 1;
241 if (doLDLT) nonZerosPerCol[j]--;
246 static void init_matrix(
const StorageIndex size,
const StorageIndex* nonZerosPerCol, CholMatrixType& L) {
247 eigen_assert(L.outerIndexPtr()[0] == 0);
248 std::partial_sum(nonZerosPerCol, nonZerosPerCol + size, L.outerIndexPtr() + 1);
249 L.resizeNonZeros(convert_index<Index>(L.outerIndexPtr()[size]));
253 static void run(
const StorageIndex size,
const CholMatrixType& ap, CholMatrixType& L, VectorI& parent,
254 VectorI& workSpace,
bool doLDLT) {
255 parent.resize(convert_index<Index>(size));
256 workSpace.resize(4 * convert_index<Index>(size));
257 L.resize(convert_index<Index>(size), convert_index<Index>(size));
259 StorageIndex* tmp1 = workSpace.data();
260 StorageIndex* tmp2 = workSpace.data() + size;
261 StorageIndex* tmp3 = workSpace.data() + 2 * size;
262 StorageIndex* tmp4 = workSpace.data() + 3 * size;
265 StorageIndex* hadj_outer = L.outerIndexPtr();
266 calc_hadj_outer(size, ap, hadj_outer);
268 ei_declare_aligned_stack_constructed_variable(StorageIndex, hadj_inner, convert_index<Index>(hadj_outer[size]),
270 calc_hadj_inner(size, ap, hadj_outer, hadj_inner, tmp1);
272 calc_etree(size, ap, parent.data(), tmp1);
273 calc_lineage(size, parent.data(), tmp1, tmp2);
274 calc_post(size, parent.data(), tmp1, tmp2, tmp3, tmp4);
275 calc_colcount(size, hadj_outer, hadj_inner, parent.data(), tmp1, tmp2, tmp3, tmp4, doLDLT);
276 init_matrix(size, tmp4, L);
282#if EIGEN_COMP_CXXVER < 17
283template <
typename Scalar,
typename StorageIndex>
284constexpr StorageIndex simpl_chol_helper<Scalar, StorageIndex>::kEmpty;
289template <
typename Derived>
290void SimplicialCholeskyBase<Derived>::analyzePattern_preordered(
const CholMatrixType& ap,
bool doLDLT) {
291 using Helper = internal::simpl_chol_helper<Scalar, StorageIndex>;
293 eigen_assert(ap.innerSize() == ap.outerSize());
294 const StorageIndex size = internal::convert_index<StorageIndex>(ap.outerSize());
296 Helper::run(size, ap, m_matrix, m_parent, m_workSpace, doLDLT);
298 m_isInitialized =
true;
300 m_analysisIsOk =
true;
301 m_factorizationIsOk =
false;
304template <
typename Derived>
305template <
bool DoLDLT,
bool NonHermitian>
306void SimplicialCholeskyBase<Derived>::factorize_preordered(
const CholMatrixType& ap) {
307 EIGEN_IF_CONSTEXPR (internal::packet_traits<Scalar>::Vectorizable && internal::packet_traits<Scalar>::HasMul &&
308 internal::packet_traits<Scalar>::HasSub && !NumTraits<Scalar>::IsComplex) {
309 const StorageIndex* outer = m_matrix.outerIndexPtr();
310 for (Index i = 0; i < m_matrix.cols(); ++i) {
312 if (outer[i + 1] - outer[i] > internal::kSparseScatterPacketMinSize + (DoLDLT ? 0 : 1)) {
313 factorize_preordered_impl<DoLDLT, NonHermitian, true>(ap);
318 factorize_preordered_impl<DoLDLT, NonHermitian, false>(ap);
321template <
typename Derived>
322template <
bool DoLDLT,
bool NonHermitian,
bool UsePackets>
323void SimplicialCholeskyBase<Derived>::factorize_preordered_impl(
const CholMatrixType& ap) {
324 const StorageIndex size = StorageIndex(ap.rows());
326 eigen_assert(m_analysisIsOk &&
"You must first call analyzePattern()");
327 EIGEN_INTERNAL_ASSERT_SQUARE(ap);
328 eigen_assert(m_parent.size() == size);
329 eigen_assert(m_workSpace.size() >= 3 * size);
331 const StorageIndex* Lp = m_matrix.outerIndexPtr();
332 StorageIndex* Li = m_matrix.innerIndexPtr();
333 Scalar* Lx = m_matrix.valuePtr();
335 ei_declare_aligned_stack_constructed_variable(Scalar, y, internal::convert_index<Index>(size), 0);
336 StorageIndex* nonZerosPerCol = m_workSpace.data();
337 StorageIndex* pattern = m_workSpace.data() + size;
338 StorageIndex* tags = m_workSpace.data() + 2 * size;
341 m_diag.resize(DoLDLT ? internal::convert_index<Index>(size) : 0);
343 for (StorageIndex k = 0; k < size; ++k) {
346 StorageIndex top = size;
348 nonZerosPerCol[k] = 0;
349 for (
typename CholMatrixType::InnerIterator it(ap, internal::convert_index<Index>(k)); it; ++it) {
350 StorageIndex i = it.index();
352 y[i] += getSymm(it.value());
354 for (len = 0; tags[i] != k; i = m_parent[internal::convert_index<Index>(i)]) {
358 while (len > 0) pattern[--top] = pattern[--len];
365 getDiag(y[k]) * m_shiftScale + m_shiftOffset;
367 for (; top < size; ++top) {
368 Index i = internal::convert_index<Index>(pattern[top]);
374 EIGEN_IF_CONSTEXPR (DoLDLT)
375 l_ki = yi / getDiag(m_diag[i]);
377 yi = l_ki = yi / Lx[Lp[i]];
379 Index p2 = internal::convert_index<Index>(Lp[i] + nonZerosPerCol[i]);
380 Index p = internal::convert_index<Index>(Lp[i]) + (DoLDLT ? 0 : 1);
381 EIGEN_IF_CONSTEXPR (UsePackets)
382 p += internal::sparse_scatter_sub_packets<!NonHermitian>(y, Li + p, Lx + p, p2 - p, yi);
383 for (; p < p2; ++p) y[Li[p]] -= getSymm(Lx[p]) * yi;
384 d -= getDiag(l_ki * getSymm(yi));
389 EIGEN_IF_CONSTEXPR (DoLDLT) {
390 m_diag[internal::convert_index<Index>(k)] = d;
391 if (d == RealScalar(0)) {
396 Index p = internal::convert_index<Index>(Lp[k] + nonZerosPerCol[k]++);
399 EIGEN_IF_CONSTEXPR (NonHermitian) {
402 failed = NumTraits<Scalar>::IsComplex ? (d == RealScalar(0)) : (numext::real(d) <= RealScalar(0));
404 failed = numext::real(d) <= RealScalar(0);
410 Lx[p] = numext::sqrt(d);
415 m_factorizationIsOk =
true;
@ NumericalIssue
Definition Constants.h:459
@ Success
Definition Constants.h:457