// ===================================================================
// $Id: matrix4.cpp,v 1.4 2002/12/05 14:45:00 havran Exp $
//
// matrix4.cpp
//     CMatrix4 routines for size 4x4
//
// 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 Nicholas Wilt, functions added by Vlastimil Havran.
//
// Given to BRDFLIB project for the Polina Kondratieva's
// HIWI JOB assignment in November 2003.
// ===================================================================


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

// standard headers
#include <iomanip>

__BEGIN_BRDFLIB_SOURCE


// full constructor
CMatrix4::CMatrix4(float x11, float x12, float x13, float x14,
		   float x21, float x22, float x23, float x24,
		   float x31, float x32, float x33, float x34,
		   float x41, float x42, float x43, float x44)
{
 // first index is column [x], the second is row [y]
  x[0][0] = x11;
  x[1][0] = x12;
  x[2][0] = x13;
  x[3][0] = x14;

  x[0][1] = x21;
  x[1][1] = x22;
  x[2][1] = x23;
  x[3][1] = x24;

  x[0][2] = x31;
  x[1][2] = x32;
  x[2][2] = x33;
  x[3][2] = x34;

  x[0][3] = x41;
  x[1][3] = x42;
  x[2][3] = x43;
  x[3][3] = x44;
}

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

// 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
CMatrix4::Invert()
{
  int indxc[4],indxr[4],ipiv[4];
  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 = 4;

  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.
CMatrix4
Invert(const CMatrix4& M)
{
  CMatrix4 InvertMe = M;
  InvertMe.Invert();
  return InvertMe;
}


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

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

// Construct an identity matrix.
CMatrix4 CMatrix4::IdentityMatrix()
{
	return IdentityMatrix();
}

CMatrix4 IdentityMatrix()
{
  CMatrix4 M;

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

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

// Construct a translation matrix given the location to translate to.
CMatrix4
TranslationMatrix(const CVector3D& Location)
{
  CMatrix4 M = IdentityMatrix();
  M.x[3][0] = Location.x;
  M.x[3][1] = Location.y;
  M.x[3][2] = Location.z;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// X axis.
CMatrix4
RotationXMatrix(float Angle)
{
  CMatrix4 M = IdentityMatrix();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[1][1] = Cosine;
  M.x[2][1] = -Sine;
  M.x[1][2] = Sine;
  M.x[2][2] = Cosine;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// Y axis.
CMatrix4
RotationYMatrix(float Angle)
{
  CMatrix4 M = IdentityMatrix();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[0][0] = Cosine;
  M.x[2][0] = -Sine;
  M.x[0][2] = Sine;
  M.x[2][2] = Cosine;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// Z axis.
CMatrix4
RotationZMatrix(float Angle)
{
  CMatrix4 M = IdentityMatrix();
  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 yaw-pitch-roll rotation matrix.	Rotate Yaw
// radians about the XY axis, rotate Pitch radians in the
// plane defined by the Yaw rotation, and rotate Roll radians
// about the axis defined by the previous two angles.
CMatrix4
RotationYPRMatrix(float Yaw, float Pitch, float Roll)
{
  CMatrix4 M;
  float ch = cos(Yaw);
  float sh = sin(Yaw);
  float cp = cos(Pitch);
  float sp = sin(Pitch);
  float cr = cos(Roll);
  float sr = sin(Roll);

  M.x[0][0] = ch * cr + sh * sp * sr;
  M.x[1][0] = -ch * sr + sh * sp * cr;
  M.x[2][0] = sh * cp;
  M.x[0][1] = sr * cp;
  M.x[1][1] = cr * cp;
  M.x[2][1] = -sp;
  M.x[0][2] = -sh * cr - ch * sp * sr;
  M.x[1][2] = sr * sh + ch * sp * cr;
  M.x[2][2] = ch * cp;
  for (int i = 0; i < 4; i++)
    M.x[3][i] = M.x[i][3] = 0;
  M.x[3][3] = 1;

  return M;
}

// Construct a rotation of a given angle about a given axis.
// Derived from Eric Haines's SPD (Standard Procedural 
// Database).
CMatrix4
RotationAxisMatrix(const CVector3D& axis, float angle)
{
  CMatrix4 M;
  double cosine = cos(angle);
  double sine = sin(angle);
  double one_minus_cosine = 1 - cosine;

  M.x[0][0] = axis.x * axis.x + (1.0 - axis.x * axis.x) * cosine;
  M.x[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
  M.x[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
  M.x[0][3] = 0;

  M.x[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
  M.x[1][1] = axis.y * axis.y + (1.0 - axis.y * axis.y) * cosine;
  M.x[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
  M.x[1][3] = 0;

  M.x[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
  M.x[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
  M.x[2][2] = axis.z * axis.z + (1.0 - axis.z * axis.z) * cosine;
  M.x[2][3] = 0;

  M.x[3][0] = 0;
  M.x[3][1] = 0;
  M.x[3][2] = 0;
  M.x[3][3] = 1;

  return M;
}


// Constructs the rotation matrix that rotates 'vec1' to 'vec2'
CMatrix4
RotationVectorsMatrix(const CVector3D &vecStart,
		      const CVector3D &vecTo)
{
  CVector3D vec = CrossProd(vecStart, vecTo);

  if (Magnitude(vec) > CLimits::Small) {
    // vector exist, compute angle
    float angle = acos(DotProd(vecStart, vecTo));
    // normalize for sure
    vec.Normalize();
    return RotationAxisMatrix(vec, angle);
  }

  // opposite or colinear vectors
  CMatrix4 ret = IdentityMatrix();
  if (DotProd(vecStart, vecTo) < 0.0)
    ret *= -1.0; // opposite vectors

  return ret;
}


// Construct a scale matrix given the X, Y, and Z parameters
// to scale by.  To scale uniformly, let X==Y==Z.
CMatrix4
ScaleMatrix(float X, float Y, float Z)
{
  CMatrix4 M = IdentityMatrix();

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

  return M;
}

// Construct a rotation matrix that makes the x, y, z axes
// correspond to the vectors given.
CMatrix4
GenRotation(const CVector3D &x, const CVector3D &y, const CVector3D &z)
{
  CMatrix4 M = 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;
}

// Construct a quadric matrix.	After Foley et al. pp. 528-529.
CMatrix4
QuadricMatrix(float a, float b, float c, float d, float e,
	      float f, float g, float h, float j, float k)
{
  CMatrix4 M;

  M.x[0][0] = a;  M.x[0][1] = d;  M.x[0][2] = f;  M.x[0][3] = g;
  M.x[1][0] = d;  M.x[1][1] = b;  M.x[1][2] = e;  M.x[1][3] = h;
  M.x[2][0] = f;  M.x[2][1] = e;  M.x[2][2] = c;  M.x[2][3] = j;
  M.x[3][0] = g;  M.x[3][1] = h;  M.x[3][2] = j;  M.x[3][3] = k;

  return M;
}

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

CMatrix4
MirrorY()
{
  CMatrix4 M = IdentityMatrix();
  M.x[1][1] = -1;
  return M;
}

CMatrix4
MirrorZ()
{
  CMatrix4 M = IdentityMatrix();
  M.x[2][2] = -1;
  return M;
}

CMatrix4
RotationOnly(const CMatrix4& x)
{
  CMatrix4 M = x;
  M.x[3][0] = M.x[3][1] = M.x[3][2] = 0;
  return M;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

// Apply the rotation portion of a matrix to a vector.
CVector3D
RotateOnly(const CMatrix4& M, const CVector3D& v)
{
  CVector3D ret;
  float denom;

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

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

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

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

// Rotate a direction vector...
CVector3D
PlaneRotate(const CMatrix4& tform, const CVector3D& p)
{
  // I sure hope that matrix is invertible...
  CMatrix4 use = Transpose(Invert(tform));

  return RotateOnly(use, p);
}

// Transform a normal
CVector3D
TransformNormal(const CMatrix4& tform, const CVector3D& n)
{
  CMatrix4 use = NormalTransformMatrix(tform);

  return RotateOnly(use, n);
}
 
CMatrix4 
NormalTransformMatrix(const CMatrix4 &tform)
{
  CMatrix4 m = tform;
  // for normal translation vector must be zero!
  m.x[3][0] = m.x[3][1] = m.x[3][2] = 0.0; 
  // I sure hope that matrix is invertible...
  return Transpose(Invert(m));
}

CVector3D
GetTranslation(const CMatrix4 &M)
{
  return CVector3D(M.x[3][0], M.x[3][1], M.x[3][2]);
}

__END_BRDFLIB_SOURCE
