/*
     cvecmat.c: C library for vector and matrix manipulations
     Copyright (C) 2001  CCLRC, Martyn Winn

     This library is free software: you can redistribute it and/or
     modify it under the terms of the GNU Lesser General Public License
     version 3, modified in accordance with the provisions of the 
     license to address the requirements of UK law.
 
     You should have received a copy of the modified GNU Lesser General 
     Public License along with this library.  If not, copies may be 
     downloaded from http://www.ccp4.ac.uk/ccp4license.php
 
     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 Lesser General Public License for more details.
*/

/** @file cvecmat.c
 *  C library for vector and matrix manipulations.
 *  Martyn Winn 
 */

#include <math.h>
#include "cvecmat.h"
/* rcsid[] = "$Id$" */

/*  c = a X b  */

void ccp4_dcross(const double a[3], const double b[3], double c[3])
{
  c[0] = a[1]*b[2] - b[1]*a[2];
  c[1] = a[2]*b[0] - b[2]*a[0];
  c[2] = a[0]*b[1] - b[0]*a[1];
}

void ccp4_3matmul(double c[3][3], const double a[3][3], const double b[3][3])
{
  int i,j,k;

  for ( i = 0; i < 3; i++ ) 
    for ( j = 0; j < 3; j++ ) {
      c[i][j] = 0.0;
      for ( k = 0; k < 3; k++ ) 
        c[i][j] += a[i][k]*b[k][j];
    }
}

void ccp4_4matmul( float c[4][4], const float  a[4][4], const float b[4][4])
{
  int i,j,k;

  for ( i = 0; i < 4; i++ ) 
    for ( j = 0; j < 4; j++ ) {
      c[i][j] = 0.0;
      for ( k = 0; k < 4; k++ ) 
        c[i][j] += a[i][k]*b[k][j];
    }
}

/*   A (I)   3*3 matrix to be inverted */
/*   AI (O)   inverse matrix */
/*   returns determinant */

double invert3matrix(const double a[3][3], double ai[3][3])

{ int i,j;
  double c[3][3],d;

  ccp4_dcross(a[1],a[2],c[0]);
  ccp4_dcross(a[2],a[0],c[1]);
  ccp4_dcross(a[0],a[1],c[2]);

  d = a[0][0]*c[0][0] + a[0][1]*c[0][1] + a[0][2]*c[0][2];

  if (fabs(d) > 1.0e-30) {
    for ( i = 0; i < 3; i++ ) 
      for ( j = 0; j < 3; j++ ) 
        ai[i][j] = c[j][i] / d;
  } else {
    return 0.0;
  }
  return d;
}

/*   A (I)   4*4 matrix to be inverted */
/*   AI (O)   inverse matrix */
/*   returns determinant */

float invert4matrix(const float a[4][4], float ai[4][4])

{
    double c[4][4], d;
    int i, j;
    double x[3][3];
    int i1, j1, i2 ;
    double am, q;
    int ii, jj;

    /* Function Body */
    for (ii = 0; ii < 4; ++ii) {
	for (jj = 0; jj < 4; ++jj) {
            ai[ii][jj] = 0.0;
	    i = -1;
	    for (i1 = 0; i1 < 4; ++i1) {
		if (i1 != ii) {
		    ++i;
		    j = -1;
		    for (j1 = 0; j1 < 4; ++j1) {
			if (j1 != jj) {
			    ++j;
			    x[i][j] = a[i1][j1];
			}
		    }
		}
	    }

	    am = x[0][0]*x[1][1]*x[2][2] - x[0][0]*x[1][2]*x[2][1] +
                 x[0][1]*x[1][2]*x[2][0] - x[0][1]*x[1][0]*x[2][2] +
                 x[0][2]*x[1][0]*x[2][1] - x[0][2]*x[1][1]*x[2][0];
	    i2 = ii + jj;
	    c[ii][jj] = ccp4_pow_ii(-1.0, i2) * am;
	}
    }

/* ---- Calculate determinant */

    d = 0.0;

    for (i = 0; i < 4; ++i) {
	d = a[i][0] * c[i][0] + d;
    }

/* ---- Get inverse matrix */


  if (fabs(d) > 1.0e-30) {
    q = 1.0/d;
    for (i = 0; i < 4; ++i) {
	for (j = 0; j < 4; ++j) {
	  ai[i][j] = (float) (c[j][i] * q);
	}
    }
  } else {
    return 0.0;
  }

  return ((float) d);
} 

float ccp4_pow_ii(const float base, const int power) {

  int i = 0;
  float pow = 1;

  while (++i <= power)
    pow *= base;

  return pow;
}