// ===================================================================
// $Id: matrix3.cpp,v 1.2 2002/11/25 20:31:12 havran Exp $
//
// matrix.cpp
//     CMatrix routines.
//
// Licence and usage conditions in November 2003:
// This file can be used and modified only with written permission of
// Vlastimil Havran, Pohodli 27, 57001 Litomysl, the Czech Republic.
// This library is granted to be used by Polina Kondratieva within
// her MPI HIWI job working on BRDFLIB library. The usage condition
// may change in the next version of the library. At the moment,
// the library development is supported by Max-Planck-Institut for
// Informatik, Stuhlsatzenhausweg 85, 66123 Saarbruecken Germany
// See WWW: http://www.mpi-sb.mpg.de
// REPLACEMENT_STRING
//
// Initial coding by Petr Mladek using template matrix.cpp
//
// Given to BRDFLIB project for the Polina Kondratieva's
// HIWI JOB assignment in November 2003.
// ===================================================================


// BRDFLIB headers
#include "config.h"
#include "matrix3.h"
#include "matrix4.h"
#include "vector2.h"
#include "vector3.h"

// standard headers
#include <iomanip>

__BEGIN_BRDFLIB_SOURCE

// full constructor
CMatrix3::CMatrix3(float x11, float x12, float x13,
		   float x21, float x22, float x23,
		   float x31, float x32, float x33)
{
  x[0][0] = x11;
  x[1][0] = x12;
  x[2][0] = x13;
  x[0][1] = x21;
  x[1][1] = x22;
  x[2][1] = x23;
  x[0][2] = x31;
  x[1][2] = x32;
  x[2][2] = x33;
}

// full constructor - take only the part of matrix
CMatrix3::CMatrix3(const CMatrix4 &src)
{
  for (int i=0; i < 3; i++)
    for (int j=0; j < 3; j++)
      x[i][j] = src.x[i][j];
}

// inverse matrix computation gauss_jacobiho method .. from N.R. in C
// if matrix is regular = computatation successfull = returns 0
// in case of singular matrix returns 1
int
CMatrix3::Invert()
{
  int indxc[3],indxr[3],ipiv[3];
  int i,icol,irow,j,k,l,ll,n;
  float big,dum,pivinv,temp;
  // satisfy the compiler
  icol = irow = 0;

  // the size of the matrix
  n = 3;

  for ( j = 0 ; j < n ; j++) /* zero pivots */
    ipiv[j] = 0;

  for ( i = 0; i < n; i++)
  {
    big = 0.0;
    for (j = 0 ; j < n ; j++)
      if (ipiv[j] != 1)
        for ( k = 0 ; k<n ; k++)
        {
          if (ipiv[k] == 0)
	  {
	    if (fabs(x[k][j]) >= big)
            {
	      big = fabs(x[k][j]);
              irow = j;
              icol = k;
            }
          }
          else
            if (ipiv[k] > 1)
	      return 1; /* singular matrix */
        }
    ++(ipiv[icol]);
    if (irow != icol)
    {
      for ( l = 0 ; l<n ; l++)
      {
        temp = x[l][icol];
        x[l][icol] = x[l][irow];
        x[l][irow] = temp;
      }
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (x[icol][icol] == 0.0)
      return 1; /* singular matrix */

    pivinv = 1.0 / x[icol][icol];
    x[icol][icol] = 1.0 ;
    for ( l = 0 ; l<n ; l++)
      x[l][icol] = x[l][icol] * pivinv ;
    
    for (ll = 0 ; ll < n ; ll++)
      if (ll != icol)
      {
        dum = x[icol][ll];
	x[icol][ll] = 0.0;
        for ( l = 0 ; l<n ; l++)
	  x[l][ll] = x[l][ll] - x[l][icol] * dum ;
      }
  }
  for ( l = n; l--; )
  {
    if (indxr[l] != indxc[l])
      for ( k = 0; k<n ; k++)
      {
        temp = x[indxr[l]][k];
	x[indxr[l]][k] = x[indxc[l]][k];
	x[indxc[l]][k] = temp;
      }
  }

  return 0 ; // matrix is regular .. inversion has been succesfull
}

// Invert the given matrix using the above inversion routine.
CMatrix3
Invert(const CMatrix3& M)
{
  CMatrix3 InvertMe = M;
  InvertMe.Invert();
  return InvertMe;
}


// Transpose the matrix.
void
CMatrix3::Transpose()
{
  for (int i = 0; i < 3; i++)
    for (int j = i; j < 3; j++)
      if (i != j) {
	float temp = x[i][j];
	x[i][j] = x[j][i];
	x[j][i] = temp;
      }
}

// Construct a rotation matrix that makes the x, y, z axes
// correspond to the vectors given.
CMatrix3
GenRotation3(const CVector3D &x, const CVector3D &y, const CVector3D &z)
{
  CMatrix4 M = CMatrix4::IdentityMatrix();
 
#if 1
  //  x  y
  M.x[0][0] = x.x;
  M.x[1][0] = x.y;
  M.x[2][0] = x.z;

  M.x[0][1] = y.x;
  M.x[1][1] = y.y;
  M.x[2][1] = y.z;

  M.x[0][2] = z.x;
  M.x[1][2] = z.y;
  M.x[2][2] = z.z;
#else
  //  x  y -- old version
  M.x[0][0] = x.x;
  M.x[0][1] = x.y;
  M.x[0][2] = x.z;

  M.x[1][0] = y.x;
  M.x[1][1] = y.y;
  M.x[1][2] = y.z;

  M.x[2][0] = z.x;
  M.x[2][1] = z.y;
  M.x[2][2] = z.z;
#endif
  
  return M;
}

// Transpose the given matrix using the transpose routine above.
CMatrix3
Transpose(const CMatrix3& M)
{
  CMatrix3 TransposeMe = M;
  TransposeMe.Transpose();
  return TransposeMe;
}

// Construct an identity matrix.
CMatrix3
IdentityMatrix3()
{
  CMatrix3 M;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      M.x[i][j] = (i == j) ? 1.0 : 0.0;
  return M;
}

// Construct a zero matrix.
CMatrix3
ZeroMatrix3()
{
  CMatrix3 M;
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      M.x[i][j] = 0;
  return M;
}

// Construct a translation matrix given the location to translate to.
CMatrix3
TranslationMatrix3(const CVector2D& Location)
{
  CMatrix3 M = IdentityMatrix3();
  M.x[2][0] = Location.xx;
  M.x[2][1] = Location.yy;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians
CMatrix3
RotationMatrix3(float Angle)
{
  CMatrix3 M = IdentityMatrix3();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[0][0] = Cosine;
  M.x[1][0] = -Sine;
  M.x[0][1] = Sine;
  M.x[1][1] = Cosine;
  return M;
}

// Construct a scale matrix given the X, Y  parameters
// to scale by.  To scale uniformly, let X==Y.
CMatrix3
ScaleMatrix3(float X, float Y)
{
  CMatrix3 M = IdentityMatrix3();

  M.x[0][0] = X;
  M.x[1][1] = Y;

  return M;
}

// Construct various "mirror" matrices, which flip coordinate
// signs in the various axes specified.
CMatrix3
MirrorX2D()
{
  CMatrix3 M = IdentityMatrix3();
  M.x[0][0] = -1;
  return M;
}

CMatrix3
MirrorY2D()
{
  CMatrix3 M = IdentityMatrix3();
  M.x[1][1] = -1;
  return M;
}

// Add corresponding elements of the two matrices.
CMatrix3&
CMatrix3::operator+= (const CMatrix3& A)
{
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      x[i][j] += A.x[i][j];
  return *this;
}

// Subtract corresponding elements of the matrices.
CMatrix3&
CMatrix3::operator-= (const CMatrix3& A)
{
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      x[i][j] -= A.x[i][j];
  return *this;
}

// Scale each element of the matrix by A.
CMatrix3&
CMatrix3::operator*= (float A)
{
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      x[i][j] *= A;
  return *this;
}

// Multiply two matrices.
CMatrix3&
CMatrix3::operator*= (const CMatrix3& A)
{
  CMatrix3 ret = *this;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++) {
      float subt = 0;
      for (int k = 0; k < 3; k++)
	subt += ret.x[i][k] * A.x[k][j];
      x[i][j] = subt;
    }
  return *this;
}

// Add corresponding elements of the matrices.
CMatrix3
operator+ (const CMatrix3& A, const CMatrix3& B)
{
  CMatrix3 ret;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      ret.x[i][j] = A.x[i][j] + B.x[i][j];
  return ret;
}

// Subtract corresponding elements of the matrices.
CMatrix3
operator- (const CMatrix3& A, const CMatrix3& B)
{
  CMatrix3 ret;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      ret.x[i][j] = A.x[i][j] - B.x[i][j];
  return ret;
}

// Multiply matrices.
CMatrix3
operator* (const CMatrix3& A, const CMatrix3& B)
{
  CMatrix3 ret;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++) {
      float subt = 0;
      for (int k = 0; k < 3; k++)
	subt += A.x[i][k] * B.x[k][j];
      ret.x[i][j] = subt;
    }
  return ret;
}

// Transform a vector by a matrix.
CVector2D
operator* (const CMatrix3& M, const CVector2D& v)
{
  CVector2D ret;
  float denom;

  ret.xx = v.xx * M.x[0][0] + v.yy * M.x[1][0] + M.x[2][0];
  ret.yy = v.xx * M.x[0][1] + v.yy * M.x[1][1] + M.x[2][1];
  denom = M.x[0][2] + M.x[1][2] + M.x[2][2];
  if (denom != 1.0)
    ret /= denom;
  return ret;
}

// Scale each element of the matrix by B.
CMatrix3
operator* (const CMatrix3& A, float B)
{
  CMatrix3 ret;

  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      ret.x[i][j] = A.x[i][j] * B;
  return ret;
}

// Overloaded << for C++-style output.
ostream&
operator<< (ostream& s, const CMatrix3& M)
{
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
                                           // x  y 
      s << setprecision(4) << setw(10) << M.x[j][i];
    }
    s << '\n';
  }
  return s;
}

__END_BRDFLIB_SOURCE
