Double Gram-Schmidt inversion

gs.c

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/* Get A_{ij} */
#define ENT(A, n, i, j) ((A)[(n)*(i) + (j)])


static void
print_matrix(double *m, int n)
{
    int i, j;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++)
            printf("%f |", ENT(m, n, i, j));
        printf("\n");
    }
    printf("\n");
}


static void
matrix_transpose(int n, double *m)
{
    double tmp;
    int i, j;
    for (i = 0; i < n; i++) {
        for (j = 0; j < i; j++) {
            tmp = ENT(m, n, i, j);
            ENT(m, n, i, j) = ENT(m, n, j, i);
            ENT(m, n, j, i) = tmp;
        }
    }
}

static void
matrix_mult(int n, double *a, double *b, double *res)
{
    int i, j, k;
    double cross;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cross = 0;
            for (k = 0; k < n; k++)
                cross += ENT(a, n, i, k)*ENT(b, n, k, j);
            ENT(res, n, i, j) = cross;
        }
    }
}

static double
row_magnitude(double *a, int n, int i)
{
    int j;
    double res = 0;
    for (j = 0; j < n; j++)
        res += pow(ENT(a, n, i, j), 2);
    return sqrt(res);
}

static void
swap_rows(double *a, int n, int i, int k)
{
    int j;
    double tmp;
    for (j = 0; j < n; j++) {
        tmp = ENT(a, n, i, j);
        ENT(a, n, i, j) = ENT(a, n, k, j);
        ENT(a, n, k, j) = tmp;
    }
}

void
inv_double_gs(double *a, int n, double *u, double *b)
{
    int i, j, k, winner;
    double magnitude, winner_magnitude, cross;
    double *tmp;

    /* Initialize u. */
    memcpy(u, a, sizeof(double) * n * n);
    /* Set b to the identity matrix. */
    for (i = 0; i < n; i++)
        for (j = 0; j < n; j++)
            ENT(b, n, i, j) = i == j;

    /* Perform double Gram-Schmidt. */
    for (i = 0; i < n; i++) {
        /* Pivot */
        winner = i;
        winner_magnitude = 0;
        for (k = i; k < n; k++) {
            magnitude = row_magnitude(a, n, k);
            if (magnitude > winner_magnitude) {
                winner_magnitude = magnitude;
                winner = k;
            }
        }
        if (i != winner) {
            swap_rows(u, n, i, winner);
            swap_rows(b, n, i, winner);
        }
        for (k = 0; k < i; k++) {
            /* Orthogonalize to the kth row of u. */
            cross = 0;
            for (j = 0; j < n; j++)
                cross += ENT(u, n, i, j) * ENT(u, n, k, j);
            for (j = 0; j < n; j++) {
                ENT(u, n, i, j) -= cross * ENT(u, n, k, j);
                ENT(b, n, i, j) -= cross * ENT(b, n, k, j);
            }
        }
        /* Normalize magnitude. */
        magnitude = row_magnitude(u, n, i);
        /* Could be singular. */
        if (!magnitude)
            continue;
        for (j = 0; j < n; j++) {
            ENT(u, n, i, j) /= magnitude;
            ENT(b, n, i, j) /= magnitude;
        }
        /* Orthogonalize all the rows after i to i. */
        for (k = i + 1; k < n; k++) {
            cross = 0;
            for (j = 0; j < n; j++)
                cross += ENT(u, n, k, j) * ENT(u, n, i, j);
            for (j = 0; j < n; j++) {
                ENT(u, n, k, j) -= cross * ENT(u, n, i, j);
                ENT(b, n, k, j) -= cross * ENT(b, n, i, j);
            }
        }
    }
    /* Compute inverse. */
    matrix_transpose(n, u);
    tmp = malloc(sizeof(double) * n * n);
    memcpy(tmp, b, sizeof(double) * n * n);
    matrix_mult(n, u, tmp, b);
    free(tmp);
    matrix_transpose(n, u);
}

int
main(int argc, char **argv)
{
    const int P = 15;
    double a[P*P], b[P*P], c[P*P], d[P*P];
    int i, j;
    srandom(getpid());
    for (i = 0; i < P; i++)
        for (j = 0; j < P; j++)
            ENT(a, P, i, j) = random() % 5000;
    a[1] = 1;
    inv_double_gs(a, P, b, c);
    print_matrix(a, P);
    print_matrix(b, P);
    print_matrix(c, P);
    matrix_mult(P, c, a, d);
    print_matrix(d, P);
    return 0;
}