/*
 * CMat4D.cpp
 * $Id: CMat4D.cpp,v 1.3 2003/03/05 13:59:09 jhirche Exp $
 *
 * Copyright (C) 1999, 2000 Michael Meissner, Markus Janich, Rainer Jaeger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * As a special exception to the GPL, the QGLViewer authors (Markus
 * Janich, Michael Meissner, Richard Guenther, Alexander Buck and Thomas
 * Woerner) give permission to link this program with Qt (non-)commercial
 * edition, and distribute the resulting executable, without including
 * the source code for the Qt (non-)commercial edition in the source
 * distribution.
 *
 */

//  Description : Class CMat4D
//  Purpose     : Provides  funcionality

/** documentation stuff

  @author Michael Meissner
    
  @version 0.0 //see cvs docu
          
*/



// Qt
///////

// System
///////////
#include <iostream>
#include <iomanip.h>
#include <math.h>
#include <stdlib.h>

// Own
///////////
#include "CMat4D.h"
#include "CV3D.h"




bool LowerUpperDecomposition(double **a, int n, int *indx, double *d);
void ForwardBackwardSubstitution(double **a, int n, int *indx, double b[]);

// swap two double's
void Swap(double& a, double& b) {
  double temp = a;
  a = b;
  b = temp;

  return;
}


//////////////////
// CONSTRUCTORS //
//////////////////

CMat4D::CMat4D(void) 
/*********************************************************************/
{
  clear(); 
  m_ardValues[0] = m_ardValues[5] = m_ardValues[10] = m_ardValues[15] = 1.0;
}


CMat4D::CMat4D(const double *field) 
/*********************************************************************/
{
  for(int i=0; i<16; i++) {
    m_ardValues[i] = field[i];
  }
}


CMat4D::CMat4D(const CV4D &col1, const CV4D &col2, 
               const CV4D &col3, const CV4D &col4) 
/*********************************************************************/
{
  for(int i=0; i<4; i++) {
    m_ardValues[i]    = col1[i];
    m_ardValues[4+i]  = col2[i];
    m_ardValues[8+i]  = col3[i];
    m_ardValues[12+i] = col4[i];
  }
}


CMat4D::CMat4D(double d0, double d1, double d2, double d3,
	       double d4, double d5, double d6, double d7,
	       double d8, double d9, double d10, double d11,
	       double d12, double d13, double d14, double d15)
/*********************************************************************/
{
  m_ardValues[0] = d0;
  m_ardValues[1] = d1;
  m_ardValues[2] = d2;
  m_ardValues[3] = d3;
  m_ardValues[4] = d4;
  m_ardValues[5] = d5;
  m_ardValues[6] = d6;
  m_ardValues[7] = d7;
  m_ardValues[8] = d8;
  m_ardValues[9] = d9;
  m_ardValues[10] = d10;
  m_ardValues[11] = d11;
  m_ardValues[12] = d12;
  m_ardValues[13] = d13;
  m_ardValues[14] = d14;
  m_ardValues[15] = d15;
}


CMat4D::CMat4D (const CMat4D &m)
/*********************************************************************/
{
  for (int i=0;i<16;i++) m_ardValues[i] = m.m_ardValues[i];
}



// Function  : PIdentity 
// Parameters: void
// Purpose   :
// Comments  :
CMat4D CMat4D::PIdentity(void)
/*********************************************************************/
{
  return CMat4D();
}



// Function  : PTranslate 
// Parameters: const CV4D &v
// Purpose   :
// Comments  :
CMat4D CMat4D::PTranslate(const CV4D &v)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[12] = v[0]; 
  mat.m_ardValues[13] = v[1];
  mat.m_ardValues[14] = v[2];

  return mat;
}



// Function  : PTranslate 
// Parameters: const CV3D &v
// Purpose   : FIXME: Could be removed since CV3D is automatically cast to CV4D, right?
// Comments  :
CMat4D CMat4D::PTranslate(const CV3D &v)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[12] = v[0]; 
  mat.m_ardValues[13] = v[1]; 
  mat.m_ardValues[14] = v[2]; 

  return mat;
}



// Function  : PTranslate 
// Parameters: double rdX, double rdY, double rdZ
// Purpose   : 
// Comments  :
CMat4D CMat4D::PTranslate(double rdX, double rdY, double rdZ)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[12] = rdX; 
  mat.m_ardValues[13] = rdY; 
  mat.m_ardValues[14] = rdZ; 

  return mat;
}



// Function  : PScale 
// Parameters: const CV4D &v
// Purpose   : 
// Comments  :
CMat4D CMat4D::PScale(const CV4D &v)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[0]  = v[0]; 
  mat.m_ardValues[5]  = v[1]; 
  mat.m_ardValues[10] = v[2]; 

  return mat;
}



// Function  : PScale 
// Parameters: const CV3D &v
// Purpose   : FIXME: Could be removed since CV3D is automatically cast to CV4D, right?
// Comments  :
CMat4D CMat4D::PScale(const CV3D &v)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[0]  = v[0]; 
  mat.m_ardValues[5]  = v[1]; 
  mat.m_ardValues[10] = v[2]; 

  return mat;
}



// Function  : PScale 
// Parameters: double rdX, double rdY, double rdZ
// Purpose   : 
// Comments  :
CMat4D CMat4D::PScale(double rdX, double rdY, double rdZ)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  mat.m_ardValues[0]  = rdX; 
  mat.m_ardValues[5]  = rdY; 
  mat.m_ardValues[10] = rdZ; 

  return mat;
}



// Function  : PRotate 
// Parameters: const CV4D &Axis, double rdAngle
// Purpose   : 
// Comments  :
CMat4D CMat4D::PRotate(const CV4D &Axis, double rdAngle)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  double rdCosA = cos(rdAngle);
  double rdSinA = sin(rdAngle);
  double rdInvCosA = 1.0-rdCosA;
  double rdA = Axis[0];
  double rdB = Axis[1];
  double rdC = Axis[2];

  mat.m_ardValues[0]  = rdInvCosA*rdA*rdA + rdCosA;
  mat.m_ardValues[1]  = rdInvCosA*rdA*rdB - rdSinA*rdC;
  mat.m_ardValues[2]  = rdInvCosA*rdA*rdC + rdSinA*rdB;

  mat.m_ardValues[4]  = rdInvCosA*rdA*rdB + rdSinA*rdC;
  mat.m_ardValues[5]  = rdInvCosA*rdB*rdB + rdCosA;
  mat.m_ardValues[6]  = rdInvCosA*rdB*rdC - rdSinA*rdA;

  mat.m_ardValues[8]  = rdInvCosA*rdA*rdC - rdSinA*rdB;
  mat.m_ardValues[9]  = rdInvCosA*rdB*rdC + rdSinA*rdA;
  mat.m_ardValues[10] = rdInvCosA*rdC*rdC + rdCosA;

  return mat;
}



// Function  : PRotate 
// Parameters: const CV3D &Axis, double rdAngle
// Purpose   : 
// Comments  :
CMat4D CMat4D::PRotate(const CV3D &Axis, double rdAngle)
/*********************************************************************/
{
  CV4D vec4D(Axis[0], Axis[1], Axis[2]);
  return PRotate(vec4D, rdAngle);
}



// Function  : PRotate 
// Parameters: CQuat &quad
// Purpose   : 
// Comments  :
CMat4D  CMat4D::PRotate(CQuat &quad)
/*********************************************************************/
{
  CMat4D mat; // automatically identity
  quad.normalize();

  mat.m_ardValues[0]  = 1-2*quad.yv()*quad.yv()-2*quad.zv()*quad.zv();
  mat.m_ardValues[4]  = 2*quad.xv()*quad.yv()-2*quad.wv()*quad.zv();
  mat.m_ardValues[8]  = 2*quad.xv()*quad.zv()+2*quad.wv()*quad.yv();

  mat.m_ardValues[1]  = 2*quad.xv()*quad.yv()+2*quad.wv()*quad.zv();
  mat.m_ardValues[5]  = 1-2*quad.xv()*quad.xv()-2*quad.zv()*quad.zv();
  mat.m_ardValues[9]  = 2*quad.yv()*quad.zv()-2*quad.wv()*quad.xv();

  mat.m_ardValues[2]  = 2*quad.xv()*quad.zv()-2*quad.wv()*quad.yv();
  mat.m_ardValues[6]  = 2*quad.yv()*quad.zv()+2*quad.wv()*quad.xv();
  mat.m_ardValues[10] = 1-2*quad.xv()*quad.xv()-2*quad.yv()*quad.yv();

  return mat;
}



////////////////
// DESTRUCTOR //
////////////////

CMat4D::~CMat4D(void)
{
  // Empty  delete [] m_ardValues;
}


//////////////////////////
// OVERLOADED OPERATORS //
//////////////////////////



// Function  : operator=
// Parameters: const CMat4D &mat
// Purpose   :
// Comments  :
const CMat4D& CMat4D::operator=(const CMat4D& mat) 
/*********************************************************************/
{
  for (int i=0; i<16; i++) 
    m_ardValues[i] = mat.m_ardValues[i];
  return *this;
}



// Function  : operator=
// Parameters: const double a
// Purpose   :
// Comments  :
const CMat4D& CMat4D::operator=(const double a) 
/*********************************************************************/
{
  for (int i=0; i<16; i++) 
    m_ardValues[i] = (i%5) ? 0.0 : a;
  return *this;
}



// Function  : operator+=
// Parameters: const CMat4D &mat
// Purpose   :
// Comments  :
CMat4D& CMat4D::operator+=(const CMat4D &mat)
/*********************************************************************/
{ 
  for (int i=0;i<16;i++) 
    m_ardValues[i] += mat.m_ardValues[i];
  return *this;
}



// Function  : operator-=
// Parameters: const CMat4D &mat
// Purpose   :
// Comments  :
CMat4D& CMat4D::operator-=(const CMat4D &mat)
/*********************************************************************/
{
  for (int i=0;i<16;i++) 
    m_ardValues[i] -= mat.m_ardValues[i];
  return *this;
}



// Function  : operator*=
// Parameters: const CMat4D &m
// Purpose   :
// Comments  :
CMat4D& CMat4D::operator*=(const CMat4D &m)
/*********************************************************************/
{
  *this = *this * m;
  return *this;
}



// Function  : operator+
// Parameters: const CMat4D &mat
// Purpose   :
// Comments  :
CMat4D CMat4D::operator+(const CMat4D &mat) const
/*********************************************************************/
{
  CMat4D matrix;
  for (int i=0;i<16;i++) 
    matrix.m_ardValues[i] += mat.m_ardValues[i];
  return matrix;
}



// Function  : operator-
// Parameters: const CMat4D &mat
// Purpose   :
// Comments  :
CMat4D CMat4D::operator-(const CMat4D &mat) const
/*********************************************************************/
{
  CMat4D matrix;
  for (int i=0;i<16;i++) 
    matrix.m_ardValues[i] -= mat.m_ardValues[i];
  return matrix;
}



// Function  : operator*
// Parameters: const CMat4D &m
// Purpose   :
// Comments  :
CMat4D CMat4D::operator*(const CMat4D &m) const
/*********************************************************************/
{
  CMat4D matrix;
  double elmt;
  for (int i=0; i<4; i++) { // row index
    for (int j=0; j<4; j++) { // column index
      // calculate element (i,k), e.g. m_ardValues[4*i+k]
      elmt = 0.0;
      for (int k=0;k<4;k++) 
	elmt += m_ardValues[k*4+i] * m.m_ardValues[j*4+k];
      matrix.setCoeff(i,j,elmt); // slow: FIXME
    }
  }
  return matrix;
}



// Function  : operator*
// Parameters: const CMat4D &m, const CV4D &v
// Purpose   :
// Comments  :
CV4D operator*(const CMat4D &m, const CV4D &v)
/*********************************************************************/
{
  CV4D vector; // Assumes that vector is initialized with (0,0,0,0)
  for (int i=0; i<4; i++) { // row index
    for (int j=0; j<4; j++) { // column index
      vector[i] += m.m_ardValues[j*4+i] * v[j];
    }
  }
  return vector;
}



// Function  : operator*
// Parameters: const CMat4D &m, const CP4D &p
// Purpose   :
// Comments  :
CP4D operator*(const CMat4D &m, const CP4D &p) 
/*********************************************************************/
{
  CP4D point;
  for (int i=0; i<4; i++) { // row index
    for (int j=0; j<4; j++) { // column index
      point[i] += m.m_ardValues[j*4+i] * p[j];
    }
  }
  return point;
}



// Function  : operator*
// Parameters: double scalar
// Purpose   :
// Comments  :
CMat4D CMat4D::operator*(double rdScalar) const
/*********************************************************************/
{
  CMat4D matrix;
  for (int i=0;i<16;i++) 
    matrix.m_ardValues[i] = m_ardValues[i] * rdScalar;
  return matrix;
}



// Function  : operator*
// Parameters: double scalar, const CMat4D &m
// Purpose   :
// Comments  :
CMat4D operator*(double rdScalar, const CMat4D &m) 
/*********************************************************************/
{
  CMat4D matrix;
  for (int i=0;i<16;i++) 
    matrix.m_ardValues[i] = m.m_ardValues[i] * rdScalar;
  return matrix;
}



// Function  : operator==
// Parameters: const CMat4D &m
// Purpose   :
// Comments  :
bool CMat4D::operator==(const CMat4D &m) const
/*********************************************************************/
{
  bool flag = true;
  for (int i=0; i<16 && flag==true; i++, flag = m_ardValues[i] == m.m_ardValues[i]);
  return flag;
}



// Function  : operator!=
// Parameters: const CMat4D &m
// Purpose   :
// Comments  :
bool CMat4D::operator!=(const CMat4D &m) const
/*********************************************************************/
{
  return !(*this == m);
}



// Function  : operator()
// Parameters: int i,int j
// Purpose   :
// Comments  :
double CMat4D::operator()(int i,int j) const
/*********************************************************************/
{
  if (i<0 || j<0 || i>=4 || j>=4) {
    cout << "CMat4D::operator(i,j) Indices out of range" << endl;
    exit(0);
  }
  return m_ardValues[i+j*4];
}



// Function  : operator[]
// Parameters: int i
// Purpose   :
// Comments  :
CV4D CMat4D::operator[](int i) const 
/*********************************************************************/
{
  if (i<0 || i>=4) {
    cout << "CMat4D::operator[i] Indices out of range" << endl;
    exit(0);
  }
  return CV4D(m_ardValues[i], m_ardValues[i+4], m_ardValues[i+8], m_ardValues[i+12]);
}



// Function  : operator()
// Parameters: int j
// Purpose   :
// Comments  :
CV4D CMat4D::operator()(int j) const 
/*********************************************************************/
{
  if (j<0 || j>=4) {
    cout << "CMat4D::operator(j) Indices out of range" << endl;
    exit(0);
  }
  return CV4D(m_ardValues[j*4], m_ardValues[j*4+1], m_ardValues[j*4+2], m_ardValues[j*4+3]);
}



// Function  : operator<<
// Parameters: ostream& s, const CMat4D &m
// Purpose   :
// Comments  :
ostream& operator<<(ostream& s, const CMat4D &m)
/*********************************************************************/
{
  for (int i=0; i<4; i++) {
    s << " | ";
    for (int j=0; j<4; j++) {
      s << setprecision (4) 
	<< setiosflags (ios::showpoint | ios::fixed) 
	<< setw (10) 
	<< m.getCoeff(i,j); 
    } 
    s <<" | "<< endl;
  }
  return s ; 
}



//////////////////////
// MEMBER FUNCTIONS //
//////////////////////



// Function  : clear
// Parameters: void
// Purpose   :
// Comments  :
void CMat4D::clear(void) 
/*********************************************************************/
{
  for (int i=0; i<16; i++) 
    m_ardValues[i] = 0.0;

  return;
}



// Function  : getCoeff
// Parameters: int i, int j
// Purpose   :
// Comments  :
double CMat4D::getCoeff(int i, int j) const
/*********************************************************************/
{
  if (i<0 || j<0 || i>=4 || j>=4) {
    cout << "CMat4D::getCoeff() Indices out of range" << endl;
    exit(0);
  }
  return m_ardValues[i+j*4];
}



// Function  : setCoeff
// Parameters: int i, int j, double a
// Purpose   :
// Comments  :
void CMat4D::setCoeff(int i, int j, double a) 
/*********************************************************************/
{
  if (i<0 || j<0 || i>=4 || j>=4) {
    cout << "CMat4D::setCoeff() Indices out of range" << endl;
    exit(0);
  }
  m_ardValues[i+j*4] = a;

  return;
}



// Function  : setIdentity
// Parameters: void
// Purpose   :
// Comments  :
void CMat4D::setIdentity(void)
/*********************************************************************/
{
  for (int i=0; i<16; i++) 
    m_ardValues[i] = 0.0;
  m_ardValues[0]  = 1.0;
  m_ardValues[5]  = 1.0;
  m_ardValues[10] = 1.0;
  m_ardValues[15] = 1.0;

  return;
}



// Function  : getRow
// Parameters: int nRow
// Purpose   :
// Comments  :
CV4D CMat4D::getRow(int i)
/*********************************************************************/
{
  if (i<0 || i>=4) {
    cout << "CMat4D::getRow() Indices out of range" << endl;
    exit(0);
  }
  return CV4D(m_ardValues[i], 
              m_ardValues[i+4], 
              m_ardValues[i+8], 
	      m_ardValues[i+12]);
}



// Function  : getCol
// Parameters: int nCol
// Purpose   :
// Comments  :
CV4D CMat4D::getCol(int i)
/*********************************************************************/
{
  if (i<0 || i>=4) {
    cout << "CMat4D::getCol() Indices out of range" << endl;
    exit(0);
  }
  return CV4D(m_ardValues[i*4], 
              m_ardValues[i*4+1], 
	      m_ardValues[i*4+2], 
	      m_ardValues[i*4+3]);
}



// Function  : setValues
// Parameters: double* prdValue
// Purpose   :
// Comments  :
void CMat4D::setValues(double *prdValue)
/*********************************************************************/
{
  for (int i=0; i<16; i++) 
    m_ardValues[i] = prdValue[i];

  return;
}
 
 
 
// Function  : setRow
// Parameters: int nRow, const CV4D &cRow
// Purpose   :
// Comments  :
void CMat4D::setRow(int nRow, const CV4D &cRow)
/*********************************************************************/
{
  for (int i=0; i<4; i++) {
    m_ardValues[i*4+nRow] = cRow[i];
  }
} 
 
 
 
// Function  : setCol
// Parameters: int nCol, const CV4D &cCol
// Purpose   :
// Comments  :
void CMat4D::setCol(int nCol, const CV4D &cCol)
/*********************************************************************/
{
  for (int i=0; i<4; i++) {
    m_ardValues[i+4*nCol]  = cCol[i];
  }
}



// Function  : setRows
// Parameters: const CV4D &row0, const CV4D &row1, const CV4D &row2, const CV4D &row3
// Purpose   :
// Comments  :
void CMat4D::setRows(const CV4D &row0, const CV4D &row1, 
                     const CV4D &row2, const CV4D &row3)
/*********************************************************************/
{
  for (int i=0; i<4; i++) {
    m_ardValues[i*4]   = row0[i];
    m_ardValues[i*4+1] = row1[i];
    m_ardValues[i*4+2] = row2[i];
    m_ardValues[i*4+3] = row3[i];
  }

  return;
}



// Function  : setCols
// Parameters: const CV4D &col0, const CV4D &col1, const CV4D &col2, const CV4D &col3
// Purpose   :
// Comments  :
void CMat4D::setCols(const CV4D &col0, const CV4D &col1, 
                     const CV4D &col2, const CV4D &col3)
/*********************************************************************/
{
  for (int i=0; i<4; i++) {
    m_ardValues[i]    = col0[i];
    m_ardValues[i+4]  = col1[i];
    m_ardValues[i+8]  = col2[i];
    m_ardValues[i+12] = col3[i];
  }

  return;
}



// Function  : setScaling
// Parameters: const CV4D &v
// Purpose   :
// Comments  :
void CMat4D::setScaling(const CV4D &v)
/*********************************************************************/
{
  clear();
  m_ardValues[0]  = v[0];
  m_ardValues[5]  = v[1];
  m_ardValues[10] = v[2];
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setScaling
// Parameters: const CV3D &v
// Purpose   :
// Comments  :
void CMat4D::setScaling(const CV3D &v)
/*********************************************************************/
{
  clear();
  m_ardValues[0]  = v[0];
  m_ardValues[5]  = v[1];
  m_ardValues[10] = v[2];
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setScaling
// Parameters: double rdX, double rdY, double rdZ
// Purpose   :
// Comments  :
void CMat4D::setScaling(double rdX, double rdY, double rdZ)
/*********************************************************************/
{
  clear();
  m_ardValues[0]  = rdX;
  m_ardValues[5]  = rdY;
  m_ardValues[10] = rdZ;
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setTranslation
// Parameters: const CV4D &v
// Purpose   :
// Comments  :
void CMat4D::setTranslation(const CV4D &v)
/*********************************************************************/
{
  clear();
  m_ardValues[3]  = v[0];
  m_ardValues[7]  = v[1];
  m_ardValues[11] = v[2];
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setTranslation
// Parameters: const CV3D &v
// Purpose   :
// Comments  :
void CMat4D::setTranslation(const CV3D &v)
/*********************************************************************/
{
  clear();
  m_ardValues[3] = v[0];
  m_ardValues[7] = v[1];
  m_ardValues[11] = v[2];
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setTranslation
// Parameters: double rdX, double rdY, double rdZ
// Purpose   :
// Comments  :
void CMat4D::setTranslation(double rdX, double rdY, double rdZ)
/*********************************************************************/
{
  clear();
  m_ardValues[3]  = rdX;
  m_ardValues[7]  = rdY;
  m_ardValues[11] = rdZ;
  m_ardValues[15] = 1.0;

  return;
}



// Function  : setRotation
// Parameters: CV4D &Axis, double rdAngle
// Purpose   :
// Comments  :
void CMat4D::setRotation(CV4D &Axis, double rdAngle)
/*********************************************************************/
{
  *this = CMat4D::PRotate(Axis, rdAngle);

  return;
}



// Function  : setRotation
// Parameters: CV3D &Axis, double rdAngle
// Purpose   :
// Comments  :
void CMat4D::setRotation(CV3D &Axis, double rdAngle)
/*********************************************************************/
{
  CV4D vec4D(Axis[0], Axis[1], Axis[2]);
  setRotation(vec4D, rdAngle);

  return;
}



// Function  : setRotation
// Parameters: CQuat &quad
// Purpose   :
// Comments  :
void CMat4D::setRotation(CQuat &quad)
/*********************************************************************/
{
  clear();
  quad.normalize();

  m_ardValues[0]  = 1 - 2*quad.yv()*quad.yv() - 2*quad.zv()*quad.zv();
  m_ardValues[4]  =     2*quad.xv()*quad.yv() - 2*quad.wv()*quad.zv();
  m_ardValues[8]  =     2*quad.xv()*quad.zv() + 2*quad.wv()*quad.yv();

  m_ardValues[1]  =     2*quad.xv()*quad.yv() + 2*quad.wv()*quad.zv();
  m_ardValues[5]  = 1 - 2*quad.xv()*quad.xv() - 2*quad.zv()*quad.zv();
  m_ardValues[9]  =     2*quad.yv()*quad.zv() - 2*quad.wv()*quad.xv();

  m_ardValues[2]  =     2*quad.xv()*quad.zv() - 2*quad.wv()*quad.yv();
  m_ardValues[6]  =     2*quad.yv()*quad.zv() + 2*quad.wv()*quad.xv();
  m_ardValues[10] = 1 - 2*quad.xv()*quad.xv() - 2*quad.yv()*quad.yv();
  
  m_ardValues[15] = 1.0;
  
  return;
}



// Function  : getTransposed
// Parameters: void
// Purpose   :
// Comments  :
CMat4D CMat4D::getTransposed(void) const 
/*********************************************************************/
{
  CMat4D temp;

  for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      temp.setCoeff(i, j, getCoeff(j,i));
    }
  }

  return temp;
}



// Function  : transpose
// Parameters: void
// Purpose   :
// Comments  :
void CMat4D::transpose(void)
/*********************************************************************/
{
  for (int i=0;i<4;i++) {
    for (int j=0;j<i;j++) {
      Swap((*this).m_ardValues[i*4+j], (*this).m_ardValues[j*4+i]);
    }
  }

  return;
}



// Function  : getInverted
// Parameters: void
// Purpose   :
// Comments  :
CMat4D CMat4D::getInverted(void) const 
/*********************************************************************/
{
  CMat4D temp(*this);
  temp.invert();
  return temp;
}



// Function  : invert
// Parameters: void
// Purpose   :
// Comments  :
bool CMat4D::invert(void)
/*********************************************************************/
{
  double d;
  double **matrix = get2DField();
  double *b = new double[4];
  int i;
  int *indx = new int[4];

  bool flag = LowerUpperDecomposition(matrix, 4, indx, &d);
  if (flag==false) 
    return false;

  for (int iv=0;iv<4;iv++) {
    for (i=0;i<4;i++) b[i] = 0.0;
    b[iv] = 1.0;
    ForwardBackwardSubstitution(matrix, 4, indx, b); 
    for (i=0;i<4;i++) 
      m_ardValues[iv*4+i] = b[i];
  }
  return true;
}



// Function  : get1DField
// Parameters: void
// Purpose   :
// Comments  :
double*  CMat4D::get1DField(void)
/*********************************************************************/
{
  double *field = new double[16];
  for (int i=0; i<16; i++) 
    field[i] = m_ardValues[i];
  return field;
}



// Function  : get2DField
// Parameters: void
// Purpose   :
// Comments  :
double** CMat4D::get2DField(void)
/*********************************************************************/
{
  double **field;
  field = (double**) new double[4];
  for (int i=0;i<4;i++) field[i] = new double[4];

  for(int irow=0;irow<4;irow++) {
    for (int icol=0;icol<4;icol++) {
      field[irow][icol] = m_ardValues[icol*4+irow];
    }
  }
  return field;
}



// Function  : print
// Parameters: void
// Purpose   :
// Comments  :
void CMat4D::print(void)
/*********************************************************************/
{
  cout << endl;
  for (int i=0;i<4;i++) {
    cout << "|" << m_ardValues[i] << "\t" << m_ardValues[i+4] << "\t";
    cout << m_ardValues[i+8] << "\t" << m_ardValues[i+12] << "|" << endl;
  }

  return;
}



//////////////////////
// HELPER FUNCTIONS //
//////////////////////

bool LowerUpperDecomposition(double **a, int n, int *indx, double *d)
{
  int i,imax,j,k;
  double big,dum,sum,temp;
  double *vv = new double[n];
  double TINY = 1.0e-20;

  *d=1.0;
  for (i=0; i<n; i++) {
    big=0.0;
    for (j=0; j<n; j++) 
      if ((temp=fabs(a[i][j])) > big) 
        big = temp;
    if (big==0.0) {
      cout<<"Tried to LU-Decomposite singular matrix !!!"<<endl;
      return false;
    }
    vv[i]=1.0/big;
  }

  for (j=0;j<n;j++) {
    for (i=0;i<j;i++) {
      sum=a[i][j];
      for (k=0;k<i;k++) 
        sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
    }
    big=0.0;
    for (i=j;i<n;i++) {
      sum=a[i][j];
      for (k=0;k<j;k++) sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
      if((dum=vv[i]*fabs(sum)) >= big) {
	big = dum;
	imax = i;
      }
    }
    if (j != imax) {
      for (k=0;k<n;k++) {
	dum = a[imax][k];
	a[imax][k]=a[j][k];
	a[j][k]=dum;
      }
      *d = -(*d);
      vv[imax]=vv[j];
    }

    indx[j] = imax;
    if (a[j][j] == 0.0) 
      a[j][j] = TINY;
    if (j!=n) {
      dum=1.0/(a[j][j]);
      for(i=j+1;i<n;i++) a[i][j] *= dum;
    }
  }
  delete [] vv;

  return true;
}



void ForwardBackwardSubstitution(double **a, int n, int *indx, double b[])
{
  int i,ii=-1,ip,j;
  double sum;

  for (i=0;i<n;i++) {
    ip=indx[i];
    sum=b[ip];
    b[ip]=b[i];
    if(ii>=0)
      for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
    else if (sum) ii=i;
    b[i]=sum;
  }

  for (i=n-1;i>=0;i--) {
    sum=b[i];
    for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
    b[i]=sum/a[i][i];
  }
}