inverse.kt

fun inverse(matrix: Matrix): Matrix? {
  require(matrix.rows == matrix.cols)

  // check matrix invertible
  if (determinant(matrix).equalsDelta(0.0)) return null

  // copy matrix
  val inverse = Matrix.diagonal(matrix.rows, 1.0, 0.0)
  val temp = Matrix(
    matrix.rows, 
    matrix.cols, 
    matrix.elements.map { it.toDouble() }
  )
  for (r in 0 until matrix.rows) {
    for (c in 0 until matrix.cols) temp[r,c] = matrix[r,c]
  }

  for (fdRow in 0 until matrix.rows) {
    // find focus-diagonal
    while (fdCol < matrix.cols 
      && temp[fdRow, fdCol].equalsDelta(0.0)) fdCol++

    // matrix not invertible if all row is zero
    if (fdCol == matrix.cols) return null

    // multiply with 1/fd
    val scalingFactor = 1 / temp[fdRow, fdCol]
    for (c in 0 until matrix.cols) {
      temp[fdRow, c] *= scalingFactor
      inverse[fdRow, c] *= scalingFactor
    }

    // subtract with k*R
    for (r in 0 until matrix.rows) {
      if (r == fdRow) continue
      val subFactor = temp[r, fdCol]
      for (c in 0 until matrix.cols) {
        temp[r, c] -= subFactor * temp[fdRow, c]
        inverse[r, c] -= subFactor * inverse[fdRow, c]
      }
    }
  }

  return inverse
}