/*
 * Math3d - The 3D Computer Graphics Math Library
 * Copyright (C) 1996-2000 by J.E. Hoffmann <je-h@gmx.net>
 * All rights reserved.
 *
 * This program is  free  software;  you can redistribute it and/or modify it
 * under the terms of the  GNU Lesser General Public License  as published by 
 * the  Free Software Foundation;  either version 2.1 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 Lesser General Public  
 * License for more details.
 *
 * You should  have received  a copy of the GNU Lesser General Public License
 * along with  this program;  if not, write to the  Free Software Foundation,
 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 * $Id: m4x4.cpp,v 1.2 2002/11/26 06:51:32 rst Exp $
 */
#define _MATH3D_EXPORT
#include "m4x4.h"
#include "m3d.h"
#include "m4d.h"
#include "mquat.h"
#include "mrot.h"
#include "mrect.h"
#include <cmath>
#include <iostream>


/*!
 *
 */
Math3d::M4x4::M4x4(const M4x4& A)
{
  memcpy(d_m,A.d_m,sizeof(d_m)); 
}


/*!
 *
 */
const Math3d::M4x4&
Math3d::M4x4::operator=(const M4x4& A)
{
  memcpy(d_m,A.d_m,sizeof(d_m)); 
  return(*this);
}
 

/*!
 *
 */
void
Math3d::M4x4::zero()
{
  int i,j;

  for (i=0; i<4; i++) {
    for (j=0; j<4; j++) d_m[i][j]=0.0;
  }
}


/*!
 *
 */
void
Math3d::M4x4::identity()
{
  int i,j;

  for (i=0; i<4; i++) {
    for (j=0; j<4; j++) d_m[i][j]=0.0;
  }
  for (i=0; i<4; i++) d_m[i][i]=1.0;
}


/*!
 *
 */
void
Math3d::M4x4::copy(const M4x4& A)
{
  memcpy(d_m,A.d_m,sizeof(d_m)); 
}


/*!
 *
 */
void
Math3d::M4x4::setRow(int row, const M3d& A)
{
  ASSERT((row>=0) && (row<=4));
  d_m[0][row]=A[0];
  d_m[1][row]=A[1];
  d_m[2][row]=A[2];
  if (row==3) {
    d_m[3][row]=1.0;
  }
  else {
    d_m[3][row]=0.0;
  }
}


/*!
 *
 */
void
Math3d::M4x4::setRow(int row, const M4d& A)
{
  ASSERT((row>=0) && (row<=4));
  d_m[0][row]=A[0];
  d_m[1][row]=A[1];
  d_m[2][row]=A[2];
  d_m[3][row]=A[3];
}


/*!
 *
 */
void
Math3d::M4x4::setCol(int col, const M3d& A)
{
  ASSERT((col>=0) && (col<=4));
  d_m[col][0]=A[0];
  d_m[col][1]=A[1];
  d_m[col][2]=A[2];
  if (col==3) {
    d_m[col][3]=1.0;
  }
  else {
    d_m[col][3]=0.0;
  }
}


/*!
 *
 */
void
Math3d::M4x4::setCol(int col, const M4d& A)
{
  ASSERT((col>=0) && (col<=4));
  d_m[col][0]=A[0];
  d_m[col][1]=A[1];
  d_m[col][2]=A[2];
  d_m[col][3]=A[3];
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator+(const M4x4& A)
{
  M4x4 M;
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      M[j][i]=d_m[j][i]+A.d_m[j][i];
    }
  }
  return(M);
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator+()
{
  return(*this);
}


/*!
 *
 */
const Math3d::M4x4&
Math3d::M4x4::operator+=(const M4x4& A)
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]+=A.d_m[j][i];
    }
  }
  return(*this);
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator-(const M4x4& A)
{
  M4x4 M;
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      M[j][i]=d_m[j][i]+A.d_m[j][i];
    }
  }
  return(M);
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator-()
{
  M4x4 M;
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      M[j][i]=-d_m[j][i];
    }
  }
  return(M);
}


/*!
 *
 */
const Math3d::M4x4& 
Math3d::M4x4::operator-=(const M4x4& A)
{
  int i,j;
 
  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]-=A.d_m[j][i];
    }
  }
  return(*this);
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator*(const M4x4& A)
{
  M4x4 M;
  int i,j,k;
  double ab;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      ab=0.0f;
      for (k=0; k<4; k++) ab+=d_m[k][i]*A.d_m[j][k];
      M.d_m[j][i]=ab;
    }
  }
  return(M);
}


/*!
 *
 */
const Math3d::M4x4& 
Math3d::M4x4::operator*=(const M4x4& A)
{
  double mm[4][4];
  int i,j,k;
  double ab;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      ab=0.0f;
      for (k=0; k<4; k++) ab+=d_m[k][i]*A.d_m[j][k];
      mm[j][i]=ab;
    }
  }
  memcpy(d_m,mm,sizeof(d_m));
  return(*this);
}


/*!
 *
 */
Math3d::M4x4
Math3d::M4x4::operator*(double k)
{
  M4x4 M(*this);
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      M.d_m[j][i]=d_m[j][i]*k;
    }
  }
  return(M);
}


/*!
 *
 */
const Math3d::M4x4& 
Math3d::M4x4::operator*=(double k)
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]*=k;
    }
  }
  return(*this);
}


/*!
 *
 */
void 
Math3d::M4x4::neg()
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]=-d_m[j][i];
    }
  }
}


/*!
 *
 */
void 
Math3d::M4x4::abs()
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]=fabs(d_m[j][i]);
    }
  }
}


/*!
 *
 */
void
Math3d::M4x4::transpose()
{
  int i,j;
  double swp;

  for (j=0; j<4; j++) {
    for (i=j+1; i<4; i++) {
      swp=d_m[j][i];
      d_m[j][i]=d_m[i][j];
      d_m[i][j]=swp;
    }
  }
}


/*!
 *
 */
void
Math3d::M4x4::add(const M4x4& A, const M4x4& B)
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]=A.d_m[j][i]+B.d_m[j][i];
    }
  }
}


/*!
 *
 */
void
Math3d::M4x4::sub(const M4x4& A, const M4x4& B)
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]=A.d_m[j][i]+B.d_m[j][i];
    }
  }
}


/*!
 *
 */
void
Math3d::M4x4::mul(const M4x4& A, const M4x4& B)
{
  int i,j,k;
  double ab;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      ab=0.0f;
      for (k=0; k<4; k++) ab+=A.d_m[k][i]*B.d_m[j][k];
      d_m[j][i]=ab;
    }
  }
}


/*!
 *
 */
void
Math3d::M4x4::scalar(double k)
{
  int i,j;

  for (j=0; j<4; j++) {
    for (i=0; i<4; i++) {
      d_m[j][i]*=k;
    }
  }
}


static inline
double det2x2(
  double a, double b,
  double c, double d) 
{
  return((a)*(d)-(b)*(c));
}


static double
det3x3(
  double a1, double a2, double a3,
  double b1, double b2, double b3,
  double c1, double c2, double c3)
{
  return(
    a1*det2x2(b2,b3,c2,c3)-
    b1*det2x2(a2,a3,c2,c3)+
    c1*det2x2(a2,a3,b2,b3)
  );
}


/*!
 *
 */
double
Math3d::M4x4::det()
{
  double a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;

  a1 = d_m[0][0];
  b1 = d_m[1][0];
  c1 = d_m[2][0];
  d1 = d_m[3][0];
  a2 = d_m[0][1];
  b2 = d_m[1][1];
  c2 = d_m[2][1];
  d2 = d_m[3][1];
  a3 = d_m[0][2];
  b3 = d_m[1][2];
  c3 = d_m[2][2];
  d3 = d_m[3][2];
  a4 = d_m[0][3];
  b4 = d_m[1][3];
  c4 = d_m[2][3];
  d4 = d_m[3][3];
  return(
    a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)-
    b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)+
    c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)-
    d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
  );
}


/*!
 *
 */
void
Math3d::M4x4::adjoint()
{
  double a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;

  a1 = d_m[0][0];
  b1 = d_m[1][0];
  c1 = d_m[2][0];
  d1 = d_m[3][0];
  a2 = d_m[0][1];
  b2 = d_m[1][1];
  c2 = d_m[2][1];
  d2 = d_m[3][1];
  a3 = d_m[0][2];
  b3 = d_m[1][2];
  c3 = d_m[2][2];
  d3 = d_m[3][2];
  a4 = d_m[0][3];
  b4 = d_m[1][3];
  c4 = d_m[2][3];
  d4 = d_m[3][3];
  d_m[0][0]=  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
  d_m[0][1]= -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
  d_m[0][2]=  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
  d_m[0][3]= -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  d_m[1][0]= -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
  d_m[1][1]=  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
  d_m[1][2]= -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
  d_m[1][3]=  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
  d_m[2][0]=  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
  d_m[2][1]= -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
  d_m[2][2]=  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
  d_m[2][3]= -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
  d_m[3][0]= -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
  d_m[3][1]=  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
  d_m[3][2]= -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
  d_m[3][3]=  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
}


/*!
 * GGemsII, K.Wu, Fast Matrix Inversion 
 */
bool
Math3d::M4x4::inv()
{                          
  int i,j,k;               
  int pvt_i[4], pvt_j[4];            /* Locations of pivot elements */
  double pvt_val;                     /* Value of current pivot element */
  double hold;                        /* Temporary storage */
  double determinat;                  /* Determinant */

  determinat = 1.0f;
  for (k=0; k<4; k++)  {
    /** Locate k'th pivot element **/
    pvt_val=d_m[k][k];            /** Initialize for search **/
    pvt_i[k]=k;
    pvt_j[k]=k;
    for (i=k; i<4; i++) {
      for (j=k; j<4; j++) {
        if (fabs(d_m[i][j]) > fabs(pvt_val)) {
          pvt_i[k]=i;
          pvt_j[k]=j;
          pvt_val=d_m[i][j];
        }
      }
    }

    /** Product of pivots, gives determinant when finished **/
    determinat*=pvt_val;
    if (fabs(determinat)<1e-7) {    
      return(FALSE);  /** Matrix is singular (zero determinant). **/
    }

    /** "Interchange" rows (with sign change stuff) **/
    i=pvt_i[k];
    if (i!=k) {               /** If rows are different **/
      for (j=0; j<4; j++) {
        hold=-d_m[k][j];
        d_m[k][j]=d_m[i][j];
        d_m[i][j]=hold;
      }
    }

    /** "Interchange" columns **/
    j=pvt_j[k];
    if (j!=k) {              /** If columns are different **/
      for (i=0; i<4; i++) {
        hold=-d_m[i][k];
        d_m[i][k]=d_m[i][j];
        d_m[i][j]=hold;
      }
    }
    
    /** Divide column by minus pivot value **/
    for (i=0; i<4; i++) {
      if (i!=k) d_m[i][k]/=( -pvt_val) ; 
    }

    /** Reduce the matrix **/
    for (i=0; i<4; i++) {
      hold = d_m[i][k];
      for (j=0; j<4; j++) {
        if (i!=k && j!=k) d_m[i][j]+=hold*d_m[k][j];
      }
    }

    /** Divide row by pivot **/
    for (j=0; j<4; j++) {
      if (j!=k) d_m[k][j]/=pvt_val;
    }

    /** Replace pivot by reciprocal (at last we can touch it). **/
    d_m[k][k] = 1.0f/pvt_val;
  }

  /* That was most of the work, one final pass of row/column interchange */
  /* to finish */
  for (k=4-2; k>=0; k--) { /* Don't need to work with 1 by 1 corner*/
    i=pvt_j[k];            /* Rows to swap correspond to pivot COLUMN */
    if (i!=k) {            /* If rows are different */
      for(j=0; j<4; j++) {
        hold = d_m[k][j];
        d_m[k][j]=-d_m[i][j];
        d_m[i][j]=hold;
      }
    }

    j=pvt_i[k];           /* Columns to swap correspond to pivot ROW */
    if (j!=k)             /* If columns are different */
    for (i=0; i<4; i++) {
      hold=d_m[i][k];
      d_m[i][k]=-d_m[i][j];
      d_m[i][j]=hold;
    }
  }
  return(TRUE);                          
}


//bool Math3d::M4x4::Inv()  
//{
//  int i,j;
//  double det;
//
//  det=Det();
//  if (fabs(det)<EPSILON) return(FALSE);
//  Adjoint();
//  for (i=0; i<4; i++) for(j=0; j<4; j++) d_m[i][j]/=det;
//  return(TRUE);
//}


/*
 * GGemsI, E. Raible, Matrix Orthogonalization 
 *
 * Reorthogonalize matrix R - that is find an orthogonal matrix that is
 * "close" to R by computing an approximation to the orthogonal matrix
 *
 *           T  -1/2
 *   RC = R(R R)
 *                               T     -1
 * [RC is orthogonal because (RC) = (RC) ]
 *                                                                -1/2
 * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
 * (where x = C - I) about x=0.
 * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
 */
void
Math3d::M4x4::reorthogonalize(int limit)
{
  M4x4 I, x, Xp, sum, RtR, temp;
  int p;
  static double coef[10] = {   /* From mathematica */
    1.0, -1/2.0, 3/8.0, -5/16.0, 35/128.0, 
    -63/256.0, 231/1024.0, -429/2048.0, 
    6435/32768.0, -12155/65536.0 
  };

  if (limit>10) limit=10;

  M4x4 R(*this);
  M4x4 Rt(R);
  Rt.transpose(); 
  RtR.mul(Rt,R);  
  I.identity();
  x.sub(RtR,I);   
  
  Xp.identity();
  sum.identity();
  for (p=1; p<limit; p++) {
    temp.mul(Xp,x);
    Xp=temp;
    temp.scalar(coef[p]);
    sum+=temp;
  }
  mul(R,sum);
}


/*!
 *
 */
void
Math3d::M4x4::loadTranslate(double tx, double ty, double tz)
{
  identity();
  d_m[3][0]=tx;
  d_m[3][1]=ty;
  d_m[3][2]=tz;
}


/*!
 *
 */
void
Math3d::M4x4::translate(double tx, double ty, double tz)
{
  int i;
  
  for (i=0; i<3; i++) {
    d_m[3][i]+= d_m[0][i]*tx + d_m[1][i]*ty + d_m[2][i]*tz;
  }
}


/*!
 *
 */
void
Math3d::M4x4::translate(const M3d& T)
{
  int i;

  for (i=0; i<3; i++) {
    d_m[3][i]+= d_m[0][i]*T.d_v[0] + d_m[1][i]*T.d_v[1] + d_m[2][i]*T.d_v[2];
  }
}


/*!
 *
 */
void
Math3d::M4x4::translateNeg(const M3d& T)
{
  int i;

  for (i=0; i<3; i++) {
    d_m[3][i]-= d_m[0][i]*T.d_v[0] + d_m[1][i]*T.d_v[1] + d_m[2][i]*T.d_v[2];
  }
}




/*!
 *
 */
void
Math3d::M4x4::loadScale(double sx, double sy, double sz)
{
  zero();
  d_m[0][0]=sx;
  d_m[1][1]=sy;
  d_m[2][2]=sz;
  d_m[3][3]=1.0f;
}


/*!
 *
 */
void
Math3d::M4x4::scale(double sx, double sy, double sz)
{
  int i;

  for (i=0; i<4; i++) {
    d_m[0][i]*=sx;
    d_m[1][i]*=sy;
    d_m[2][i]*=sz;
  }
}


/*!
 *
 */
void
Math3d::M4x4::scale(const M3d& S)
{
  int i;

  for (i=0; i<4; i++) {
    d_m[0][i]*=S.d_v[0];
    d_m[1][i]*=S.d_v[1];
    d_m[2][i]*=S.d_v[2];
  }
}


/*!
 *
 */
void
Math3d::M4x4::loadRotateX(double phi)
{
  double SinPhi,CosPhi;

  zero();
  SinPhi=sin(phi);
  CosPhi=cos(phi);
  d_m[0][0]=1.0;
  d_m[1][1]=CosPhi;
  d_m[2][1]=-SinPhi;
  d_m[1][2]=SinPhi;
  d_m[2][2]=CosPhi;
  d_m[3][3]=1.0;
}


/*!
 *
 */
void
Math3d::M4x4::rotateX(double phi)
{
  double SinPhi,CosPhi;
  double a1[4],a2[4];

  SinPhi=sin(phi);
  CosPhi=cos(phi);
  memcpy(a1,d_m[1],4*sizeof(d_m[1][0]));
  memcpy(a2,d_m[2],4*sizeof(d_m[2][0]));
  d_m[1][0]=CosPhi*a1[0]+SinPhi*a2[0];
  d_m[1][1]=CosPhi*a1[1]+SinPhi*a2[1];
  d_m[1][2]=CosPhi*a1[2]+SinPhi*a2[2];
  d_m[1][3]=CosPhi*a1[3]+SinPhi*a2[3];
  d_m[2][0]=-SinPhi*a1[0]+CosPhi*a2[0];
  d_m[2][1]=-SinPhi*a1[1]+CosPhi*a2[1];
  d_m[2][2]=-SinPhi*a1[2]+CosPhi*a2[2];
  d_m[2][3]=-SinPhi*a1[3]+CosPhi*a2[3];
}


/*!
 *
 */
void
Math3d::M4x4::loadRotateY(double phi)
{
  double SinPhi,CosPhi;

  zero();
  SinPhi=sin(phi);
  CosPhi=cos(phi);
  d_m[0][0]=CosPhi;
  d_m[2][0]=SinPhi;
  d_m[1][1]=1.0;
  d_m[0][2]=-SinPhi;
  d_m[2][2]=CosPhi;
  d_m[3][3]=1.0;
}


/*!
 *
 */
void
Math3d::M4x4::rotateY(double phi)
{
  double SinPhi,CosPhi;
  double a0[4],a2[4];

  SinPhi=sin(phi);
  CosPhi=cos(phi);
  memcpy(a0,d_m[0],4*sizeof(d_m[0][0]));
  memcpy(a2,d_m[2],4*sizeof(d_m[2][0]));
  d_m[0][0]=CosPhi*a0[0]-SinPhi*a2[0];
  d_m[0][1]=CosPhi*a0[1]-SinPhi*a2[1];
  d_m[0][2]=CosPhi*a0[2]-SinPhi*a2[2];
  d_m[0][3]=CosPhi*a0[3]-SinPhi*a2[3];
  d_m[2][0]=SinPhi*a0[0]+CosPhi*a2[0];
  d_m[2][1]=SinPhi*a0[1]+CosPhi*a2[1];
  d_m[2][2]=SinPhi*a0[2]+CosPhi*a2[2];
  d_m[2][3]=SinPhi*a0[3]+CosPhi*a2[3];
}


/*!
 *
 */
void
Math3d::M4x4::loadRotateZ(double phi)
{
  double SinPhi,CosPhi;

  zero();
  SinPhi=sin(phi);
  CosPhi=cos(phi);
  d_m[0][0]=CosPhi;
  d_m[1][0]=-SinPhi;
  d_m[0][1]=SinPhi;;
  d_m[1][1]=CosPhi;
  d_m[2][2]=1.0;
  d_m[3][3]=1.0;
}


/*!
 *
 */
void
Math3d::M4x4::rotateZ(double phi)
{
  double SinPhi,CosPhi;
  double a0[4],a1[4];
  
  SinPhi=sin(phi);
  CosPhi=cos(phi);
  memcpy(a0,d_m[0],4*sizeof(d_m[0][0]));
  memcpy(a1,d_m[1],4*sizeof(d_m[1][0]));
  d_m[0][0]=CosPhi*a0[0]+SinPhi*a1[0];
  d_m[0][1]=CosPhi*a0[1]+SinPhi*a1[1];
  d_m[0][2]=CosPhi*a0[2]+SinPhi*a1[2];
  d_m[0][3]=CosPhi*a0[3]+SinPhi*a1[3];
  d_m[1][0]=-SinPhi*a0[0]+CosPhi*a1[0];
  d_m[1][1]=-SinPhi*a0[1]+CosPhi*a1[1];
  d_m[1][2]=-SinPhi*a0[2]+CosPhi*a1[2];
  d_m[1][3]=-SinPhi*a0[3]+CosPhi*a1[3];
}


/*!
 *
 */
void
Math3d::M4x4::loadRotate(const MQuat& q)
{
  double s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz,l;

  l=q.d_v[0]*q.d_v[0] + q.d_v[1]*q.d_v[1] + q.d_v[2]*q.d_v[2] + q.d_v[3]*q.d_v[3];
  if (fabs(l)<EPSILON) {
    s=1.0f;
  }
  else {
    s=2.0f/l;
  }

  xs = q.d_v[0] * s;   ys = q.d_v[1] * s;  zs = q.d_v[2] * s;
  wx = q.d_v[3] * xs;  wy = q.d_v[3] * ys; wz = q.d_v[3] * zs;
  xx = q.d_v[0] * xs;  xy = q.d_v[0] * ys; xz = q.d_v[0] * zs;
  yy = q.d_v[1] * ys;  yz = q.d_v[1] * zs; zz = q.d_v[2] * zs;

  d_m[0][0]=1.0 - (yy +zz);
  d_m[1][0]=xy - wz;
  d_m[2][0]=xz + wy;
  d_m[0][1]=xy + wz;
  d_m[1][1]=1.0 - (xx +zz);
  d_m[2][1]=yz - wx;
  d_m[0][2]=xz - wy;
  d_m[1][2]=yz + wx;
  d_m[2][2]=1.0 - (xx + yy);
  d_m[3][0]=d_m[3][1]=d_m[3][2]=d_m[0][3]=d_m[1][3]=d_m[2][3]=0.0;
  d_m[3][3]=1.0;
}


/*!
 *
 */
void
Math3d::M4x4::loadRotate(const M3d& axis, double phi)
{
  MQuat q;
  q.set(axis,phi);
  loadRotate(q);
}


/*!
 *
 */
void
Math3d::M4x4::loadRotate(const MRot& rot)
{
  MQuat q;
  q.set(rot.axis(), rot.angle());
  loadRotate(q);
}


/*!
 *
 */
void
Math3d::M4x4::rotate(const MQuat& q)
{
  double s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz,l;
  M4x4 M;

  l=q.d_v[0]*q.d_v[0] + q.d_v[1]*q.d_v[1] + q.d_v[2]*q.d_v[2] + q.d_v[3]*q.d_v[3];
  if (fabs(l)<EPSILON) {
    s=1.0f;
  }
  else {
    s=2.0f/l;
  }

  xs = q.d_v[0] * s;   ys = q.d_v[1] * s;  zs = q.d_v[2] * s;
  wx = q.d_v[3] * xs;  wy = q.d_v[3] * ys; wz = q.d_v[3] * zs;
  xx = q.d_v[0] * xs;  xy = q.d_v[0] * ys; xz = q.d_v[0] * zs;
  yy = q.d_v[1] * ys;  yz = q.d_v[1] * zs; zz = q.d_v[2] * zs;

  M[0][0]=1.0f - (yy +zz);
  M[1][0]=xy - wz;
  M[2][0]=xz + wy;
  M[0][1]=xy + wz;
  M[1][1]=1.0f - (xx +zz);
  M[2][1]=yz - wx;
  M[0][2]=xz - wy;
  M[1][2]=yz + wx;
  M[2][2]=1.0f - (xx + yy);
  M[3][0]=M[3][1]=M[3][2]=M[0][3]=M[1][3]=M[2][3]=0.0f;
  M[3][3]=1.0f;
  operator*=(M);
}


/*!
 *
 */
void
Math3d::M4x4::rotate(const M3d& axis, double phi)
{
  MQuat q;
  q.set(axis,phi);
  rotate(q);
}


/*!
 *
 */
void
Math3d::M4x4::rotate(const MRot& rot)
{
  MQuat q;
  q.set(rot.axis(), rot.angle());
  rotate(q);
}


/*!
 *
 */
void
Math3d::M4x4::loadProj(double fov, double clipNear, double clipFar, double aspect)
{
  double w=clipNear*tan(fov/2);
  double h=w/aspect;

  zero();
  d_m[0][0]=2.0*clipNear/w;
  d_m[1][1]=2.0*clipNear/h;
  d_m[2][2]=-(clipFar+clipNear)/(clipFar-clipNear);
  d_m[2][3]=-2.0*clipFar*clipNear/(clipFar-clipNear);
  d_m[3][2]=-1.0;
}


/*!
 *
 */
void
Math3d::M4x4::proj(double fov, double clipNear, double clipFar, double aspect)
{
  M4x4 M;
  M.loadProj(fov,clipNear,clipFar,aspect);
  operator*=(M);
}


/*!
 * Load picking matrix.
 */
void
Math3d::M4x4::loadPick(const MRect& rect, const MRect& viewport)
{
  identity();
  d_m[0][0]=viewport.width() / rect.width();  
  d_m[1][1]=viewport.height() / rect.height();   
  d_m[3][0]=(viewport.width() + 2.0 * (viewport.x() - rect.x())) / rect.width();
  d_m[3][1]=(viewport.height() + 2.0 * (viewport.y() - rect.y())) / rect.height();
}


/*!
 * Multiply with picking matrix.
 */
void
Math3d::M4x4::pick(const MRect& rect, const MRect& viewport)
{
  M4x4 M;
  M.loadPick(rect,viewport);
  operator*=(M);
}


/*!
 *
 */
Math3d::M3d
Math3d::M4x4::getRow(int row) const
{
  ASSERT((row>=0) && (row<=4));
  return(M3d(
    d_m[0][row],
    d_m[1][row],
    d_m[2][row]
  ));
}


/*!
 *
 */
Math3d::M3d
Math3d::M4x4::getCol(int col) const
{
  ASSERT((col>=0) && (col<=4));
  return(M3d(
    d_m[col][0],
    d_m[col][1],
    d_m[col][2]
  ));
}


/*!
 *
 */
Math3d::M3d 
Math3d::M4x4::operator*(const M3d& C) const
{
  M3d m;
  m.d_v[0]= d_m[0][0]*C.d_v[0] + d_m[1][0]*C.d_v[1] + d_m[2][0]*C.d_v[2] + d_m[3][0];
  m.d_v[1]= d_m[0][1]*C.d_v[0] + d_m[1][1]*C.d_v[1] + d_m[2][1]*C.d_v[2] + d_m[3][1];
  m.d_v[2]= d_m[0][2]*C.d_v[0] + d_m[1][2]*C.d_v[1] + d_m[2][2]*C.d_v[2] + d_m[3][2];
  return(m);
}


/*!
 *
 */
#if 0
Math3d::M4d 
Math3d::M4x4::operator*(const M4d& C) const
{
  M4d m;
  m.d_v[0]= d_m[0][0]*C.d_v[0] + d_m[1][0]*C.d_v[1] + d_m[2][0]*C.d_v[2] + d_m[3][0]*C.d_v[3];
  m.d_v[1]= d_m[0][1]*C.d_v[0] + d_m[1][1]*C.d_v[1] + d_m[2][1]*C.d_v[2] + d_m[3][1]*C.d_v[3];
  m.d_v[2]= d_m[0][2]*C.d_v[0] + d_m[1][2]*C.d_v[1] + d_m[2][2]*C.d_v[2] + d_m[3][2]*C.d_v[3];
  m.d_v[3]= d_m[0][2]*C.d_v[0] + d_m[1][2]*C.d_v[1] + d_m[2][2]*C.d_v[2] + d_m[3][2]*C.d_v[3];
  return(m);
}
#endif

/*!
 *
 */
void
Math3d::M4x4::transform(M3d& C) const
{
  double x,y,z;

  x=C.d_v[0];
  y=C.d_v[1];
  z=C.d_v[2];
  C.d_v[0]= d_m[0][0]*x + d_m[1][0]*y + d_m[2][0]*z + d_m[3][0];
  C.d_v[1]= d_m[0][1]*x + d_m[1][1]*y + d_m[2][1]*z + d_m[3][1];
  C.d_v[2]= d_m[0][2]*x + d_m[1][2]*y + d_m[2][2]*z + d_m[3][2];
}


/*!
 *
 */
void
Math3d::M4x4::transform(M4d& C) const
{
  double x,y,z,w;

  x=C.d_v[0];
  y=C.d_v[1];
  z=C.d_v[2];
  w=C.d_v[3];
  C.d_v[0]= d_m[0][0]*x + d_m[1][0]*y + d_m[2][0]*z + d_m[3][0]*w;
  C.d_v[1]= d_m[0][1]*x + d_m[1][1]*y + d_m[2][1]*z + d_m[3][1]*w;
  C.d_v[2]= d_m[0][2]*x + d_m[1][2]*y + d_m[2][2]*z + d_m[3][2]*w;
  C.d_v[3]= d_m[0][3]*x + d_m[1][3]*y + d_m[2][3]*z + d_m[3][3]*w;
}


bool 
Math3d::M4x4::operator==(const M4x4& A) const
{
  return(cmp(A));
}


bool 
Math3d::M4x4::operator!=(const M4x4& A) const
{
  return(!cmp(A));
}


/*!
 *
 */
bool
Math3d::M4x4::cmp(const M4x4& A, double epsilon) const
{
  int i,j;
  
  for (i=0; i<4; ++i) {
    for (j=0; j<4; ++j) {
      if (fabs(d_m[i][j]-A.d_m[i][j])>epsilon) return(false);
    }
  }
  return(true);
}


/*!
 *
 */
Math3d::M4x4
operator+(const Math3d::M4x4& A, const Math3d::M4x4& B)
{
  Math3d::M4x4 M;

  M.add(A,B);
  return(Math3d::M4x4(M));
}


/*!
 *
 */
Math3d::M4x4
operator-(const Math3d::M4x4& A, const Math3d::M4x4& B)
{
  Math3d::M4x4 M;

  M.sub(A,B);
  return(Math3d::M4x4(M));
}


/*!
 *
 */
Math3d::M4x4
operator*(const Math3d::M4x4& A, const Math3d::M4x4& B)
{
  Math3d::M4x4 M;

  M.mul(A,B);
  return(Math3d::M4x4(M));
}


/*!
 *
 */
ostream& 
Math3d::operator << (ostream& co, const M4x4& m)
{
  // FIXME:
  // This is maybe not so clever ?!?
  // Any idea?  --jeh
  int i;
  for (i=0; i<4; ++i) {
    co << m[i][0] << " " 
       << m[i][1] << " " 
       << m[i][2] << " " 
       << m[i][3] << std::endl;
  }
  return co;
}