Matrix.nxc

Go to the documentation of this file.

/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
    Matrix.nxc
    Copyright (C) 2008 Naba Kumar   <naba@gnome.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef __MATRIX_H_
#define __MATRIX_H_

//#define ENABLE_DEBUG
//#define ENABLE_TEST
#include "Debug.nxc"
//#define ASSERT(a,b)
//#define TEST(a,b)

#define LONG_MAX       2147483647 /* 2^31 - 1 */

#define MIJ(m,i,j)     m.elements[(i) * m.cols + (j)]

#define MI(m,i)        m.elements[i]

struct Matrix
{
    long elements[]; 
    int rows;        
    int cols;        
    int size;        
};

void MatrixInit(Matrix &matrix, int rows, int cols)
{
    matrix.rows = rows;
    matrix.cols = cols;
    matrix.size = rows * cols;
    ArrayInit(matrix.elements, 0, matrix.size);
}

void MatrixInitElements(Matrix &matrix, int rows, int cols, long &elements[])
{
    matrix.rows = rows;
    matrix.cols = cols;
    matrix.size = rows * cols;
    ArrayBuild(matrix.elements, elements);
}

void MatrixInitIdentity(Matrix &matrix, int rows, int cols)
{
    matrix.rows = rows;
    matrix.cols = cols;
    matrix.size = rows * cols;
    ArrayInit(matrix.elements, 0, matrix.size);
    for (int i = 0, j = 0; i < rows; i++, j++)
        matrix.elements[i * cols + j] = 1;
}

void MatrixInitDiagonal(Matrix &matrix, int rows, int cols, long value)
{
    matrix.rows = rows;
    matrix.cols = cols;
    matrix.size = rows * cols;
    ArrayInit(matrix.elements, 0, matrix.size);
    for (int i = 0, j = 0; i < rows; i++, j++)
        matrix.elements[i * cols + j] = value;
}

inline long MatrixGet(Matrix &matrix, int row, int col)
{
    return matrix.elements[matrix.cols * row + col];
}

inline void MatrixSet(Matrix &matrix, int row, int col, long value)
{
    matrix.elements[matrix.cols * row + col] = value;
}

bool MatrixCompare (Matrix &matrixA, Matrix &matrixB)
{
    if (matrixA.rows != matrixB.rows || matrixA.cols != matrixB.cols)
        return false;

    for (int i = 0; i < matrixA.size; i++)
       if (MI(matrixA, i) != MI(matrixB, i))
           return false;
    return true;
}

bool MatrixIsNull (Matrix &matrix)
{
    long flag;
    for (int i = 0; i < matrix.size; i++)
         flag |= MI(matrix, i);
    if (flag) return false;
    return true;
}

void MatrixTranspose (Matrix &matrix)
{
    ASSERT(matrix.rows == matrix.cols, "Matrix is not square");
    
    for (int i = 0; i < matrix.rows; i++)
        for (int j = i; j < matrix.cols; j++)
            if (i != j)
            {
                long tmp1 = MatrixGet(matrix, i, j);
                long tmp2 = MatrixGet(matrix, j, i);
                MatrixSet(matrix, i, j, tmp2);
                MatrixSet(matrix, j, i, tmp1);
            }
}

long MatrixMaxima (Matrix &matrix)
{
    long maxima = 0;
    for (int i = 0; i < matrix.size; i++)
    {
        long element = Abs(MI(matrix, i));
        if (element > maxima)
            maxima = element;
    }
    return maxima;
}

long MatrixMinima (Matrix &matrix)
{
    long minima = LONG_MAX;
    for (int i = 0; i < matrix.size; i++)
    {
        long element = Abs(MI(matrix, i));
        if (element && element < minima)
            minima = element;
     }
     return minima;
}

inline void MatrixShiftLeft (Matrix &matrix, int numShift)
{
    if (numShift)
        for (int i = 0; i < matrix.size; i++)
            MI(matrix, i) <<= numShift;
}

inline void MatrixShiftRight (Matrix &matrix, int numShift)
{
    if (numShift)
        for (int i = 0; i < matrix.size; i++)
            MI(matrix, i) >>= numShift;
}

inline void MatrixAdd (Matrix &matrixA, Matrix &matrixB)
{
    ASSERT(matrixA.rows == matrixB.rows && matrixB.cols == matrixB.cols,
           "Matrix Addtion invalid size");

    for (int i = 0; i < matrixA.size; i++)
        MI(matrixA, i) = MI(matrixA, i) + MI(matrixB, i);
}

inline void MatrixSubtract (Matrix &matrixA, Matrix &matrixB)
{
    ASSERT(matrixA.rows == matrixB.rows && matrixB.cols == matrixB.cols,
           "Matrix Addtion invalid size");

    for (int i = 0; i < matrixA.size; i++)
        MI(matrixA, i) = MI(matrixA, i) - MI(matrixB, i);
}

void MatrixMultiply (Matrix &matrixA, Matrix &matrixB, Matrix &matrixC)
{
    ASSERT(matrixA.cols == matrixB.rows, "Mismatch matrix sizes");

    MatrixInit(matrixC, matrixA.rows, matrixB.cols);
    for (int i = 0; i < matrixA.rows; i++)
        for (int j = 0; j < matrixB.cols; j++)
            for (int k = 0; k < matrixA.cols; k++)
                MatrixSet(matrixC, i, j, (MatrixGet(matrixC, i, j) +
                          MatrixGet(matrixA, i, k) * MatrixGet(matrixB, k, j)));
}

inline void MatrixScale (Matrix &matrix, long scale)
{
    for (int i = 0; i < matrix.size; i++)
        MI(matrix, i) *= scale;
}

inline void MatrixReduce (Matrix &matrix, long scale)
{
    for (int i = 0; i < matrix.size; i++)
        MI(matrix, i) /= scale;
}

void MatrixMinor(Matrix &matrix, int minorI, int minorJ, Matrix &matrixM)
{
    int mI = 0, mJ = 0;

    ASSERT(matrix.rows == matrix.cols, "Matrix is not square");
    ASSERT(matrix.rows > 1, "Matrix is not > 1x1");

    MatrixInit(matrixM, matrix.rows - 1, matrix.cols - 1);
    for (int i = 0; i < matrix.rows; i++)
    {
        if (i == minorI) continue;
        for (int j = 0; j < matrix.cols; j++)
        {
            if (j == minorJ) continue;
            MatrixSet(matrixM, mI, mJ, MatrixGet(matrix, i, j));
            mJ++;
        }
        mJ = 0;
        mI++;
    }
}

long MatrixDeterminant3(Matrix &matrix)
{
    ASSERT(matrix.rows == matrix.cols, "Matrix is not square");
    ASSERT(matrix.rows > 0 && matrix.rows <= 3, "Invalid matrix size");
    
    if (matrix.rows == 1)
        return MI(matrix, 0);
    if (matrix.rows == 2)
        return MI(matrix, 0) * MI(matrix, 3) - MI(matrix, 2) * MI(matrix, 1);
    return
        MI(matrix, 0) * MI(matrix, 4) * MI(matrix, 8) -
        MI(matrix, 0) * MI(matrix, 5) * MI(matrix, 7) -
        MI(matrix, 1) * MI(matrix, 3) * MI(matrix, 8) +
        MI(matrix, 1) * MI(matrix, 5) * MI(matrix, 6) +
        MI(matrix, 2) * MI(matrix, 3) * MI(matrix, 7) -
        MI(matrix, 2) * MI(matrix, 4) * MI(matrix, 6) ;
}

long MatrixCofactorIJ (Matrix &matrixIn, int i, int j)
{
    struct RecurseData
    {
        int i; int j; /* Inputs to the frame */
        long minorDet; int x; int flip; int row; /* Intermediate variables in frame */
        long cofactorIJ; /* Output from the frame */
    };
    RecurseData stack[10];
    RecurseData frame;
    
    /* Separate stack for matrices because NXC doesn't seem to work
     * with array of struct of struct so can't use RecurseData for them.
     */
    Matrix matrixStack[];
    Matrix minor, matrix;

    ASSERT(matrixIn.rows == matrixIn.cols, "Matrix is not square");
    ASSERT(matrixIn.rows >= 1, "Matrix is not >= 1x1");
    if (matrixIn.rows == 1)
        return 1;

    int l = 0;
    /* Depth of the recursion tree is N */
    ArrayInit(stack, frame, matrixIn.rows);
    ArrayInit(matrixStack, matrix, matrixIn.rows);

    /* Set frame 0 */
    frame.i = i;
    frame.j = j;
    stack[0] = frame;
    matrixStack[0] = matrixIn;
    matrix = matrixIn; //matrixStack[l];

recurseEnter:
    /* C(i,j) = (-1)^(i+j) |M(i,j)| */

    frame.minorDet = 0;

    /* flip = (-1)^(i + j) */
    if ((frame.i + frame.j) & 1) frame.flip = -1;
    else frame.flip = 1;
    
    /* Get Minor(i,j) */
    MatrixMinor(matrix, frame.i, frame.j, minor);
    
    /* Compute minor determinant |M| */
    if (minor.rows < 4)
    {
        frame.minorDet = MatrixDeterminant3(minor);
    }
    else
    {
        /* |M| = m(0,1) * Cm(0,1) + m(0,2) * Cm(0,2) + .. + m(0,n) * Cm(0,n) */
        for (frame.row = 0; frame.row < minor.rows; frame.row++)
        {
            frame.x = MatrixGet(minor, 0, frame.row);
            
            /* Push frame: Prepare for recursion. */
            stack[l] = frame;
            int rowSave = frame.row;
            /* Push end */

            /* Set next frame */
            l++;
            frame.i = 0;
            frame.j = rowSave;
            matrixStack[l] = minor;
            matrix = minor;

            goto recurseEnter;
            /* Doing: retCofactorIJ = MatrixCofactorIJ(minor, 0, row) */
            
recurseExit:
            /* Result of last frame */
            long retCofactorIJ = frame.cofactorIJ;
            minor = matrixStack[l];
            
            /* Pop frame from stack */
            l--;
            matrix = matrixStack[l];
            frame = stack[l];
            /* Pop end */
            
            frame.minorDet += frame.x * retCofactorIJ;
        }
    }
    frame.cofactorIJ = frame.minorDet * frame.flip;
    if (l > 0) goto recurseExit;
    return frame.cofactorIJ;
}

void MatrixCofactor (Matrix &matrix, Matrix &retCofactor)
{
    ASSERT(matrix.rows == matrix.cols, "Matrix is not square");
    ASSERT(matrix.rows >= 1, "Matrix size is not >= 1x1");

    MatrixInit(retCofactor, matrix.rows, matrix.cols);
    for (int i = 0; i < matrix.rows; i++)
        for (int j = 0; j < matrix.cols; j++)
        {
            long cofac = MatrixCofactorIJ(matrix, i, j);
            MatrixSet(retCofactor, i, j, cofac);
        }
}

long MatrixDeterminant (Matrix &matrix)
{
    if (matrix.rows < 4)
        return MatrixDeterminant3(matrix);

    ASSERT(matrix.rows == matrix.cols, "Matrix is not square");

    /* |A| = a(0,1) * C(0,1) + a(0,2) * C(0,2) + ... + a(0,n) * C(0,n) */

    long det = 0;
    for (int i = 0; i < matrix.rows; i++)
        det += MatrixGet(matrix, 0, i) * MatrixCofactorIJ(matrix, 0, i);
    return det;
}

void MatrixAdjugate (Matrix &matrix, Matrix &retAdjugate)
{
     MatrixCofactor(matrix, retAdjugate);
     MatrixTranspose(retAdjugate);
}

void MatrixPrint (Matrix &matrix, string title, int pauseSecs)
{
    ClearScreen();
    TextOut(0, LCD_LINE1, "----------------------");
    TextOut(0, LCD_LINE1, title);
    for (int i = 1; i <= matrix.rows; i++)
        for (int j = 0; j < matrix.cols; j++)
            NumOut(30 * j, 63 - (8 * i), MatrixGet(matrix, i - 1, j), false);
    if (pauseSecs > 0)
        Wait(1000 * pauseSecs);
}

inline bool checkMultiplyOverflow(long a, long b)
{
     while (a > 0) {a <<= 1; b >>= 1;}
     if (b) /* overflow */
         return true;
     return false;
}

long MatrixInverse (Matrix matrix, long precision, Matrix &retInverse)
{
     ASSERT(matrix.rows > 0, "Matrix invalid size");
     ASSERT(precision > 0, "Invalid precision");

     long retScale = precision;
     
     /* Determine dynamic range of the input matrix */
     long maxima = MatrixMaxima(matrix);
     long dynamicRange = 1;
     dynamicRange <<= (31/matrix.rows);
     dynamicRange--;   /* 2^(31/N) - 1 */

     /* Reduce the input to fit in above dynamic range */
     int shiftCount = 0;
     while (maxima > dynamicRange) { maxima >>= 1; shiftCount++; }
     MatrixShiftRight(matrix, shiftCount);

     /* Find matrix adjugate and determinant for computing inverse. */
     MatrixAdjugate(matrix, retInverse);
     long det = MatrixDeterminant(matrix);
     
     /* Neutralize the dynamic range adjustments: First try increasing
      * scale factor as much as it can hold and then the rest in the matrix.
      * We shift the precision right
      * (effectively, dividing it by 2 each time) until it is 1 and at
      * the same time decrease shiftCount for residual factor.
      */
     while (retScale >= 0 && shiftCount > 0)
         { retScale <<= 1; shiftCount--; }
     if (retScale < 0) { shiftCount++; retScale >>= 1; }
     if (shiftCount > 0) MatrixShiftRight(retInverse, shiftCount);
     
     /* Find the largest integer in matrix. */
     maxima = MatrixMaxima(retInverse);
     
     if (checkMultiplyOverflow(maxima, precision))
         det /= precision; /* Overflow detected: Apply the scale on det */
     else
         MatrixScale(retInverse, precision); /* No overflow */
     MatrixReduce(retInverse, det);
     return retScale;
}

/* Tests */

#ifdef ENABLE_TEST

int elementsA[] = {
-3,  2, -5,
-1,  0, -2,
 3, -4,  1,
};

int elementsA1[] = {
-3,
};

int elementsA2[] = {
-3,  2,
-1,  4,
};

int elementsA3[] = {
-2, 2, -3,
-1, 1,  3,
 2, 0, -1,
};

int elementsA4[] = {
 2,  5, 1, -3,
-2, -3, 2,  6,
 1,  0, 2,  1,
 0,  2, 2,  1,
};

int elementsA5[] = {
1,  2,  3, 4,  5,
2, -1,  0, 3,  0,
2,  1,  4, 2,  2,
0,  3, -1, 5,  1,
6,  0,  7, 3,  1
};

int elementsB[] = {
 4,  1, -6,
-1, -3,  1,
-3,  2,  2,
};

int elementsC[] = {
 1, 0, 2,
-1, 3, 1,
};

int elementsD[] = {
3, 1,
2, 1,
1, 0,
};

int r_elementsA_trans[] = {
-3, -1,  3,
 2,  0, -4,
-5, -2,  1,
};

int r_elementsAB_add[] = {
 1,  3, -11,
-2, -3,  -1,
 0, -2,   3,
};

int r_elementsAB_sub[] = {
-7,  1,  1,
 0,  3, -3,
 6, -6, -1,
};

int r_elementsA_scale5[] = {
-15,  10, -25,
 -5,   0, -10,
 15, -20,   5,
};

int r_elementsCD_multi[] = {
5, 1,
4, 2,
};

int r_elementsA_minor00[] = {
 0, -2,
-4,  1,
};

int r_elementsA_minor11[] = {
-3, -5,
 3,  1,
};

int r_elementsA_adj[] = {
-8,  18, -4,
-5,  12, -1,
 4,  -6,  2,
};

int r_elementsA3_inv[] = {
 -55,   111, 500,
 277,   444, 500,
-111,   222,   0,
};

int r_elementsA4_adj[] = {
-20, -14, -5, 29,
-10, -7, 6, 6,
19, 15, -8, -25,
-18, -16, 4, 21,
};

int r_elementsA5_adj[] = {
118, -102, -392, 18, 176,
44, -120, -184, 54, 94,
-88, 54, 275, -15, -95,
-64, 90, 200, 6, -86,
100, -36, -173, -21, 53,
};

void TestMatrix()
{
    Matrix A, A1, A2, A3, A4, A5, B, C, D, R, E, T;
    MatrixInitElements(A,  3, 3, elementsA);
    MatrixInitElements(A1, 1, 1, elementsA1);
    MatrixInitElements(A2, 2, 2, elementsA2);
    MatrixInitElements(A3, 3, 3, elementsA3);
    MatrixInitElements(A4, 4, 4, elementsA4);
    MatrixInitElements(A5, 5, 5, elementsA5);
    MatrixInitElements(B,  3, 3, elementsB);
    MatrixInitElements(C,  2, 3, elementsC);
    MatrixInitElements(D,  3, 2, elementsD);

    /* Test Transpose */
    MatrixTranspose(A);
    MatrixInitElements(E, 3, 3, r_elementsA_trans);
    TEST(MatrixCompare(E, A), "Matrix Transpose");
    //MatrixPrint(A);
    MatrixTranspose(A);

    /* Test Addition */
    R = A;
    MatrixAdd(R, B);
    MatrixInitElements(E, 3, 3, r_elementsAB_add);
    TEST(MatrixCompare(E, R), "Matrix Addition");
    //MatrixPrint(R);

    /* Test Subtraction */
    R = A;
    MatrixSubtract(R, B);
    MatrixInitElements(E, 3, 3, r_elementsAB_sub);
    TEST(MatrixCompare(E, R), "Matrix Subtraction");
    //MatrixPrint(R);

    /* Test Scaling */
    R = A;
    MatrixScale(R, 5);
    MatrixInitElements(E, 3, 3, r_elementsA_scale5);
    TEST(MatrixCompare(E, R), "Matrix Scaling");
    //MatrixPrint(R);

    /* Test Reduce */
    MatrixReduce(R, 5);
    TEST(MatrixCompare(A, R), "Matrix Reduction");
    //MatrixPrint(R);

    /* Test Multiply */
    MatrixMultiply(C, D, R);
    MatrixInitElements(E, 2, 2, r_elementsCD_multi);
    TEST(MatrixCompare(E, R), "Matrix Multiply");
    //MatrixPrint(R);

    /* Test Minor A(0,0) */
    MatrixMinor(A, 0, 0, R);
    MatrixInitElements(E, 2, 2, r_elementsA_minor00);
    TEST(MatrixCompare(E, R), "Matrix Minor 00");
    //MatrixPrint(R);

    /* Test Minor A(1,1) */
    MatrixMinor(A, 1, 1, R);
    MatrixInitElements(E, 2, 2, r_elementsA_minor11);
    TEST(MatrixCompare(E, R), "Matrix Minor 11");
    //MatrixPrint(R);

    /* Test Determinants */
    TEST((MatrixDeterminant(A1) == -3), "Matrix determinant");
    TEST((MatrixDeterminant(A2) == -10), "Matrix determinant");
    TEST((MatrixDeterminant(A3) == 18), "Matrix determinant");

    /* Test Cofactor (i,j) */
    TEST((MatrixCofactorIJ(A, 0, 0) == -8), "Matrix Cofac 00");
    TEST((MatrixCofactorIJ(A, 0, 1) == -5), "Matrix Cofac 01");
    TEST((MatrixCofactorIJ(A, 0, 2) ==  4), "Matrix Cofac 02");

    /* Test C(ij), Cofactor and Adjugate */
    MatrixAdjugate(A, R);
    MatrixInitElements(E, 3, 3, r_elementsA_adj);
    TEST(MatrixCompare(E, R), "Matrix Adjugate");
    //MatrixPrint(R);

    /* Test Inverse */
    long scale = MatrixInverse(A3, 1000, R);
    MatrixInitElements(E, 3, 3, r_elementsA3_inv);
    TEST(MatrixCompare(E, R), "Matrix Inverse");
    //MatrixPrint(R, "A3 Inv = ", 5);

#if 0
    MatrixAdjugate(A4, R);
    MatrixInitElements(E, 4, 4, r_elementsA4_adj);
    TEST(MatrixCompare(E, R), "Matrix Adjugate");
    //MatrixPrint(R);

    MatrixAdjugate(A5, R);
    MatrixInitElements(E, 5, 5, r_elementsA5_adj);
    TEST(MatrixCompare(E, R), "Matrix Adjugate");
    //MatrixPrint(R);

    int tick1 = CurrentTick();
    for (int i = 0; i < 1; i++)
    {
        MatrixAdjugate(A, R);
    }
    int tick2 = CurrentTick();
    NumOut(0, LCD_LINE1, (tick2 - tick1), true);
    //MatrixPrint(R);
#endif

    TextOut(0, LCD_LINE1, "Matrix passed");
}
#endif /* ENABLE_TEST */
#endif /* _MATRIX_H_ */