func blkUpperLDL(A, W *matrix.FloatMatrix, p *pPivots, nb int) (err error) {
var ATL, ATR, ABL, ABR matrix.FloatMatrix
var A00, A01, A02, A11, A12, A22 matrix.FloatMatrix
var D1, wrk matrix.FloatMatrix
var pT, pB, p0, p1, p2 pPivots
err = nil
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pBOTTOMRIGHT)
partitionPivot2x1(
&pT,
&pB, p, 0, pBOTTOM)
for ATL.Rows() > 0 {
repartition2x2to3x3(&ATL,
&A00, &A01, &A02,
nil, &A11, &A12,
nil, nil, &A22, A, nb, pTOPLEFT)
repartPivot2x1to3x1(&pT,
&p0, &p1, &p2 /**/, p, nb, pTOP)
// --------------------------------------------------------
// A11 = LDL(A11)
err = unblkUpperLDL(&A11, &p1)
if err != nil {
return
}
applyColPivots(&A01, &p1, 0, BACKWARD)
applyRowPivots(&A12, &p1, 0, BACKWARD)
scalePivots(&p1, ATL.Rows()-A11.Rows())
A11.Diag(&D1)
// A01 = A01*A11.-T
SolveTrm(&A01, &A11, 1.0, UPPER|UNIT|RIGHT|TRANSA)
// A01 = A01*D1.-1
SolveDiag(&A01, &D1, RIGHT)
// W = D1*U01.T = U01*D1
W.SubMatrix(&wrk, 0, 0, A01.Rows(), nb)
A01.CopyTo(&wrk)
MultDiag(&wrk, &D1, RIGHT)
// A00 = A00 - U01*D1*U01.T = A22 - U01*W.T
UpdateTrm(&A00, &A01, &wrk, -1.0, 1.0, UPPER|TRANSB)
// ---------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &A11, &A22, A, pTOPLEFT)
contPivot3x1to2x1(
&pT,
&pB, &p0, &p1, p, pTOP)
}
return
}
/*
* ( A11 a12 ) ( U11 u12 )( D1 0 )( U11.t 0 )
* ( a21 a22 ) ( 0 1 )( 0 d2 )( u12.t 1 )
*
* a22 = d2
* a01 = u12*d2 => u12 = a12/d2
* A11 = u12*d2*u12.t + U11*D1*U11.t => U11 = A11 - u12*d2*u12.t
*/
func unblkUpperLDL(A *matrix.FloatMatrix, p *pPivots) (err error) {
var ATL, ATR, ABL, ABR matrix.FloatMatrix
var A00, a01, A02, a11, a12, A22 matrix.FloatMatrix
var AL, AR, acol matrix.FloatMatrix
var pT, pB, p0, p1, p2 pPivots
err = nil
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pBOTTOMRIGHT)
partitionPivot2x1(
&pT,
&pB, p, 0, pBOTTOM)
for ATL.Rows() > 0 {
repartition2x2to3x3(&ATL,
&A00, &a01, &A02,
nil, &a11, &a12,
nil, nil, &A22, A, 1, pTOPLEFT)
repartPivot2x1to3x1(&pT,
&p0, &p1, &p2 /**/, p, 1, pTOP)
// --------------------------------------------------------
// search diagonal; diag(A00;a11)
ATL.Diag(&acol)
//merge2x1(&acol, &a01, &a11)
imax := IAMax(&acol)
if imax < ATL.Rows()-1 {
merge1x2(&AL, &ATL, &ATR)
merge1x2(&AR, &a11, &a12)
// pivot diagonal in symmetric matrix; will swap a11 and [imax,imax]
applyPivotSym(&AL, &AR, imax, UPPER)
p1.pivots[0] = imax + 1
} else {
p1.pivots[0] = 0
}
if a11.Float() == 0.0 {
err = onError("zero on diagonal.")
return
}
// A00 = A00 - u01*d11*u01.T = A00 - a01*a01.T/a11; triangular update
err = MVUpdateTrm(&A00, &a01, &a01, -1.0/a11.Float(), UPPER)
// u01 = a01/a11
InvScale(&a01, a11.Float())
// ---------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &a11, &A22, A, pTOPLEFT)
contPivot3x1to2x1(
&pT,
&pB, &p0, &p1, p, pTOP)
}
return
}
func blockedBuildQT(A, T, W *matrix.FloatMatrix, nb int) error {
var err error = nil
var ATL, ATR, ABL, ABR, AL matrix.FloatMatrix
var A00, A01, A11, A12, A21, A22 matrix.FloatMatrix
var TTL, TTR, TBL, TBR matrix.FloatMatrix
var T00, T01, T02, T11, T12, T22 matrix.FloatMatrix
var tau1, Wrk matrix.FloatMatrix
var mb int
mb = A.Rows() - A.Cols()
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, mb, 0, pBOTTOMRIGHT)
partition2x2(
&TTL, &TTR,
&TBL, &TBR, T, 0, 0, pBOTTOMRIGHT)
// clearing of the columns of the right and setting ABR to unit diagonal
// (only if not applying all reflectors, kb > 0)
for ATL.Rows() > 0 && ATL.Cols() > 0 {
repartition2x2to3x3(&ATL,
&A00, &A01, nil,
nil, &A11, &A12,
nil, &A21, &A22, A, nb, pTOPLEFT)
repartition2x2to3x3(&TTL,
&T00, &T01, &T02,
nil, &T11, &T12,
nil, nil, &T22, T, nb, pTOPLEFT)
// --------------------------------------------------------
// update with current block reflector (I - Y*T*Y.T)*Atrailing
W.SubMatrix(&Wrk, 0, 0, A12.Cols(), A11.Cols())
updateWithQT(&A12, &A22, &A11, &A21, &T11, &Wrk, nb, false)
// use unblocked version to compute current block
W.SubMatrix(&Wrk, 0, 0, 1, A11.Cols())
// elementary scalar coefficients on the diagonal, column vector
T11.Diag(&tau1)
merge2x1(&AL, &A11, &A21)
// do an unblocked update to current block
unblockedBuildQ(&AL, &tau1, &Wrk, 0)
// zero upper part
A01.SetIndexes(0.0)
// --------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &A11, &A22, A, pTOPLEFT)
continue3x3to2x2(
&TTL, &TTR,
&TBL, &TBR, &T00, &T11, &T22, T, pTOPLEFT)
}
return err
}
func blkUpperLDLnoPiv(A, W *matrix.FloatMatrix, nb int) (err error) {
var ATL, ATR, ABL, ABR matrix.FloatMatrix
var A00, A01, A02, A11, A12, A22 matrix.FloatMatrix
var D1, wrk matrix.FloatMatrix
err = nil
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pBOTTOMRIGHT)
for ATL.Rows() > 0 {
repartition2x2to3x3(&ATL,
&A00, &A01, &A02,
nil, &A11, &A12,
nil, nil, &A22, A, nb, pTOPLEFT)
// --------------------------------------------------------
// A11 = LDL(A11)
unblkUpperLDLnoPiv(&A11)
A11.Diag(&D1)
// A01 = A01*A11.-T
SolveTrm(&A01, &A11, 1.0, UPPER|UNIT|RIGHT|TRANSA)
// A01 = A01*D1.-1
SolveDiag(&A01, &D1, RIGHT)
// W = D1*U01.T = U01*D1
W.SubMatrix(&wrk, 0, 0, A01.Rows(), nb)
A01.CopyTo(&wrk)
MultDiag(&wrk, &D1, RIGHT)
// A00 = A00 - U01*D1*U01.T = A22 - U01*W.T
UpdateTrm(&A00, &A01, &wrk, -1.0, 1.0, UPPER|TRANSB)
// ---------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &A11, &A22, A, pTOPLEFT)
}
return
}
/*
* Generic rank update of diagonal matrix.
* diag(D) = diag(D) + alpha * x * y.T
*
* Arguments:
* D N element column or row vector or N-by-N matrix
*
* x, y N element vectors
*
* alpha scalar
*/
func MVUpdateDiag(D, x, y *matrix.FloatMatrix, alpha float64) error {
var d *matrix.FloatMatrix
var dvec matrix.FloatMatrix
if !isVector(x) || !isVector(y) {
return errors.New("x, y not vectors")
}
if D.Rows() > 0 && D.Cols() == D.Rows() {
D.Diag(&dvec)
d = &dvec
} else if isVector(D) {
d = D
} else {
return errors.New("D not a diagonal")
}
N := d.NumElements()
for k := 0; k < N; k++ {
val := d.GetIndex(k)
val += x.GetIndex(k) * y.GetIndex(k) * alpha
d.SetIndex(k, val)
}
return nil
}
/*
* ( a11 a12 ) ( 1 0 )( d1 0 )( l l21.t )
* ( a21 A22 ) ( l21 L22 )( 0 A22 )( 0 L22.t )
*
* a11 = d1
* a21 = l21*d1 => l21 = a21/d1
* A22 = l21*d1*l21.t + L22*D2*L22.t => L22 = A22 - l21*d1*l21t
*/
func unblkLowerLDL(A *matrix.FloatMatrix, p *pPivots) (err error) {
var ATL, ATR, ABL, ABR matrix.FloatMatrix
var A00, a10, a11, A20, a21, A22, acol matrix.FloatMatrix
var pT, pB, p0, p1, p2 pPivots
err = nil
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pTOPLEFT)
partitionPivot2x1(
&pT,
&pB, p, 0, pTOP)
for ATL.Rows() < A.Rows() {
repartition2x2to3x3(&ATL,
&A00, nil, nil,
&a10, &a11, nil,
&A20, &a21, &A22, A, 1, pBOTTOMRIGHT)
repartPivot2x1to3x1(&pT,
&p0, &p1, &p2 /**/, p, 1, pBOTTOM)
// --------------------------------------------------------
ABR.Diag(&acol)
//merge2x1(&acol, &a11, &a21)
imax := IAMax(&acol)
//fmt.Printf("imax=%d, val=%e\n", imax, acol.GetAt(0, imax))
if imax > 0 {
// pivot diagonal in symmetric matrix; will swap a11 and [imax,imax]
applyPivotSym(&ABL, &ABR, imax, LOWER)
p1.pivots[0] = imax + ATL.Rows() + 1
} else {
p1.pivots[0] = 0
}
if a11.Float() == 0.0 {
err = onError("zero value on diagonal")
return
}
//fmt.Printf("unblk pivoted %d, a11=%e, A:\n%v\n", imax, a11.Float(), A)
//var Ablk matrix.FloatMatrix
//merge1x2(&Ablk, &a21, &A22)
//fmt.Printf("unblk update with a11=%e, a21|A22:\n%v\n", a11.Float(), &Ablk)
// A22 = A22 - l21*d11*l21.T = A22 - a21*a21.T/a11; triangular update
err = MVUpdateTrm(&A22, &a21, &a21, -1.0/a11.Float(), LOWER)
// l21 = a21/a11
InvScale(&a21, a11.Float())
//merge1x2(&Ablk, &ABL, &ABR)
//fmt.Printf("unblk imax=%d, Ablk:\n%v\n", imax, &Ablk)
// ---------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &a11, &A22, A, pBOTTOMRIGHT)
contPivot3x1to2x1(
&pT,
&pB, &p0, &p1, p, pBOTTOM)
}
return
}
func unblkBoundedLowerLDL(A, W *matrix.FloatMatrix, p *pPivots, ncol int) (error, int) {
var ATL, ATR, ABL, ABR matrix.FloatMatrix
var A00, a10, a11, A20, a21, A22, adiag, wcol matrix.FloatMatrix
var w00, w10, w11 matrix.FloatMatrix
var pT, pB, p0, p1, p2 pPivots
var err error = nil
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pTOPLEFT)
partitionPivot2x1(
&pT,
&pB, p, 0, pTOP)
// copy current diagonal to last column of workspace
W.SubMatrix(&wcol, 0, W.Cols()-1, A.Rows(), 1)
A.Diag(&adiag)
adiag.CopyTo(&wcol)
//fmt.Printf("initial diagonal:\n%v\n", &wcol)
nc := 0
for ABR.Cols() > 0 && nc < ncol {
partition2x2(
&w00, nil,
&w10, &w11, W, nc, nc, pTOPLEFT)
dmax := findAndBuildPivot(&ABL, &ABR, &w10, &w11, nc)
//fmt.Printf("dmax=%d\n", dmax)
if dmax > 0 {
// pivot diagonal in symmetric matrix; will swap a11 [0,0] and [imax,imax]
applyPivotSym(&ABL, &ABR, dmax, LOWER)
swapRows(&w10, 0, dmax)
pB.pivots[0] = dmax + ATL.Rows() + 1
} else {
pB.pivots[0] = 0
}
//fmt.Printf("blk pivoted %d, A:\n%v\nW:\n%v\n", dmax, A, W)
repartition2x2to3x3(&ATL,
&A00, nil, nil,
&a10, &a11, nil,
&A20, &a21, &A22, A, 1, pBOTTOMRIGHT)
repartPivot2x1to3x1(&pT,
&p0, &p1, &p2 /**/, p, 1, pBOTTOM)
// --------------------------------------------------------
// Copy updated column from working space
w11.SubMatrix(&wcol, 1, 0, a21.Rows(), 1)
wcol.CopyTo(&a21)
a11.SetAt(0, 0, w11.GetAt(0, 0))
// l21 = a21/a11
InvScale(&a21, a11.Float())
// here: wcol == l21*d11 == a21
if ncol-nc > 1 {
// update diagonal in workspace if not last column of block
w11.SubMatrix(&adiag, 1, w11.Cols()-1, a21.Rows(), 1)
MVUpdateDiag(&adiag, &wcol, &wcol, -1.0/a11.Float())
}
//fmt.Printf("nc=%d, a11=%e\n", nc, a11.Float())
//fmt.Printf("l21\n%v\n", &a21)
//fmt.Printf("a21\n%v\n", &wcol)
//fmt.Printf("diag\n%v\n", &adiag)
//var Ablk, wblk matrix.FloatMatrix
//merge1x2(&Ablk, &ABL, &ABR)
//merge1x2(&wblk, &w10, &w11)
//fmt.Printf("unblk Ablk:\n%v\n", &Ablk)
//fmt.Printf("unblk wblk:\n%v\n", &wblk)
// ---------------------------------------------------------
nc++
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &a11, &A22, A, pBOTTOMRIGHT)
contPivot3x1to2x1(
&pT,
&pB, &p0, &p1, p, pBOTTOM)
}
return err, nc
}
