1namespace Eigen {
2
3namespace internal {
4
5template <typename Scalar>
6void covar(
7        Matrix< Scalar, Dynamic, Dynamic > &r,
8        const VectorXi &ipvt,
9        Scalar tol = sqrt(NumTraits<Scalar>::epsilon()) )
10{
11    typedef DenseIndex Index;
12
13    /* Local variables */
14    Index i, j, k, l, ii, jj;
15    bool sing;
16    Scalar temp;
17
18    /* Function Body */
19    const Index n = r.cols();
20    const Scalar tolr = tol * abs(r(0,0));
21    Matrix< Scalar, Dynamic, 1 > wa(n);
22    assert(ipvt.size()==n);
23
24    /* form the inverse of r in the full upper triangle of r. */
25    l = -1;
26    for (k = 0; k < n; ++k)
27        if (abs(r(k,k)) > tolr) {
28            r(k,k) = 1. / r(k,k);
29            for (j = 0; j <= k-1; ++j) {
30                temp = r(k,k) * r(j,k);
31                r(j,k) = 0.;
32                r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
33            }
34            l = k;
35        }
36
37    /* form the full upper triangle of the inverse of (r transpose)*r */
38    /* in the full upper triangle of r. */
39    for (k = 0; k <= l; ++k) {
40        for (j = 0; j <= k-1; ++j)
41            r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
42        r.col(k).head(k+1) *= r(k,k);
43    }
44
45    /* form the full lower triangle of the covariance matrix */
46    /* in the strict lower triangle of r and in wa. */
47    for (j = 0; j < n; ++j) {
48        jj = ipvt[j];
49        sing = j > l;
50        for (i = 0; i <= j; ++i) {
51            if (sing)
52                r(i,j) = 0.;
53            ii = ipvt[i];
54            if (ii > jj)
55                r(ii,jj) = r(i,j);
56            if (ii < jj)
57                r(jj,ii) = r(i,j);
58        }
59        wa[jj] = r(j,j);
60    }
61
62    /* symmetrize the covariance matrix in r. */
63    r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
64    r.diagonal() = wa;
65}
66
67} // end namespace internal
68
69} // end namespace Eigen
70