仕事用に書いた ガウスの消去法実装の抜粋です。より高速であったり高精度であったりする実装は他にあるでしょうが、お目汚しご容赦ください。
特徴は右辺に行列を使えるところでしょうか。右辺に単位行列を入れておくと逆行列が得られます。
ソースコード
まず前向き消去です。
// Performs Gauss-Jordan elimination to an augmented matrix [A|B] and
// transforms it to a reduced row echelon form.
float eliminate_forward(const size_t n, float a[restrict n][n],
const size_t n2, float b[restrict n][n2], size_t p[restrict n])
{
// Minimum absolute pivot value.
float min_abs_pivot = FLT_MAX;
// For each row:
for (size_t i = 0; i != n; i += 1) {
// Find the pivot.
size_t pivot_row = i;
float abs_pivot = fabsf(a[p[i]][i]);
for (size_t j = i + 1; j != n; j += 1) {
float abs_a_j_i = fabsf(a[p[j]][i]);
if (abs_a_j_i > abs_pivot) {
pivot_row = j;
abs_pivot = abs_a_j_i;
}
}
// Keep the minimum absolute pivot value.
if (min_abs_pivot > abs_pivot) {
min_abs_pivot = abs_pivot;
}
// Swap two rows so that the pivot is as large as possible.
if (i != pivot_row) {
const size_t w = p[i];
p[i] = p[pivot_row];
p[pivot_row] = w;
}
// Perform reduction.
const float a_i_i = a[p[i]][i];
for (size_t k = i; k != n; k += 1) {
a[p[i]][k] /= a_i_i;
}
for (size_t k = 0; k != n2; k += 1) {
b[p[i]][k] /= a_i_i;
}
// Perform elimination.
for (size_t j = i + 1; j != n; j += 1) {
const float a_j_i = a[p[j]][i];
for (size_t k = i; k != n; k += 1) {
a[p[j]][k] -= a_j_i * a[p[i]][k];
}
for (size_t k = 0; k != n2; k += 1) {
b[p[j]][k] -= a_j_i * b[p[i]][k];
}
}
}
return min_abs_pivot;
}
次に後ろ向き消去です。
// Performs the second part of Gaussian elimination to an augmented matrix
// [A|B} in a reduced row echelon form.
void eliminate_backward(const size_t n, float a[restrict n][n],
const size_t n2, float b[restrict n][n2], const size_t p[restrict n])
{
// For each row in the reverse order:
for (size_t i = n; i != 0; i -= 1) {
// Perform elimination.
for (size_t j = i - 1; j != 0; j -= 1) {
const float a_j_i = a[p[j - 1]][i - 1];
for (size_t k = j; k != i; k += 1) {
a[p[j - 1]][k] -= a_j_i * a[p[i - 1]][k];
}
for (size_t k = 0; k != n2; k += 1) {
b[p[j - 1]][k] -= a_j_i * b[p[i - 1]][k];
}
}
}
}
最後に合わせます。
// Performs Gaussian elimination to an augmented matrix [A|B].
float eliminate(const size_t n, float a[restrict n][n],
const size_t n2, float b[restrict n][n2], size_t p[restrict n])
{
float min_abs_pivot = eliminate_forward(n, a, n2, b, p);
eliminate_backward(n, a, n2, b, p);
return min_abs_pivot;
}