QR Decomposition of 3x3 Matrix Using Householder Reflections

qr.cpp

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

void printMatrix3x3(float *mat)
{
   printf("%0.3f\t%0.3f\t%0.3f\n", mat[0], mat[1], mat[2]);
   printf("%0.3f\t%0.3f\t%0.3f\n", mat[3], mat[4], mat[5]);
   printf("%0.3f\t%0.3f\t%0.3f\n", mat[6], mat[7], mat[8]);
   printf("\n");
}

void makeEricsMatrix(float *mat, float a, float b, float c, float d, float x, float y)
{
   mat[0] = a; mat[1] = b; mat[2] = x;
   mat[3] = c; mat[4] = d; mat[5] = y;
   mat[6] = 0; mat[7] = 0; mat[8] = 1;
}

float sign(float x)
{
   if(x > 0.0f)
      return 1.0f;
   if(x < 0.0f)
      return -1.0f;
   return 0.0f;
}

float mag3(float *x)
{
   return sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
}

float mag2(float *x)
{
   return sqrtf(x[0] * x[0] + x[1] * x[1]);
}

void getQ1(float *mat, float *q1)
{
   float x[3];
   x[0] = mat[0]; x[1] = mat[3]; x[2] = mat[6];
   //printf("x = [%0.3f, %0.3f, %0.3f]\n", x[0], x[1], x[2]);
   float magx = mag3(x);
   //printf("magx = %0.3f\n", magx);
   float alpha = magx * -1.0f * sign(mat[0]); //alpha = ||x|| * sign(xk)
   //printf("alpha = %0.3f\n", alpha);
   float u[3];
   u[0] = x[0] + alpha; u[1] = x[1]; u[2] = x[2]; //u = x + alpha * e1
   //printf("u = [%0.3f, %0.3f, %0.3f]\n", u[0], u[1], u[2]);
   float magu = mag3(u);
   //printf("magu = %0.3f\n", magu);
   float v[3];
   v[0] = u[0] / magu; v[1] = u[1] / magu; v[2] = u[2] / magu; //v = u / ||u||
   //printf("v = [%0.3f, %0.3f, %0.3f]\n", v[0], v[1], v[2]);

   //q = I - 2vvT
   q1[0] = 1 - 2 * v[0] * v[0]; q1[1] = -2 * v[0] * v[1]; q1[2] = -2 * v[0] * v[2];
   q1[3] = -2 * v[0] * v[1]; q1[4] = 1 - 2 * v[1] * v[1]; q1[5] = -2 * v[1] * v[2];
   q1[6] = -2 * v[0] * v[2]; q1[7] = -2 * v[1] * v[2]; q1[8] = 1 - 2 * v[2] * v[2];
}

void getQ2(float *mat, float *q2)
{
   float x[2];
   x[0] = mat[4]; x[1] = mat[7];
   float magx = mag2(x);
   float alpha = magx * -1.0f * sign(mat[4]); //alpha = ||x|| * sign(xk)
   float u[2];
   u[0] = x[0] + alpha; u[1] = x[1]; //u = x + alpha * e1
   float magu = mag2(u);
   float v[2];
   v[0] = u[0] / magu; v[1] = u[1] / magu; //v = u / ||u||

   //q = I - 2vvT
   q2[0] = 1; q2[1] = 0; q2[2] = 0;
   q2[3] = 0; q2[4] = 1 - 2 * v[0] * v[0]; q2[5] = -2 * v[0] * v[1];
   q2[6] = 0; q2[7] = -2 * v[0] * v[1]; q2[8] = 1 - 2 * v[1] * v[1];
}

void matMult(float *res, float *a, float *b)
{
   res[0] = a[0] * b[0] + a[1] * b[3] + a[2] * b[6]; res[1] = a[0] * b[1] + a[1] * b[4] + a[2] * b[7]; res[2] = a[0] * b[2] + a[1] * b[5] + a[2] * b[8];
   res[3] = a[3] * b[0] + a[4] * b[3] + a[5] * b[6]; res[4] = a[3] * b[1] + a[4] * b[4] + a[5] * b[7]; res[5] = a[3] * b[2] + a[4] * b[5] + a[5] * b[8];
   res[6] = a[6] * b[0] + a[7] * b[3] + a[8] * b[6]; res[7] = a[6] * b[1] + a[7] * b[4] + a[8] * b[7]; res[8] = a[6] * b[2] + a[7] * b[5] + a[8] * b[8];
}

void transpose(float *res, float *a)
{
   res[0] = a[0]; res[1] = a[3]; res[2] = a[6];
   res[3] = a[1]; res[4] = a[4]; res[5] = a[7];
   res[6] = a[2]; res[7] = a[5]; res[8] = a[8];
}

void qr(float *mat, float *q, float *r)
{
   float q1[9] = {0}, q2[9] = {0};
   float res1[9] = {0};
   //printMatrix3x3(mat);

   getQ1(mat, q1);
   //printMatrix3x3(q1);
   matMult(res1, q1, mat);
   //printMatrix3x3(res1);

   getQ2(res1, q2);
   //printMatrix3x3(q2);
   matMult(r, q2, res1);
   //printMatrix3x3(r);

   float q1t[9] = {0}, q2t[9] = {0};
   transpose(q1t, q1);
   transpose(q2t, q2);
   matMult(q, q1t, q2t);
   //printMatrix3x3(q);
}

int main()
{
   float mat[9] = {12, -51, 4,
                   6, 167, -68,
                   -4, 24, -41};
   float q[9] = {0}, r[9] = {0};
   printf("Matrix:\n");
   printMatrix3x3(mat);
   qr(mat, q, r);
   printf("Q:\n");
   printMatrix3x3(q);
   printf("R:\n");
   printMatrix3x3(r);
   return 0;
}

numerically stable
-> https://github.com/moe123/macadam/blob/master/macadam/details/numa/mc_qrgs3x3.h (Gram-Schmidt)
-> https://github.com/moe123/macadam/blob/master/macadam/details/numa/mc_qrgv3x3.h (Givens rotations)
-> https://github.com/moe123/macadam/blob/master/macadam/details/numa/mc_ortho3x3.h (Modified Gram-Schmidt)