syntax-highlighter

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.

No comments: