Geografic-Information-System/vector/v.generalize/matrix.c

/****************************************************************
 *
 * MODULE:     v.generalize
 *
 * AUTHOR(S):  Daniel Bundala
 *
 * PURPOSE:    Definition of a matrix and basic operations with
 *             matrices
 *
 * COPYRIGHT:  (C) 2002-2005 by the GRASS Development Team
 *
 *             This program is free software under the
 *             GNU General Public License (>=v2).
 *             Read the file COPYING that comes with GRASS
 *             for details.
 *
 ****************************************************************/

#include <string.h>
#include <grass/gis.h>
#include <grass/glocale.h>
#include "matrix.h"

int matrix_init(int rows, int cols, MATRIX *res)
{

    int i, j;

    res->rows = rows;
    res->cols = cols;
    res->a = (double **)G_calloc(rows, sizeof(double *));
    if (res->a == NULL)
	return 0;

    for (i = 0; i < rows; i++) {
	res->a[i] = (double *)G_calloc(cols, sizeof(double));
	if (res->a[i] == NULL) {
	    for (j = 0; j < i; j++)
		G_free(res->a[j]);
	    G_free(res->a);
	    return 0;
	}
    }

    return 1;
}

void matrix_free(MATRIX *m)
{
    int i;

    for (i = 0; i < m->rows; i++)
	G_free(m->a[i]);
    G_free(m->a);
    return;
}

int matrix_mult(MATRIX *a, MATRIX *b, MATRIX *res)
{
    if (a->cols != b->rows)
	return 0;

    /*if (!matrix_init(a.rows, b.cols, res))
     * return 0;
     */

    int i, j, k;

    for (i = 0; i < a->rows; i++)
	for (j = 0; j < b->cols; j++) {
	    res->a[i][j] = 0;
	    for (k = 0; k < a->cols; k++)
		res->a[i][j] += a->a[i][k] * b->a[k][j];
	}

    return 1;
}

int matrix_add_identity(double s, MATRIX *m)
{
    if (m->rows != m->cols)
	return 0;
    int i;

    for (i = 0; i < m->rows; i++)
	m->a[i][i] += s;

    return 1;
}

/* three following functions implements elementary row operations on matrices */

/* auxialiry function for matrix_inverse, swaps two rows of given matrix */
void matrix_swap_rows(int x, int y, MATRIX *m)
{
    int i;

    for (i = 0; i < m->cols; i++) {
	double t;

	t = m->a[x][i];
	m->a[x][i] = m->a[y][i];
	m->a[y][i] = t;
    }
    return;
}

/* auxiliary function for matrix_inverse, multiplies row of a matrix by
 * a scalar */
void matrix_row_scalar(int row, double s, MATRIX *m)
{
    int i;

    for (i = 0; i < m->cols; i++)
	m->a[row][i] *= s;
    return;
}

/* auxiliary function for matrix_inverse, adds a multiple of 
 * one row to another.
 * i.e row[ra] = row[ra] + row[rb] * s;
 */
void matrix_row_add_multiple(int ra, int rb, double s, MATRIX *m)
{
    int i;

    for (i = 0; i < m->cols; i++)
	m->a[ra][i] += m->a[rb][i] * s;
    return;
}

/* TODO: don't test directly equality to zero */
int matrix_inverse(MATRIX *a, MATRIX *res, int percents)
{
    int i, j;

    /* not a square matrix */
    if (a->rows != a->cols)
	return 0;

    /* initialize output matrix to the identity matrix */
    if (!matrix_init(a->rows, a->rows, res)) {
	G_fatal_error(_("Out of memory"));
	return 0;
    }
    for (i = 0; i < a->rows; i++) {
	memset(res->a[i], 0, sizeof(double) * a->cols);
	res->a[i][i] = 1;
    }

    /* in order to obtain the inverse of a matrix, we run
     * gauss elimination on the matrix and each time we apply
     * elementary row operation on this matrix, we apply the
     * same operation on the identity matrix. Correctness of
     * this follows from the fact that an invertible matrix
     * is row equivalent to the identity matrix.
     */

    int n = a->rows;

    if (percents)
	G_percent_reset();

    for (i = 0; i < n; i++) {
	int found = 0;
	double c;

	if (percents)
	    G_percent(i, n, 1);
	for (j = i; j < n; j++) {
	    if (a->a[j][i] != 0) {	/* need to change this row to something */
		found = 1;	/* more sensible */
		matrix_swap_rows(i, j, a);
		matrix_swap_rows(i, j, res);
		break;
	    }
	}
	if (!found)
	    return 0;

	c = (double)1.0 / a->a[i][i];

	matrix_row_scalar(i, c, a);
	matrix_row_scalar(i, c, res);
	for (j = 0; j < n; j++) {
	    if (i == j)
		continue;

	    c = -a->a[j][i];

	    if (c == 0.0)
		continue;
	    matrix_row_add_multiple(j, i, c, a);
	    matrix_row_add_multiple(j, i, c, res);
	}
    }

    return 1;
}

void matrix_mult_scalar(double s, MATRIX *m)
{
    int i, j;

    for (i = 0; i < m->rows; i++)
	for (j = 0; j < m->cols; j++)
	    m->a[i][j] *= s;
}

void matrix_add(MATRIX *a, MATRIX *b, MATRIX *res)
{
    int i, j;

    for (i = 0; i < res->rows; i++)
	for (j = 0; j < res->cols; j++)
	    res->a[i][j] = a->a[i][j] + b->a[i][j];
}

void matrix_print(MATRIX *a)
{
    int i, j;

    for (i = 0; i < a->rows; i++) {
	double s = 0;

	for (j = 0; j < a->cols; j++) {
	    printf("%.3lf ", a->a[i][j]);
	    s += a->a[i][j];
	}
	printf("|%.5lf\n", s);
    }
    printf("\n");

}