## Tuesday, September 4, 2012

### 2x2 and 3x3 Matrix Inverses and Linear Solvers

Solving 2x2 and 3x3 systems comes up a lot and I have probably rewritten this code dozens of times over the last ten years.  So I'm finally posting it. The header file is below:

/**
@file solve_2x2_3x3.c
@author James Gregson (james.gregson@gmail.com)
@brief 2x2 and 3x3 matrix inverses and linear system solvers. Free for all use. Best efforts made at testing although I give no guarantee of correctness. Although not required, please leave this notice intact and pass along any bug-fixes/reports to the email address above.  See the functions test_2x2() and test_3x3() for usage details.
*/

#ifndef SOLVE_2X2_3X3
#define SOLVE_2x2_3X3

#define SOLVE_SUCCESS 0
#define SOLVE_FAILURE 1
#define SOLVE_EPSILON 1e-8

/**
@brief Compute the inverse of a 2x2 matrix A and store it in iA, returns SOLVE_FAILURE on failure.
@param[in] A Input matrix, indexed by row then column, i.e. A[row][column]
@param[out] iA Output matrix that is the inverse of A, provided a is definite
@return SOLVE_SUCCESS on success, SOLVE_FAILURE on failure
*/
int invert_2x2( const double A[2][2], double iA[2][2] );

/**
@brief Solves a 2x2 system Ax=b
@param[in] A input 2x2 matrix
@param[out] x output solution vector
@param[in] b input right-hand-side
@return SOLVE_SUCCESS on success, SOLVE_FAILURE on failure
*/
int solve_2x2( const double A[2][2], double x[2], const double b[2] );

/**
@brief Computes the squared residual of a solution x for a linear system Ax=b, for testing purposes.
@param[in] A input matrix
@param[in] x input solution vector
@param[in] b input right-hand-side
@return squared 2-norm of residual vector
*/
double residual_2x2( const double A[2][2], const double x[2], const double b[2] );

/**
@brief Generates random systems and solves them using the solve_2x2() function, printing the sum of residual 2-norms.
*/
void test_2x2();

/**
@brief Compute the inverse of a 3x3 matrix A and store it in iA, returns SOLVE_FAILURE on failure.
@param[in] A Input matrix, indexed by row then column, i.e. A[row][column]
@param[out] iA Output matrix that is the inverse of A, provided a is definite
@return SOLVE_SUCCESS on success, SOLVE_FAILURE on failure
*/
int invert_3x3( const double A[3][3], double iA[3][3] );

/**
@brief Solves a 3x3 system Ax=b
@param[in] A input 3x3 matrix
@param[out] x output solution vector
@param[in] b input right-hand-side
@return SOLVE_SUCCESS on success, SOLVE_FAILURE on failure
*/
int solve_3x3( const double A[3][3], double x[3], const double b[3] );

/**
@brief Computes the squared residual of a solution x for a linear system Ax=b, for testing purposes.
@param[in] A input matrix
@param[in] x input solution vector
@param[in] b input right-hand-side
@return squared 2-norm of residual vector
*/
double residual_3x3( const double A[3][3], const double x[3], const double b[3] );

/**
@brief Generates random systems and solves them using the solve_3x3() function, printing the sum of residual 2-norms.
*/
void test_3x3();

#endif

Here is the corresponding source file:

/**
@file solve_2x2_3x3.c
@author James Gregson (james.gregson@gmail.com)
@brief Implementation for 2x2 and 3x3 matrix inverses and linear system solution. See solve_2x2_3x3.h for details.
*/

#include<math.h>
#include<stdlib.h>
#include<stdio.h>
#include"solve_2x2_3x3.h"

int invert_2x2( const double A[2][2], double iA[2][2] ){
double det;
det = A[0][0]*A[1][1] - A[0][1]*A[1][0];
if( fabs(det) < SOLVE_EPSILON )
return SOLVE_FAILURE;

iA[0][0] =  A[1][1]/det; iA[0][1] = -A[0][1]/det;
iA[1][0] = -A[1][0]/det; iA[1][1] =  A[0][0]/det;

return SOLVE_SUCCESS;
}

int solve_2x2( const double A[2][2], double x[2], const double b[2] ){
double iA[2][2];

if( invert_2x2( A, iA ) )
return SOLVE_FAILURE;

x[0] = iA[0][0]*b[0] + iA[0][1]*b[1];
x[1] = iA[1][0]*b[0] + iA[1][1]*b[1];

return SOLVE_SUCCESS;
}

double residual_2x2( const double A[2][2], const double x[2], const double b[2] ){
double r[2];
r[0] = A[0][0]*x[0] + A[0][1]*x[1] - b[0];
r[1] = A[1][0]*x[0] + A[1][1]*x[1] - b[1];
return r[0]*r[0] + r[1]*r[1];
}

void test_2x2(){
int i, j, k;
double A[2][2], b[2], x[2], r2=0.0;
for( k=0; k<10000; k++ ){
for( i=0; i<2; i++ ){
b[i] = drand48();
for( j=0; j<2; j++ ){
A[i][j] = drand48();
}
}
if( solve_2x2( A, x, b ) == SOLVE_SUCCESS )
r2 += residual_2x2( A, x, b );
}

printf( "2x2 squared residual: %0.16E\n", r2 );
printf( "solution: [%f, %f]^T\n", x[0], x[1] );
}

int invert_3x3( const double A[3][3], double iA[3][3] ){
double det;

det = A[0][0]*(A[2][2]*A[1][1]-A[2][1]*A[1][2])
- A[1][0]*(A[2][2]*A[0][1]-A[2][1]*A[0][2])
+ A[2][0]*(A[1][2]*A[0][1]-A[1][1]*A[0][2]);
if( fabs(det) < SOLVE_EPSILON )
return SOLVE_FAILURE;

iA[0][0] =  (A[2][2]*A[1][1]-A[2][1]*A[1][2])/det;
iA[0][1] = -(A[2][2]*A[0][1]-A[2][1]*A[0][2])/det;
iA[0][2] =  (A[1][2]*A[0][1]-A[1][1]*A[0][2])/det;

iA[1][0] = -(A[2][2]*A[1][0]-A[2][0]*A[1][2])/det;
iA[1][1] =  (A[2][2]*A[0][0]-A[2][0]*A[0][2])/det;
iA[1][2] = -(A[1][2]*A[0][0]-A[1][0]*A[0][2])/det;

iA[2][0] =  (A[2][1]*A[1][0]-A[2][0]*A[1][1])/det;
iA[2][1] = -(A[2][1]*A[0][0]-A[2][0]*A[0][1])/det;
iA[2][2] =  (A[1][1]*A[0][0]-A[1][0]*A[0][1])/det;

return SOLVE_SUCCESS;
}

int solve_3x3( const double A[3][3], double x[3], const double b[3] ){
double iA[3][3];

if( invert_3x3( A, iA ) )
return SOLVE_FAILURE;

x[0] = iA[0][0]*b[0] + iA[0][1]*b[1] + iA[0][2]*b[2];
x[1] = iA[1][0]*b[0] + iA[1][1]*b[1] + iA[1][2]*b[2];
x[2] = iA[2][0]*b[0] + iA[2][1]*b[1] + iA[2][2]*b[2];

return SOLVE_SUCCESS;
}

double residual_3x3( const double A[3][3], const double x[3], const double b[3] ){
double r[3];
r[0] = A[0][0]*x[0] + A[0][1]*x[1] + A[0][2]*x[2] - b[0];
r[1] = A[1][0]*x[0] + A[1][1]*x[1] + A[1][2]*x[2] - b[1];
r[2] = A[2][0]*x[0] + A[2][1]*x[1] + A[2][2]*x[2] - b[2];
return r[0]*r[0] + r[1]*r[1] + r[2]*r[2];
}

void test_3x3(){
int i, j, k;
double A[3][3], b[3], x[3], r2=0.0;
for( k=0; k<10000; k++ ){
for( i=0; i<3; i++ ){
b[i] = drand48();
for( j=0; j<3; j++ ){
A[i][j] = drand48();
}
}
if( solve_3x3( A, x, b ) == SOLVE_SUCCESS )
r2 += residual_3x3( A, x, b );
}

printf( "3x3 squared residual: %0.16E\n", r2 );
printf( "solution: [%f, %f, %f]^T\n", x[0], x[1], x[2] );
}


The code is based on these formulas for 2x2 and 3x3 matrix inverses using Cramer's rule.  The formulas are just translated to C, which I have found in the past to be an error-prone process. They are not optimized in any way, but are tested using the test_2x2() and test_3x3() functions above. Note that Visual C++ users will need a functioning implementation of drand48(), and will need to add the header defining it to the list of included headers in the source file. OS-X and Linux users will already have this function defined in stdlib.h.