invert_matrix.c

Go to the documentation of this file.

#include "invert_matrix.h"
#include <assert.h>
#include <math.h>
#include <stdlib.h>

int invert_matrix(double *inv_A, double *A, int N)
{
    double *c = NULL, *d = NULL, *col_j, *col_k, *inv_col_k;
    double temp, scale, sum;
    int i, j, k, success = 0;
    
    assert(inv_A && A && N > 0);
    
    if (!(c = (double *)malloc(sizeof(double) * N))
        || !(d = (double *)malloc(sizeof(double) * N)))
        goto fail;
    
    for (k = 0, col_k = A; k < N - 1; ++k, col_k += N)
    {
        scale = 0.0;
        
        for (i = k; i < N; ++i)
            if ((temp = fabs(col_k[i])) > scale)
                scale = temp;
        
        if (scale == 0.0)
            goto fail; /* Singular matrix */
        
        for (i = k; i < N; ++i)
            col_k[i] /= scale;
        
        for (sum = 0.0, i = k; i < N; ++i)
            sum += col_k[i]*col_k[i];
        
        temp = (col_k[k] >= 0.0) ? sqrt(sum) : -sqrt(sum);
        col_k[k] += temp;
        c[k] = temp * col_k[k];
        d[k] = -scale * temp;
        
        for (j = k + 1, col_j = col_k + N; j < N; ++j, col_j += N)
        {
            for (scale = 0.0, i = k; i < N; ++i)
                scale += col_k[i] * col_j[i];
                
            scale /= c[k];
            
            for (i = k; i < N; ++i)
                col_j[i] -= scale * col_k[i];
        }
    }

    d[N-1] = col_k[k];
    
    if (d[N - 1] == 0.0)
        goto fail; /* Singular matrix */
    
    for (k = 0, inv_col_k = inv_A; k < N; ++k, inv_col_k += N)
    {
        for (i = 0; i < N; ++i)
            inv_col_k[i] = -A[k] * A[i] / c[0];
        
        inv_col_k[k] += 1.0;
        
        for (j = 1, col_j = A + N; j < N-1; ++j, col_j += N)
        {
            for (scale = 0.0, i = j; i < N; ++i)
                scale += col_j[i] * inv_col_k[i];
            
            scale /= c[j];
            
            for (i = j; i < N; ++i)
                inv_col_k[i] -= scale * col_j[i];
        }
        
        inv_col_k[j] /= d[N-1];
        
        for (i = N - 2; i >= 0; --i)
        {
            for (sum = 0.0, j = i + 1, col_j = A + N*(i + 1);
                j < N; ++j, col_j += N)
                sum += col_j[i] * inv_col_k[j];
            
            inv_col_k[i] = (inv_col_k[i] - sum) / d[i];
        }
    }
    
    success = 1; /* Finished successfully */
fail: /* Clean up */
    free(d);
    free(c);
    return success;
}