29 #include "../Core/util/NonMPL2.h" 31 #ifndef EIGEN_SPARSE_AMD_H 32 #define EIGEN_SPARSE_AMD_H 38 template<
typename T>
inline T amd_flip(
const T& i) {
return -i-2; }
40 template<
typename T0,
typename T1>
inline bool amd_marked(
const T0* w,
const T1& j) {
return w[j]<0; }
41 template<
typename T0,
typename T1>
inline void amd_mark(
const T0* w,
const T1& j) {
return w[j] =
amd_flip(w[j]); }
44 template<
typename Index>
45 static int cs_wclear (Index mark, Index lemax, Index *w, Index n)
48 if(mark < 2 || (mark + lemax < 0))
50 for(k = 0; k < n; k++)
59 template<
typename Index>
60 Index
cs_tdfs(Index j, Index k, Index *
head,
const Index *
next, Index *post, Index *stack)
63 if(!head || !next || !post || !stack)
return (-1);
90 template<
typename Scalar,
typename Index>
95 int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
96 k2, k3, jlast,
ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
97 ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t;
101 dense = std::max<Index> (16, Index(10 *
sqrt(
double(n))));
102 dense = std::min<Index> (n-2, dense);
106 t = cnz + cnz/5 + 2*n;
109 Index* W =
new Index[8*(n+1)];
111 Index* nv = W + (n+1);
112 Index*
next = W + 2*(n+1);
113 Index*
head = W + 3*(n+1);
114 Index* elen = W + 4*(n+1);
115 Index* degree = W + 5*(n+1);
116 Index* w = W + 6*(n+1);
117 Index* hhead = W + 7*(n+1);
118 Index* last = perm.
indices().data();
123 for(k = 0; k < n; k++)
124 len[k] = Cp[k+1] - Cp[k];
128 for(i = 0; i <= n; i++)
139 mark = internal::cs_wclear<Index>(0, 0, w, n);
145 for(i = 0; i < n; i++)
165 if(head[d] != -1) last[head[d]] = i;
174 for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {}
175 if(next[k] != -1) last[next[k]] = -1;
176 head[mindeg] = next[k];
182 if(elenk > 0 && cnz + mindeg >= nzmax)
184 for(j = 0; j < n; j++)
192 for(q = 0, p = 0; p < cnz; )
198 for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++];
208 pk1 = (elenk == 0) ? p : cnz;
210 for(k1 = 1; k1 <= elenk + 1; k1++)
224 for(k2 = 1; k2 <=
ln; k2++)
227 if((nvi = nv[i]) <= 0)
continue;
231 if(next[i] != -1) last[next[i]] = last[i];
234 next[last[i]] = next[i];
238 head[degree[i]] = next[i];
247 if(elenk != 0) cnz = pk2;
254 mark = internal::cs_wclear<Index>(mark, lemax, w, n);
255 for(pk = pk1; pk < pk2; pk++)
258 if((eln = elen[i]) <= 0)
continue;
261 for(p = Cp[i]; p <= Cp[i] + eln - 1; p++)
270 w[e] = degree[e] + wnvi;
276 for(pk = pk1; pk < pk2; pk++)
280 p2 = p1 + elen[i] - 1;
282 for(h = 0, d = 0, p = p1; p <= p2; p++)
301 elen[i] = pn - p1 + 1;
304 for(p = p2 + 1; p < p4; p++)
307 if((nvj = nv[j]) <= 0)
continue;
324 degree[i] = std::min<Index> (degree[i], d);
328 len[i] = pn - p1 + 1;
336 lemax = std::max<Index>(lemax, dk);
337 mark = internal::cs_wclear<Index>(mark+lemax, lemax, w, n);
340 for(pk = pk1; pk < pk2; pk++)
343 if(nv[i] >= 0)
continue;
347 for(; i != -1 && next[i] != -1; i = next[i], mark++)
351 for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark;
353 for(j = next[i]; j != -1; )
355 ok = (len[j] ==
ln) && (elen[j] == eln);
356 for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++)
358 if(w[Ci[p]] != mark) ok = 0;
379 for(p = pk1, pk = pk1; pk < pk2; pk++)
382 if((nvi = -nv[i]) <= 0)
continue;
384 d = degree[i] + dk - nvi;
385 d = std::min<Index> (d, n - nel - nvi);
386 if(head[d] != -1) last[head[d]] = i;
390 mindeg = std::min<Index> (mindeg, d);
395 if((len[k] = p-pk1) == 0)
400 if(elenk != 0) cnz = p;
404 for(i = 0; i < n; i++) Cp[i] =
amd_flip (Cp[i]);
405 for(j = 0; j <= n; j++) head[j] = -1;
406 for(j = n; j >= 0; j--)
408 if(nv[j] > 0)
continue;
409 next[j] = head[Cp[j]];
412 for(e = n; e >= 0; e--)
414 if(nv[e] <= 0)
continue;
417 next[e] = head[Cp[e]];
421 for(k = 0, i = 0; i <= n; i++)
423 if(Cp[i] == -1) k = internal::cs_tdfs<Index>(i, k,
head,
next, perm.
indices().data(), w);
426 perm.
indices().conservativeResize(n);
435 #endif // EIGEN_SPARSE_AMD_H
IntermediateState sqrt(const Expression &arg)
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
void amd_mark(const T0 *w, const T1 &j)
void resizeNonZeros(Index size)
iterative scaling algorithm to equilibrate rows and column norms in matrices
const Index * outerIndexPtr() const
Index cs_tdfs(Index j, Index k, Index *head, const Index *next, Index *post, Index *stack)
bool amd_marked(const T0 *w, const T1 &j)
void minimum_degree_ordering(SparseMatrix< Scalar, ColMajor, Index > &C, PermutationMatrix< Dynamic, Dynamic, Index > &perm)
static int cs_wclear(Index mark, Index lemax, Index *w, Index n)
SegmentReturnType head(Index vecSize)
const IndicesType & indices() const
IntermediateState ln(const Expression &arg)
Expression next(const Expression &arg)
const Index * innerIndexPtr() const
void resize(Index newSize)