Golang FloatMatrix.SubMatrix方法代码示例

/*
 * Solve a system of linear equations A*X = B with general M-by-N
 * matrix A using the QR factorization computed by DecomposeQRT().
 *
 * If flags&TRANS != 0:
 *   find the minimum norm solution of an overdetermined system A.T * X = B.
 *   i.e min ||X|| s.t A.T*X = B
 *
 * Otherwise:
 *   find the least squares solution of an overdetermined system, i.e.,
 *   solve the least squares problem: min || B - A*X ||.
 *
 * Arguments:
 *  B     On entry, the right hand side N-by-P matrix B. On exit, the solution matrix X.
 *
 *  A     The elements on and above the diagonal contain the min(M,N)-by-N upper
 *        trapezoidal matrix R. The elements below the diagonal with the matrix 'T',
 *        represent the ortogonal matrix Q as product of elementary reflectors.
 *        Matrix A and T are as returned by DecomposeQRT()
 *
 *  T     The N-by-N block reflector which, together with trilu(A) represent
 *        the ortogonal matrix Q as Q = I - Y*T*Y.T where Y = trilu(A).
 *
 *  W     Workspace, P-by-nb matrix used for work space in blocked invocations.
 *
 *  flags Indicator flag
 *
 *  nb    The block size used in blocked invocations. If nb is zero default
 *        value N is used.
 *
 * Compatible with lapack.GELS (the m >= n part)
 */
func SolveQRT(B, A, T, W *matrix.FloatMatrix, flags Flags, nb int) error {
	var err error = nil
	var R, BT matrix.FloatMatrix
	if flags&TRANS != 0 {
		// Solve overdetermined system A.T*X = B

		// B' = R.-1*B
		A.SubMatrix(&R, 0, 0, A.Cols(), A.Cols())
		B.SubMatrix(&BT, 0, 0, A.Cols(), B.Cols())
		err = SolveTrm(&BT, &R, 1.0, LEFT|UPPER|TRANSA)

		// Clear bottom part of B
		B.SubMatrix(&BT, A.Cols(), 0)
		BT.SetIndexes(0.0)

		// X = Q*B'
		err = MultQT(B, A, T, W, LEFT, nb)
	} else {
		// solve least square problem min ||A*X - B||

		// B' = Q.T*B
		err = MultQT(B, A, T, W, LEFT|TRANS, nb)
		if err != nil {
			return err
		}

		// X = R.-1*B'
		A.SubMatrix(&R, 0, 0, A.Cols(), A.Cols())
		B.SubMatrix(&BT, 0, 0, A.Cols(), B.Cols())
		err = SolveTrm(&BT, &R, 1.0, LEFT|UPPER)
	}
	return err
}

func blockedBuildQ(A, tau, W *matrix.FloatMatrix, nb int) error {
	var err error = nil
	var ATL, ATR, ABL, ABR, AL matrix.FloatMatrix
	var A00, A01, A02, A10, A11, A12, A20, A21, A22 matrix.FloatMatrix
	var tT, tB matrix.FloatMatrix
	var t0, tau1, t2, Tw, Wrk matrix.FloatMatrix
	var mb int

	mb = A.Rows() - A.Cols()
	Twork := matrix.FloatZeros(nb, nb)

	partition2x2(
		&ATL, &ATR,
		&ABL, &ABR, A, mb, 0, pBOTTOMRIGHT)
	partition2x1(
		&tT,
		&tB, tau, 0, pBOTTOM)

	// 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, &A02,
			&A10, &A11, &A12,
			&A20, &A21, &A22, A, nb, pTOPLEFT)
		repartition2x1to3x1(&tT,
			&t0,
			&tau1,
			&t2, tau, nb, pTOP)

		// --------------------------------------------------------

		// build block reflector from current block
		merge2x1(&AL, &A11, &A21)
		Twork.SubMatrix(&Tw, 0, 0, A11.Cols(), A11.Cols())
		unblkQRBlockReflector(&Tw, &AL, &tau1)

		// 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, &Tw, &Wrk, nb, false)

		// use unblocked version to compute current block
		W.SubMatrix(&Wrk, 0, 0, 1, A11.Cols())
		unblockedBuildQ(&AL, &tau1, &Wrk, 0)

		// zero upper part
		A01.SetIndexes(0.0)

		// --------------------------------------------------------
		continue3x3to2x2(
			&ATL, &ATR,
			&ABL, &ABR, &A00, &A11, &A22, A, pTOPLEFT)
		continue3x1to2x1(
			&tT,
			&tB, &t0, &tau1, tau, pTOP)
	}
	return err
}

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
}

func swapCols(A *matrix.FloatMatrix, src, dst int) {
	var c0, c1 matrix.FloatMatrix
	if src == dst || A.Rows() == 0 {
		return
	}
	A.SubMatrix(&c0, 0, src, A.Rows(), 1)
	A.SubMatrix(&c1, 0, dst, A.Rows(), 1)
	Swap(&c0, &c1)
}

func swapRows(A *matrix.FloatMatrix, src, dst int) {
	var r0, r1 matrix.FloatMatrix
	if src == dst || A.Rows() == 0 {
		return
	}
	A.SubMatrix(&r0, src, 0, 1, A.Cols())
	A.SubMatrix(&r1, dst, 0, 1, A.Cols())
	Swap(&r0, &r1)
}

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
}

/*
 * Compute
 *   C = C*diag(D)      flags & RIGHT == true
 *   C = diag(D)*C      flags & LEFT  == true
 *
 * Arguments
 *   C     M-by-N matrix if flags&RIGHT == true or N-by-M matrix if flags&LEFT == true
 *
 *   D     N element column or row vector or N-by-N matrix
 *
 *   flags Indicator bits, LEFT or RIGHT
 */
func MultDiag(C, D *matrix.FloatMatrix, flags Flags) {
	var c, d0 matrix.FloatMatrix
	if D.Cols() == 1 {
		// diagonal is column vector
		switch flags & (LEFT | RIGHT) {
		case LEFT:
			// scale rows; for each column element-wise multiply with D-vector
			for k := 0; k < C.Cols(); k++ {
				C.SubMatrix(&c, 0, k, C.Rows(), 1)
				c.Mul(D)
			}
		case RIGHT:
			// scale columns
			for k := 0; k < C.Cols(); k++ {
				C.SubMatrix(&c, 0, k, C.Rows(), 1)
				// scale the column
				c.Scale(D.GetAt(k, 0))
			}
		}
	} else {
		// diagonal is row vector
		var d *matrix.FloatMatrix
		if D.Rows() == 1 {
			d = D
		} else {
			D.SubMatrix(&d0, 0, 0, 1, D.Cols(), D.LeadingIndex()+1)
			d = &d0
		}
		switch flags & (LEFT | RIGHT) {
		case LEFT:
			for k := 0; k < C.Rows(); k++ {
				C.SubMatrix(&c, k, 0, 1, C.Cols())
				// scale the row
				c.Scale(d.GetAt(0, k))
			}
		case RIGHT:
			// scale columns
			for k := 0; k < C.Cols(); k++ {
				C.SubMatrix(&c, 0, k, C.Rows(), 1)
				// scale the column
				c.Scale(d.GetAt(0, k))
			}
		}
	}
}

/*
 Continue with 2 by 1 block from 3 by 1 block.

           AT      A0            AT       A0
 pBOTTOM: --  <--  A1   ; pTOP:   -- <--  --
           AB      --            AB       A1
                   A2                     A2

*/
func continue3x1to2x1(AT, AB, A0, A1, A *matrix.FloatMatrix, pdir pDirection) {
	n0 := A0.Rows()
	n1 := A1.Rows()
	switch pdir {
	case pBOTTOM:
		A.SubMatrix(AT, 0, 0, n0+n1, A.Cols())
		A.SubMatrix(AB, n0+n1, 0, A.Rows()-n0-n1, A.Cols())
	case pTOP:
		A.SubMatrix(AT, 0, 0, n0, A.Cols())
		A.SubMatrix(AB, n0, 0, A.Rows()-n0, A.Cols())
	}
}

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
}

/*
 Partition p to 2 by 1 blocks.

        AT
  A --> --
        AB

 Parameter nb is initial block size for AT (pTOP) or AB (pBOTTOM).
*/
func partition2x1(AT, AB, A *matrix.FloatMatrix, nb int, side pDirection) {
	if nb > A.Rows() {
		nb = A.Rows()
	}
	switch side {
	case pTOP:
		A.SubMatrix(AT, 0, 0, nb, A.Cols())
		A.SubMatrix(AB, nb, 0, A.Rows()-nb, A.Cols())
	case pBOTTOM:
		A.SubMatrix(AT, 0, 0, A.Rows()-nb, A.Cols())
		A.SubMatrix(AB, A.Rows()-nb, 0, nb, A.Cols())
	}
}

/*
 Partition A to 1 by 2 blocks.

  A -->  AL | AR

 Parameter nb is initial block size for AL (pLEFT) or AR (pRIGHT).
*/
func partition1x2(AL, AR, A *matrix.FloatMatrix, nb int, side pDirection) {
	if nb > A.Cols() {
		nb = A.Cols()
	}
	switch side {
	case pLEFT:
		A.SubMatrix(AL, 0, 0, A.Rows(), nb)
		A.SubMatrix(AR, 0, nb, A.Rows(), A.Cols()-nb)
	case pRIGHT:
		A.SubMatrix(AL, 0, 0, A.Rows(), A.Cols()-nb)
		A.SubMatrix(AR, 0, A.Cols()-nb, A.Rows(), nb)
	}
}

func col(A *matrix.FloatMatrix, inds ...int) *matrix.FloatMatrix {
	var c matrix.FloatMatrix
	switch len(inds) {
	case 0:
		A.SubMatrix(&c, 0, 0, A.Rows(), 1)
	case 1:
		A.SubMatrix(&c, inds[0], 0, A.Rows(), 1)
	case 2:
		A.SubMatrix(&c, inds[0], 0, inds[1], 1)
	default:
		A.SubMatrix(&c, inds[0], inds[1], inds[2], 1)
	}
	return &c
}

func row(A *matrix.FloatMatrix, inds ...int) *matrix.FloatMatrix {
	var r matrix.FloatMatrix
	switch len(inds) {
	case 0:
		A.SubMatrix(&r, 0, 0, 1, A.Cols())
	case 1:
		A.SubMatrix(&r, inds[0], 0, 1, A.Cols())
	case 2:
		A.SubMatrix(&r, inds[0], 0, 1, inds[1])
	default:
		A.SubMatrix(&r, inds[0], inds[1], 1, inds[2])
	}
	return &r
}

/*
 Repartition 1 by 2 blocks to 1 by 3 blocks.

 pRIGHT: AL | AR  --  A0 A1 | A2
 pLEFT:  AL | AR  <--  A0 | A1 A2

*/
func continue1x3to1x2(AL, AR, A0, A1, A *matrix.FloatMatrix, pdir pDirection) {

	k := A0.Cols()
	nb := A1.Cols()
	switch pdir {
	case pRIGHT:
		// AL is [A0; A1], AR is A2
		A.SubMatrix(AL, 0, 0, A.Rows(), k+nb)
		A.SubMatrix(AR, 0, AL.Cols(), A.Rows(), A.Cols()-AL.Cols())
	case pLEFT:
		// AL is A0; AR is [A1; A2]
		A.SubMatrix(AL, 0, 0, A.Rows(), k)
		A.SubMatrix(AR, 0, k, A.Rows(), A.Cols()-k)
	}
}

/*
 * Apply diagonal pivot (row and column swapped) to symmetric matrix blocks.
 *
 * LOWER triangular; moving from top-left to bottom-right
 *
 *    -----------------------
 *    | d
 *    | x P1 x  x  x  P2     -- current row/col 'srcix'
 *    | x S2 d  x  x  x
 *    | x S2 x  d  x  x
 *    | x S2 x  x  d  x
 *    | x P2 D2 D2 D2 P3     -- swap with row/col 'dstix'
 *    | x S3 x  x  x  D3 d
 *    | x S3 x  x  x  D3 x d
 *         (AR)
 *
 * UPPER triangular; moving from bottom-right to top-left
 *
 *    d x D3 x  x  x  S3 x |
 *      d D3 x  x  x  S3 x |
 *        P3 D2 D2 D2 P2 x |  -- dstinx
 *           d  x  x  S2 x |
 *              d  x  S2 x |
 *                 d  S2 x |
 *                    P1 x |  -- srcinx
 *                       d |
 *    ----------------------
 *               (ABR)
 */
func applyBKPivotSym(AR *matrix.FloatMatrix, srcix, dstix int, flags Flags) {
	var s, d matrix.FloatMatrix
	if flags&LOWER != 0 {
		// S2 -- D2
		AR.SubMatrix(&s, srcix+1, srcix, dstix-srcix-1, 1)
		AR.SubMatrix(&d, dstix, srcix+1, 1, dstix-srcix-1)
		Swap(&s, &d)
		// S3 -- D3
		AR.SubMatrix(&s, dstix+1, srcix, AR.Rows()-dstix-1, 1)
		AR.SubMatrix(&d, dstix+1, dstix, AR.Rows()-dstix-1, 1)
		Swap(&s, &d)
		// swap P1 and P3
		p1 := AR.GetAt(srcix, srcix)
		p3 := AR.GetAt(dstix, dstix)
		AR.SetAt(srcix, srcix, p3)
		AR.SetAt(dstix, dstix, p1)
		return
	}
	if flags&UPPER != 0 {
		// AL is ATL, AR is ATR; P1 is AL[srcix, srcix];
		// S2 -- D2
		AR.SubMatrix(&s, dstix+1, srcix, srcix-dstix-1, 1)
		AR.SubMatrix(&d, dstix, dstix+1, 1, srcix-dstix-1)
		Swap(&s, &d)
		// S3 -- D3
		AR.SubMatrix(&s, 0, srcix, dstix, 1)
		AR.SubMatrix(&d, 0, dstix, dstix, 1)
		Swap(&s, &d)
		//fmt.Printf("3, AR=%v\n", AR)
		// swap P1 and P3
		p1 := AR.GetAt(srcix, srcix)
		p3 := AR.GetAt(dstix, dstix)
		AR.SetAt(srcix, srcix, p3)
		AR.SetAt(dstix, dstix, p1)
		return
	}
}