/* See r4x4.h. */
/* Last edited on 2005-01-16 15:06:30 by stolfi */
/* Copyright © 2005 Jorge Stolfi, UNICAMP. See note at end of file. */

#include <r4x4.h>
#include <r4.h>
#include <math.h>
#include <stdio.h>
#include <rmxn.h>

#define n 4

void r4x4_zero(r4x4_t *R)
  { int i, j;
    for (i = 0; i < n; i++)
      for (j = 0; j < n; j++)
        { R->c[i][j] = 0.0; }
  }

void r4x4_ident(r4x4_t *R)
  { int i, j;
    for (i = 0; i < n; i++)
      for (j = 0; j < n; j++)
        { R->c[i][j] = (i == j ? 1.0 : 0.0); }
  }

void r4x4_map_row (r4_t *x, r4x4_t *A, r4_t *R)
  { int j, k;
    for (j = 0; j < n; j++)
      { double s = 0.0;
        for (k = 0; k < n; k++) s += x->c[k] * A->c[k][j];
        R->c[j] = s;
      }
  }

void r4x4_map_col (r4x4_t *A, r4_t *x, r4_t *R)
  { int i, k;
    for (i = 0; i < n; i++)
      { double s = 0.0;
        for (k = 0; k < n; k++) s += A->c[i][k] * x->c[k];
        R->c[i] = s;
      }
  }

void r4x4_mul (r4x4_t *A, r4x4_t *B, r4x4_t *R)
  { int i, j, k;
    for (i = 0; i < n; i++)
      for (j = 0; j < n; j++)
        { double s = 0.0;
          for (k = 0; k < n; k++)  s += A->c[i][k]*B->c[k][j];
          R->c[i][j] = s;
        }
  }

double r4x4_det (r4x4_t *A)
  { double A00 = A->c[0][0];
    double A01 = A->c[0][1];
    double A02 = A->c[0][2];
    double A03 = A->c[0][3];

    double A10 = A->c[1][0];
    double A11 = A->c[1][1];
    double A12 = A->c[1][2];
    double A13 = A->c[1][3];

    double A01_01 = A00 * A11 - A01 * A10;
    double A01_02 = A00 * A12 - A02 * A10;
    double A01_12 = A01 * A12 - A02 * A11;
    double A01_03 = A00 * A13 - A03 * A10;
    double A01_13 = A01 * A13 - A03 * A11;
    double A01_23 = A02 * A13 - A03 * A12;

    double A20 = A->c[2][0];
    double A21 = A->c[2][1];
    double A22 = A->c[2][2];
    double A23 = A->c[2][3];

    double A30 = A->c[3][0];
    double A31 = A->c[3][1];
    double A32 = A->c[3][2];
    double A33 = A->c[3][3];

    double A23_01 = A20 * A31 - A21 * A30;
    double A23_02 = A20 * A32 - A22 * A30;
    double A23_12 = A21 * A32 - A22 * A31;
    double A23_03 = A20 * A33 - A23 * A30;
    double A23_13 = A21 * A33 - A23 * A31;
    double A23_23 = A22 * A33 - A23 * A32;

    double d = 
        A01_01 * A23_23 - A01_02 * A23_13 + A01_12 * A23_03
      + A01_03 * A23_12 - A01_13 * A23_02 + A01_23 * A23_01;

    return d;
  }

void r4x4_adj (r4x4_t *A, r4x4_t *R)
  { double A00 = A->c[0][0];
    double A01 = A->c[0][1];
    double A02 = A->c[0][2];
    double A03 = A->c[0][3];

    double A10 = A->c[1][0];
    double A11 = A->c[1][1];
    double A12 = A->c[1][2];
    double A13 = A->c[1][3];

    double A20 = A->c[2][0];
    double A21 = A->c[2][1];
    double A22 = A->c[2][2];
    double A23 = A->c[2][3];

    double A30 = A->c[3][0];
    double A31 = A->c[3][1];
    double A32 = A->c[3][2];
    double A33 = A->c[3][3];

    double A01_01 = A00 * A11 - A01 * A10;
    double A01_02 = A00 * A12 - A02 * A10;
    double A01_12 = A01 * A12 - A02 * A11;
    double A01_03 = A00 * A13 - A03 * A10;
    double A01_13 = A01 * A13 - A03 * A11;
    double A01_23 = A02 * A13 - A03 * A12;

    double A23_01 = A20 * A31 - A21 * A30;
    double A23_02 = A20 * A32 - A22 * A30;
    double A23_12 = A21 * A32 - A22 * A31;
    double A23_03 = A20 * A33 - A23 * A30;
    double A23_13 = A21 * A33 - A23 * A31;
    double A23_23 = A22 * A33 - A23 * A32;

    double A012_012 = A01_01 * A22 - A01_02 * A21 + A01_12 * A20;
    double A012_013 = A01_01 * A23 - A01_03 * A21 + A01_13 * A20;
    double A012_023 = A01_02 * A23 - A01_03 * A22 + A01_23 * A20;
    double A012_123 = A01_12 * A23 - A01_13 * A22 + A01_23 * A21;

    double A013_012 = A01_01 * A32 - A01_02 * A31 + A01_12 * A30;
    double A013_013 = A01_01 * A33 - A01_03 * A31 + A01_13 * A30;
    double A013_023 = A01_02 * A33 - A01_03 * A32 + A01_23 * A30;
    double A013_123 = A01_12 * A33 - A01_13 * A32 + A01_23 * A31;

    double A023_012 = A00 * A23_12 - A01 * A23_02 + A02 * A23_01;
    double A023_013 = A00 * A23_13 - A01 * A23_03 + A03 * A23_01;
    double A023_023 = A00 * A23_23 - A02 * A23_03 + A03 * A23_02;
    double A023_123 = A01 * A23_23 - A02 * A23_13 + A03 * A23_12;

    double A123_012 = A10 * A23_12 - A11 * A23_02 + A12 * A23_01;
    double A123_013 = A10 * A23_13 - A11 * A23_03 + A13 * A23_01;
    double A123_023 = A10 * A23_23 - A12 * A23_03 + A13 * A23_02;
    double A123_123 = A11 * A23_23 - A12 * A23_13 + A13 * A23_12;

    R->c[0][0] =  A123_123; 
    R->c[0][1] = -A023_123; 
    R->c[0][2] =  A013_123; 
    R->c[0][3] = -A012_123;

    R->c[1][0] = -A123_023; 
    R->c[1][1] =  A023_023; 
    R->c[1][2] = -A013_023; 
    R->c[1][3] =  A012_023;

    R->c[2][0] =  A123_013; 
    R->c[2][1] = -A023_013; 
    R->c[2][2] =  A013_013; 
    R->c[2][3] = -A012_013;

    R->c[3][0] = -A123_012; 
    R->c[3][1] =  A023_012; 
    R->c[3][2] = -A013_012; 
    R->c[3][3] =  A012_012;
  }

void r4x4_inv (r4x4_t *A, r4x4_t *R)
  { int i, j;
    double d;
    r4x4_adj(A, R);
    d = A->c[0][0]*R->c[0][0]
      + A->c[0][1]*R->c[1][0] 
      + A->c[0][2]*R->c[2][0] 
      + A->c[0][3]*R->c[3][0];
    for (i = 0; i < n; i++)
      for (j = 0; j < n; j++)
        { R->c[i][j] /= d; }
  }

void r4x4_print (FILE *f, r4x4_t *A)
  { r4x4_gen_print(f, A, NULL, NULL, NULL, NULL, NULL, NULL, NULL); }

void r4x4_gen_print
  ( FILE *f, r4x4_t *A,
    char *fmt, 
    char *olp, char *osep, char *orp,
    char *ilp, char *isep, char *irp
  )
  { rmxn_gen_print(f, n, n, &(A->c[0][0]), fmt, olp, osep, orp, ilp, isep, irp); }

/* COPYRIGHT AND AUTHORSHIP NOTICE

  Copyright © 2005 Jorge Stolfi, Universidade Estadual de Campinas (UNICAMP).
  Created by Jorge Stolfi in 1992--2005.
  
  This source file can be freely distributed, used, and modified,
  provided that this copyright and authorship notice is preserved in
  all copies, and that any modified versions of this file are clearly
  marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the author nor his employers shall be held
  responsible for any losses or damages that may result from its use.

  END OF NOTICE */