ROOT logo

#include "hkalmatrixtools.h"

// from ROOT
#include "TDecompSVD.h"
#include "TMatrixDEigen.h"
#include "TMath.h"

// C/C++ libraries
#include <limits>
using namespace std;

ClassImp(HKalMatrixTools)

//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
//
// Some helper functions for matrices.
//
//-----------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////


Int_t HKalMatrixTools::checkSymmetry(TMatrixD &M, Double_t tol) {
    // Mind the meaning of the return value!
    //
    // Checks if input matrix M is (close to) symmetric.
    // Returns the amount of off-diagonal elements that were not
    // symmetric. Therefore a return value of 0 means the matrix
    // is symmetric!
    // For non quadratic matrices -1 will be returned.
    //
    // Input:
    // M:   The matrix to check.
    // tol: The allowed relative difference between diagonal
    //      elements so that the matrix is still seen as symmetric.

    Int_t nAsymCells = 0;
    if(M.GetNrows() != M.GetNcols()) {
        return -1;
    }

    Int_t dim = M.GetNrows();

    for(Int_t i = 0; i < dim; i++) {
        for(Int_t j = i+1; j < dim; j++) {
	    if(!TMath::AreEqualRel(M(i,j), M(j,i), tol)) {
                nAsymCells++;
            }
        }
    }

    return nAsymCells;
}

Bool_t HKalMatrixTools::checkCond(const TMatrixD &M) {
    // Calculate matrix condition using singular values.

    Bool_t bCondOk = kTRUE;

    // Machine precision:
    // 1 + eps   evaluates to 1 + eps
    // 1 + eps/2 evaluates to 1.
    Double_t eps = numeric_limits<Double_t>::epsilon();
    // A matrix is considered ill-conditioned for inversion if adding one to
    // its condition number in computer arithmetic has no effect.
    Double_t illCond = 2./eps;

    TDecompSVD decomp(M);
    Bool_t decompSuccess = decomp.Decompose();
    if(decompSuccess) {
        Double_t cond = decomp.Condition();
        if(cond >= illCond) {
            bCondOk = kFALSE;
        }
    } else {
        bCondOk = kFALSE;
        ::Warning("checkCond()", "Failed at SVD decomposition for matrix");
    }
    return bCondOk;
}

Bool_t HKalMatrixTools::decomposeUD(TMatrixD &M) {
    // A matrix can be written as the product M = U*D*U^T where
    // U is an upper triangular and D a diagonal matrix.
    // This function calculates the U and D factors of the input
    // matrix M. Both U and D can be stored in a single matrix.
    // Since the diagonal elements of U are always 1, they are
    // not explicitly stored and replaced by the elements of D.
    //
    // The input matrix M will be overwritten.

    Bool_t bNoErr = kTRUE;

#if kalDebug > 0
    if(checkSymmetry(M, 1.e-3) != 0) {
        ::Warning("decomposeUD()", "Input matrix is not symmetric.");
        M.Print();
        bNoErr = kFALSE;
    }
#endif

    Int_t dim = M.GetNrows();
    for(Int_t j = dim - 1; j >= 0; j--) {
        for(Int_t i = j; i >= 0; i--) {
            Double_t sigma = M(i,j);
            for(Int_t k = j+1; k < dim; k++) {
                sigma -= M(i,k) * M(k,k) * M(j,k);
            }
            if(i == j) {
                M(i,i) = sigma;
            } else {
                if(M(j,j) != 0.) {
                    M(i,j) = sigma / M(j,j);
                } else {
                    M(i,j) = 0.;
                }
            }
        }
    }

    return bNoErr;
}

Bool_t HKalMatrixTools::isPosDef(const TMatrixD &M) {
    // Returns true if matrix is positive definite, i.e. all eigenvalues are
    // real and greater than zero.

    Bool_t bPosDef = kTRUE;

    TMatrixDEigen eig(M);
    TVectorD eigRe  = eig.GetEigenValuesRe();
    TVectorD eigIm  = eig.GetEigenValuesIm();

    for(Int_t i = 0; i < eigRe.GetNrows(); i++) {
        if(eigRe(i) <= 0.) {
            bPosDef = kFALSE;
        }
        if(eigIm(i) != 0.) {
            bPosDef = kFALSE;
        }
    }

#if kalDebug > 0
    if(!bPosDef) {
        ::Warning("isPosDef()", "Matrix is not positive definite. An eigenvalue is not positive or is imaginary.");
    }
#endif
    return bPosDef;
}

Bool_t HKalMatrixTools::checkValidElems(const TMatrixD &M) {
    // Check for INFs and NaNs in matrix.

    for(Int_t iRow = 0; iRow < M.GetNrows(); iRow++) {
        for(Int_t iCol = 0; iCol < M.GetNcols(); iCol++) {
	    if(M(iRow, iCol) == numeric_limits<Double_t>::infinity() ||
	       TMath::IsNaN(M(iRow, iCol))) {
		return kFALSE;
            }
        }
    }
    return kTRUE;
}

Bool_t HKalMatrixTools::makeSymmetric(TMatrixD &M) {
    // Produce a valid symmetric matrix out of an almost symmetric TMatrixD
    // Input matrix M will be overwritten!

    Bool_t matSym = kTRUE;
    Int_t idx = TMath::Min(M.GetNrows(), M.GetNcols());

    for(Int_t i = 0; i < idx; i++) {
        for(Int_t j = i+1; j < idx; j++) {
            if(matSym && !TMath::AreEqualRel(M(i,j), M(j,i), 1.e-3)) {
                matSym = kFALSE;
            }
            Double_t average = (M(i,j) + M(j,i)) / 2.;
            M(i,j) = average;
            M(j,i) = average;
        }
    }
    return matSym;
}


Bool_t HKalMatrixTools::resolveUD(TMatrixD &U, TMatrixD &D, const TMatrixD &UD) {
    // Input is a matrix where the U and D factors are stored in a single
    // matrix.
    //
    // Extracts the U and D matrices as two separate objects from the input
    // matrix.
    // output:
    // U: upper triangular matrix.
    // D: diagonal matrix.
    // input:
    // UD: The U and D factors are stored in this single matrix.

    if(UD.GetNrows() != UD.GetNcols()) {
        ::Error("resolveUD()", "Input matrix is not quadratic.");
        return kFALSE;
    }
    Int_t dim = UD.GetNrows();
    if(U.GetNrows() != dim || U.GetNcols() != dim) {
	::Warning("resolveUD()",
		  "Output parameter for upper triangular matrix has wrong dimensions and has been resized.");
        U.ResizeTo(dim, dim);
    }
    if(D.GetNrows() != dim || D.GetNcols() != dim) {
	::Warning("resolveUD()",
		  "Output parameter for diagonal matrix has wrong dimensions and has been resized.");
        D.ResizeTo(dim, dim);
    }

#ifdef kalDebug
        Bool_t bWarnU = kFALSE;
        Bool_t bWarnD = kFALSE;
#endif

    U.UnitMatrix();
    for(Int_t i = 0; i < dim; i++) {
        D(i,i) = UD(i,i);
#ifdef kalDebug
        for(Int_t j = 0; j < dim; j++) {
            if(i < j) {
                U(i,j) = UD(i,j);
            } else {
                if(i != j && U(i,j) != 0.) {
                    if(!bWarnU) {
			::Warning("resolveUD()",
				  "Input matrix U is not an upper triangular matrix.");
                    }
                    bWarnU = kTRUE;
                    U.Print();
                }
            }
            if(i != j && D(i,j) != 0.) {
                if(!bWarnD) {
		    ::Warning("resolveUD()",
			      "Input matrix D is not diagonal.");
                }
                bWarnD = kTRUE;
                D.Print();
            }
        }
    }
    return !(bWarnU || bWarnD);
#else
        for(Int_t j = i + 1; j < dim; j++) {
            U(i,j) = UD(i,j);
        }
    }

    return kTRUE;
#endif
}
 hkalmatrixtools.cc:1
 hkalmatrixtools.cc:2
 hkalmatrixtools.cc:3
 hkalmatrixtools.cc:4
 hkalmatrixtools.cc:5
 hkalmatrixtools.cc:6
 hkalmatrixtools.cc:7
 hkalmatrixtools.cc:8
 hkalmatrixtools.cc:9
 hkalmatrixtools.cc:10
 hkalmatrixtools.cc:11
 hkalmatrixtools.cc:12
 hkalmatrixtools.cc:13
 hkalmatrixtools.cc:14
 hkalmatrixtools.cc:15
 hkalmatrixtools.cc:16
 hkalmatrixtools.cc:17
 hkalmatrixtools.cc:18
 hkalmatrixtools.cc:19
 hkalmatrixtools.cc:20
 hkalmatrixtools.cc:21
 hkalmatrixtools.cc:22
 hkalmatrixtools.cc:23
 hkalmatrixtools.cc:24
 hkalmatrixtools.cc:25
 hkalmatrixtools.cc:26
 hkalmatrixtools.cc:27
 hkalmatrixtools.cc:28
 hkalmatrixtools.cc:29
 hkalmatrixtools.cc:30
 hkalmatrixtools.cc:31
 hkalmatrixtools.cc:32
 hkalmatrixtools.cc:33
 hkalmatrixtools.cc:34
 hkalmatrixtools.cc:35
 hkalmatrixtools.cc:36
 hkalmatrixtools.cc:37
 hkalmatrixtools.cc:38
 hkalmatrixtools.cc:39
 hkalmatrixtools.cc:40
 hkalmatrixtools.cc:41
 hkalmatrixtools.cc:42
 hkalmatrixtools.cc:43
 hkalmatrixtools.cc:44
 hkalmatrixtools.cc:45
 hkalmatrixtools.cc:46
 hkalmatrixtools.cc:47
 hkalmatrixtools.cc:48
 hkalmatrixtools.cc:49
 hkalmatrixtools.cc:50
 hkalmatrixtools.cc:51
 hkalmatrixtools.cc:52
 hkalmatrixtools.cc:53
 hkalmatrixtools.cc:54
 hkalmatrixtools.cc:55
 hkalmatrixtools.cc:56
 hkalmatrixtools.cc:57
 hkalmatrixtools.cc:58
 hkalmatrixtools.cc:59
 hkalmatrixtools.cc:60
 hkalmatrixtools.cc:61
 hkalmatrixtools.cc:62
 hkalmatrixtools.cc:63
 hkalmatrixtools.cc:64
 hkalmatrixtools.cc:65
 hkalmatrixtools.cc:66
 hkalmatrixtools.cc:67
 hkalmatrixtools.cc:68
 hkalmatrixtools.cc:69
 hkalmatrixtools.cc:70
 hkalmatrixtools.cc:71
 hkalmatrixtools.cc:72
 hkalmatrixtools.cc:73
 hkalmatrixtools.cc:74
 hkalmatrixtools.cc:75
 hkalmatrixtools.cc:76
 hkalmatrixtools.cc:77
 hkalmatrixtools.cc:78
 hkalmatrixtools.cc:79
 hkalmatrixtools.cc:80
 hkalmatrixtools.cc:81
 hkalmatrixtools.cc:82
 hkalmatrixtools.cc:83
 hkalmatrixtools.cc:84
 hkalmatrixtools.cc:85
 hkalmatrixtools.cc:86
 hkalmatrixtools.cc:87
 hkalmatrixtools.cc:88
 hkalmatrixtools.cc:89
 hkalmatrixtools.cc:90
 hkalmatrixtools.cc:91
 hkalmatrixtools.cc:92
 hkalmatrixtools.cc:93
 hkalmatrixtools.cc:94
 hkalmatrixtools.cc:95
 hkalmatrixtools.cc:96
 hkalmatrixtools.cc:97
 hkalmatrixtools.cc:98
 hkalmatrixtools.cc:99
 hkalmatrixtools.cc:100
 hkalmatrixtools.cc:101
 hkalmatrixtools.cc:102
 hkalmatrixtools.cc:103
 hkalmatrixtools.cc:104
 hkalmatrixtools.cc:105
 hkalmatrixtools.cc:106
 hkalmatrixtools.cc:107
 hkalmatrixtools.cc:108
 hkalmatrixtools.cc:109
 hkalmatrixtools.cc:110
 hkalmatrixtools.cc:111
 hkalmatrixtools.cc:112
 hkalmatrixtools.cc:113
 hkalmatrixtools.cc:114
 hkalmatrixtools.cc:115
 hkalmatrixtools.cc:116
 hkalmatrixtools.cc:117
 hkalmatrixtools.cc:118
 hkalmatrixtools.cc:119
 hkalmatrixtools.cc:120
 hkalmatrixtools.cc:121
 hkalmatrixtools.cc:122
 hkalmatrixtools.cc:123
 hkalmatrixtools.cc:124
 hkalmatrixtools.cc:125
 hkalmatrixtools.cc:126
 hkalmatrixtools.cc:127
 hkalmatrixtools.cc:128
 hkalmatrixtools.cc:129
 hkalmatrixtools.cc:130
 hkalmatrixtools.cc:131
 hkalmatrixtools.cc:132
 hkalmatrixtools.cc:133
 hkalmatrixtools.cc:134
 hkalmatrixtools.cc:135
 hkalmatrixtools.cc:136
 hkalmatrixtools.cc:137
 hkalmatrixtools.cc:138
 hkalmatrixtools.cc:139
 hkalmatrixtools.cc:140
 hkalmatrixtools.cc:141
 hkalmatrixtools.cc:142
 hkalmatrixtools.cc:143
 hkalmatrixtools.cc:144
 hkalmatrixtools.cc:145
 hkalmatrixtools.cc:146
 hkalmatrixtools.cc:147
 hkalmatrixtools.cc:148
 hkalmatrixtools.cc:149
 hkalmatrixtools.cc:150
 hkalmatrixtools.cc:151
 hkalmatrixtools.cc:152
 hkalmatrixtools.cc:153
 hkalmatrixtools.cc:154
 hkalmatrixtools.cc:155
 hkalmatrixtools.cc:156
 hkalmatrixtools.cc:157
 hkalmatrixtools.cc:158
 hkalmatrixtools.cc:159
 hkalmatrixtools.cc:160
 hkalmatrixtools.cc:161
 hkalmatrixtools.cc:162
 hkalmatrixtools.cc:163
 hkalmatrixtools.cc:164
 hkalmatrixtools.cc:165
 hkalmatrixtools.cc:166
 hkalmatrixtools.cc:167
 hkalmatrixtools.cc:168
 hkalmatrixtools.cc:169
 hkalmatrixtools.cc:170
 hkalmatrixtools.cc:171
 hkalmatrixtools.cc:172
 hkalmatrixtools.cc:173
 hkalmatrixtools.cc:174
 hkalmatrixtools.cc:175
 hkalmatrixtools.cc:176
 hkalmatrixtools.cc:177
 hkalmatrixtools.cc:178
 hkalmatrixtools.cc:179
 hkalmatrixtools.cc:180
 hkalmatrixtools.cc:181
 hkalmatrixtools.cc:182
 hkalmatrixtools.cc:183
 hkalmatrixtools.cc:184
 hkalmatrixtools.cc:185
 hkalmatrixtools.cc:186
 hkalmatrixtools.cc:187
 hkalmatrixtools.cc:188
 hkalmatrixtools.cc:189
 hkalmatrixtools.cc:190
 hkalmatrixtools.cc:191
 hkalmatrixtools.cc:192
 hkalmatrixtools.cc:193
 hkalmatrixtools.cc:194
 hkalmatrixtools.cc:195
 hkalmatrixtools.cc:196
 hkalmatrixtools.cc:197
 hkalmatrixtools.cc:198
 hkalmatrixtools.cc:199
 hkalmatrixtools.cc:200
 hkalmatrixtools.cc:201
 hkalmatrixtools.cc:202
 hkalmatrixtools.cc:203
 hkalmatrixtools.cc:204
 hkalmatrixtools.cc:205
 hkalmatrixtools.cc:206
 hkalmatrixtools.cc:207
 hkalmatrixtools.cc:208
 hkalmatrixtools.cc:209
 hkalmatrixtools.cc:210
 hkalmatrixtools.cc:211
 hkalmatrixtools.cc:212
 hkalmatrixtools.cc:213
 hkalmatrixtools.cc:214
 hkalmatrixtools.cc:215
 hkalmatrixtools.cc:216
 hkalmatrixtools.cc:217
 hkalmatrixtools.cc:218
 hkalmatrixtools.cc:219
 hkalmatrixtools.cc:220
 hkalmatrixtools.cc:221
 hkalmatrixtools.cc:222
 hkalmatrixtools.cc:223
 hkalmatrixtools.cc:224
 hkalmatrixtools.cc:225
 hkalmatrixtools.cc:226
 hkalmatrixtools.cc:227
 hkalmatrixtools.cc:228
 hkalmatrixtools.cc:229
 hkalmatrixtools.cc:230
 hkalmatrixtools.cc:231
 hkalmatrixtools.cc:232
 hkalmatrixtools.cc:233
 hkalmatrixtools.cc:234
 hkalmatrixtools.cc:235
 hkalmatrixtools.cc:236
 hkalmatrixtools.cc:237
 hkalmatrixtools.cc:238
 hkalmatrixtools.cc:239
 hkalmatrixtools.cc:240
 hkalmatrixtools.cc:241
 hkalmatrixtools.cc:242
 hkalmatrixtools.cc:243
 hkalmatrixtools.cc:244
 hkalmatrixtools.cc:245
 hkalmatrixtools.cc:246
 hkalmatrixtools.cc:247
 hkalmatrixtools.cc:248
 hkalmatrixtools.cc:249
 hkalmatrixtools.cc:250
 hkalmatrixtools.cc:251
 hkalmatrixtools.cc:252
 hkalmatrixtools.cc:253
 hkalmatrixtools.cc:254
 hkalmatrixtools.cc:255
 hkalmatrixtools.cc:256