When a programmer deals with mathematics, one of the most common routines added to his/her math library is matrix inversion.
The following code implements a very efficient in-situ matrix inversion (meaning that no memory is allocated for another matrix to hold the result; the inverse of the matrix is built gradually in the matrix itself!).
The code is based on an algorithm published in Numerical Recipes.
The code can be put in a .vb file (e.g., MatrixInversion.vb).
Option Strict On
Option Explicit On
Imports System
''' <summary>
''' Return code of the in-situ matrix inversion routines.
''' </summary>
Public Enum MatrixInversionResult
Success = 0
InvalidArgument = 1
NotSquare = 2
Singular = 3
End Enum
Public Module MatrixInversion
' Machine epsilon for Double (2^-52).
Private Const Eps As Double = 2.2204460492503131E-16
''' <summary>
''' Inverts a square matrix in situ (in place) by Gauss-Jordan elimination
''' with full pivoting. On success the input array holds the inverse.
''' </summary>
''' <param name="a">
''' Square, zero-based array, e.g. Dim a(n - 1, n - 1) As Double.
''' Overwritten by the inverse. Undefined if the routine fails.
''' </param>
Public Function InvertInPlace(ByRef a(,) As Double) As MatrixInversionResult
If a Is Nothing Then Return MatrixInversionResult.InvalidArgument
Dim n As Integer = a.GetLength(0)
If a.GetLength(1) <> n Then Return MatrixInversionResult.NotSquare
If n < 1 Then Return MatrixInversionResult.InvalidArgument
Return InvertCore(a, a.GetLowerBound(0), a.GetLowerBound(1), n)
End Function
''' <summary>
''' Backwards-compatible entry point for code ported from VB6, where the
''' matrix is dimensioned (gDim, gDim) and only the subscripts 1..gDim are
''' used. Row 0 and column 0 are left untouched.
''' </summary>
Public Function MatrixInvertGaussJordan(ByRef mMatrix1(,) As Double,
ByVal gDim As Integer) As MatrixInversionResult
If mMatrix1 Is Nothing Then Return MatrixInversionResult.InvalidArgument
If gDim < 1 Then Return MatrixInversionResult.InvalidArgument
If mMatrix1.GetUpperBound(0) < gDim OrElse mMatrix1.GetUpperBound(1) < gDim Then
Return MatrixInversionResult.InvalidArgument
End If
Return InvertCore(mMatrix1, 1, 1, gDim)
End Function
''' <summary>
''' Core routine. Inverts, in situ, the n-by-n block of "a" whose first
''' element is a(r0, c0). Only O(n) extra memory is used.
''' </summary>
Private Function InvertCore(ByRef a(,) As Double,
ByVal r0 As Integer,
ByVal c0 As Integer,
ByVal n As Integer) As MatrixInversionResult
Dim indxc(n - 1) As Integer ' pivot bookkeeping, for the final unscrambling
Dim indxr(n - 1) As Integer
Dim ipiv(n - 1) As Integer ' 0 = column not yet used as pivot
Dim i, j, k, l, ll As Integer
Dim iRow, iCol As Integer
Dim big, dum, pivInv As Double
' Largest magnitude in the matrix: used for a scale-aware singularity
' test. A fixed absolute threshold such as 1.0E-15 wrongly rejects
' well-conditioned matrices whose entries are simply small.
Dim aMax As Double = 0.0
For i = 0 To n - 1
For j = 0 To n - 1
Dim v As Double = a(r0 + i, c0 + j)
If Double.IsNaN(v) OrElse Double.IsInfinity(v) Then
Return MatrixInversionResult.InvalidArgument
End If
v = Math.Abs(v)
If v > aMax Then aMax = v
Next
Next
If aMax = 0.0 Then Return MatrixInversionResult.Singular
Dim tol As Double = aMax * n * Eps
For i = 0 To n - 1
' --- search the remaining submatrix for the largest pivot -------
' big, iRow and iCol MUST be reset on every pass; otherwise a stale
' pivot from the previous pass can be reused.
big = -1.0
iRow = -1
iCol = -1
For j = 0 To n - 1
If ipiv(j) <> 1 Then
For k = 0 To n - 1
If ipiv(k) = 0 Then
Dim v As Double = Math.Abs(a(r0 + j, c0 + k))
If v > big Then
big = v
iRow = j
iCol = k
End If
ElseIf ipiv(k) > 1 Then
Return MatrixInversionResult.Singular
End If
Next
End If
Next
If iCol < 0 OrElse big <= tol Then Return MatrixInversionResult.Singular
ipiv(iCol) += 1
' --- interchange rows so that the pivot lands on the diagonal ---
If iRow <> iCol Then
For l = 0 To n - 1
dum = a(r0 + iRow, c0 + l)
a(r0 + iRow, c0 + l) = a(r0 + iCol, c0 + l)
a(r0 + iCol, c0 + l) = dum
Next
End If
indxr(i) = iRow
indxc(i) = iCol
' After the interchange the pivot is a(iCol, iCol), NOT a(iRow, iCol).
pivInv = 1.0 / a(r0 + iCol, c0 + iCol)
a(r0 + iCol, c0 + iCol) = 1.0
For l = 0 To n - 1
a(r0 + iCol, c0 + l) *= pivInv
Next
' --- reduce the remaining rows ---------------------------------
For ll = 0 To n - 1
If ll <> iCol Then
dum = a(r0 + ll, c0 + iCol)
If dum <> 0.0 Then
a(r0 + ll, c0 + iCol) = 0.0
For l = 0 To n - 1
a(r0 + ll, c0 + l) -= a(r0 + iCol, c0 + l) * dum
Next
End If
End If
Next
Next
' --- undo the column interchanges, in reverse order -----------------
For l = n - 1 To 0 Step -1
If indxr(l) <> indxc(l) Then
For k = 0 To n - 1
dum = a(r0 + k, c0 + indxr(l))
a(r0 + k, c0 + indxr(l)) = a(r0 + k, c0 + indxc(l))
a(r0 + k, c0 + indxc(l)) = dum
Next
End If
Next
Return MatrixInversionResult.Success
End Function
End Module