path: root/kpovmodeler/pmmatrix.cpp

/*
**************************************************************************
                                 description
                             --------------------
    copyright            : (C) 2000-2001 by Andreas Zehender
    email                : [email protected]
**************************************************************************

**************************************************************************
*                                                                        *
*  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.                                   *
*                                                                        *
**************************************************************************/


#include <math.h>

#include "pmmatrix.h"
#include "pmvector.h"
#include "pmdebug.h"

#include <textstream.h>

PMMatrix::PMMatrix( )
{
   int i;

   for( i = 0; i < 16; i++ )
      m_elements[i] = 0;
}

PMMatrix::~PMMatrix( )
{
}

PMMatrix& PMMatrix::operator= ( const PMMatrix& m )
{
   int i;
   for( i=0; i<16; i++ )
      m_elements[i] = m.m_elements[i];

   return *this;
}

PMMatrix PMMatrix::identity( )
{
   PMMatrix newMatrix;
   int i;

   for( i=0; i<4; i++ )
      newMatrix[i][i] = 1.0;
   
   return newMatrix;
}

PMMatrix PMMatrix::translation( double x, double y, double z )
{
   PMMatrix newMatrix;
   newMatrix[3][0] = x;
   newMatrix[3][1] = y;
   newMatrix[3][2] = z;
   newMatrix[0][0] = 1;
   newMatrix[1][1] = 1;
   newMatrix[2][2] = 1;
   newMatrix[3][3] = 1;

   return newMatrix;
}

PMMatrix PMMatrix::scale( double x, double y, double z )
{
   PMMatrix newMatrix;
   newMatrix[0][0] = x;
   newMatrix[1][1] = y;
   newMatrix[2][2] = z;
   newMatrix[3][3] = 1;

   return newMatrix;
}

PMMatrix PMMatrix::rotation( double x, double y, double z )
{
   PMMatrix newMatrix;
   double sinx, siny, sinz, cosx, cosy, cosz;
   sinx = sin( x );
   siny = sin( y );
   sinz = sin( z );
   cosx = cos( x );
   cosy = cos( y );
   cosz = cos( z );
   
   newMatrix[0][0] = cosz*cosy;
   newMatrix[1][0] = -sinz*cosx + cosz*siny*sinx;
   newMatrix[2][0] = sinz*sinx + cosz*siny*cosx;
   newMatrix[0][1] = sinz*cosy;
   newMatrix[1][1] = cosz*cosx + sinz*siny*sinx;
   newMatrix[2][1] = -cosz*sinx + sinz*siny*cosx;
   newMatrix[0][2] = -siny;
   newMatrix[1][2] = cosy*sinx;
   newMatrix[2][2] = cosy*cosx;
   newMatrix[3][3] = 1;

   return newMatrix;
}

PMMatrix PMMatrix::rotation( const PMVector& n, double a )
{
   PMMatrix result( PMMatrix::identity( ) );
   double rx, ry;

   if( n.size( ) == 3 )
   {
      rx = pmatan( n.y( ), n.z( ) );
      ry = - pmatan( n.x( ), sqrt( n.y( ) * n.y( ) + n.z( ) * n.z( ) ) );

      result = rotation( -rx, 0.0, 0.0 ) * rotation( 0.0, -ry, 0.0 )
         * rotation( rx, ry, a );
         
   }
   else
      kdError( PMArea ) << "Wrong size in PMMatrix::rotation( )\n";
   
   return result;
}

PMMatrix PMMatrix::viewTransformation( const PMVector& eye,
                                       const PMVector& lookAt,
                                       const PMVector& up )
{
   PMMatrix result;
   PMVector x, y, z;
   GLdouble len;
   int i;

   // create rotation matrix
   z = eye - lookAt;
   len = z.abs( );
   if( !approxZero( len ) )
      z /= len;
   
   y = up;
   x = PMVector::cross( y, z );
   y = PMVector::cross( z, x );

   // normalize vectors
   len = x.abs( );
   if( !approxZero( len ) )
      x /= len;

   len = y.abs( );
   if( !approxZero( len ) )
      y /= len;

   for( i = 0; i < 3; i++ )
   {
      result[i][0] = x[i];
      result[i][1] = y[i];
      result[i][2] = z[i];
      result[3][i] = 0.0;
      result[i][3] = 0.0;
   }
   result[3][3] = 1.0;

   // Translate eye to origin
   return result * translation( -eye[0], -eye[1], -eye[2] ); 
}

void PMMatrix::toRotation( double* x, double* y, double* z )
{
   PMMatrix& m = *this;
   
   if( !approx( fabs( m[0][2] ), 1.0 ) )
   {
      double cosy;
      // | m[0][2] | != 1
      // sin(y) != 1.0, cos(y) != 0.0
      *y = asin( - m[0][2] );
      cosy = cos( *y );

      // sign of cosy is important!
      *x = pmatan( m[1][2] / cosy, m[2][2] / cosy );
      *z = pmatan( m[0][1] / cosy, m[0][0] / cosy );
   }
   else if( m[0][2] < 0 )
   {
      // m[0][2] == -1
      // sin(y) == 1, cos(y) == 0
      // z and x are dependent of each other, assume z = 0
      
      double zminusx = pmatan( m[2][1], m[1][1] );

      *y = M_PI_2;
      *z = 0.0;
      *x = - zminusx;
   }
   else
   {
      // m[0][2] == 1
      // sin(y) == -1, cos(y) == 0
      // z and x are dependent of each other, assume z = 0
      
      double zplusx = pmatan( -m[2][1], m[1][1] );

      *y = -M_PI_2;
      *z = 0.0;
      *x = zplusx;
   }
}

PMMatrix PMMatrix::modelviewMatrix( )
{
   PMMatrix result;
   glGetDoublev( GL_MODELVIEW_MATRIX, result.m_elements );
   return result;
}

double PMMatrix::det( ) const
{
   PMMatrix tmp( *this );
   double result = 1.0, help;
   int i, k, e, row;

   // make a upper triangular matrix
   for( i=0; i<4; i++ )
   {
      row = tmp.notNullElementRow( i );
      if( row == -1 )
         return 0;
      if( row != i )
      {
         tmp.exchangeRows( i, row );
         result = -result;
      }

      result *= tmp[i][i];
      for( k=i+1; k<4; k++ )
      {
         help = tmp[i][k];
         for( e=0; e<4; e++ )
            tmp[e][k] -= tmp[e][i] * help/tmp[i][i];
      }
   }
   return result;
}

PMMatrix PMMatrix::inverse( ) const
{
   PMMatrix result( identity( ) );
   PMMatrix tmp( *this );
   int i, k, e, row;
   double a;
   
   // uses the Gauss algorithm
   // row operations to make tmp a identity matrix
   // result matrix is then the inverse
   for( i=0; i<4; i++ )
   {
      row = tmp.notNullElementRow( i );
      if( row == -1 )
         return identity( );
      if( row != i )
      {
         tmp.exchangeRows( i, row );
         result.exchangeRows( i, row );
      }
      // tmp[i][i] != 0

      a = tmp[i][i];
      for( e=0; e<4; e++ )
      {
         result[e][i] /= a;
         tmp[e][i] /= a;
      }
      // tmp[i][i] == 1

      for( k=0; k<4; k++ )
      {
         if( k != i )
         {
            a = tmp[i][k];
            for( e=0; e<4; e++ )
            {
               result[e][k] -= result[e][i] * a;
               tmp[e][k] -= tmp[e][i] * a;
            }
         }
      }
      // tmp[!=i][i] == 0.0
   }
   return result;
}

void PMMatrix::exchangeRows( int r1, int r2 )
{
   GLdouble help;
   int i;

   for( i=0; i<4; i++ )
   {
      help = (*this)[i][r1];
      (*this)[i][r1] = (*this)[i][r2];
      (*this)[i][r2] = help;
   }
}

int PMMatrix::notNullElementRow( const int index ) const
{
   int i, result = -1;
   GLdouble max = 0.0, v;
   
   // choose the row with abs( ) = max
   for( i=index; i<4; i++ )
   {
      v = fabs((*this)[index][i]);
      if( v > max )
      {
         result = i;
         max = v;
      }
   }
   return result;
}

PMMatrix& PMMatrix::operator*= ( const double d )
{
   int i;
   for( i=0; i<16; i++ )
      m_elements[i] *= d;
   return *this;
}

PMMatrix& PMMatrix::operator/= ( const double d )
{
   int i;
   if( approxZero( 0 ) )
      kdError( PMArea ) << "Division by zero in PMMatrix::operator/=" << "\n";
   else
      for( i=0; i<16; i++ )
         m_elements[i] /= d;
   return *this;
}

PMMatrix& PMMatrix::operator*= ( const PMMatrix& m )
{
   *this = *this * m;
   return *this;
}

PMMatrix operator- ( const PMMatrix& m )
{
   PMMatrix result;
   int r,c;
   
   for( r=0; r<4; r++ )
      for( c=0; c<4; c++ )
         result[c][r] = -m[c][r];
   return result;
}

PMMatrix operator* ( const PMMatrix& m1, const PMMatrix& m2 )
{
   PMMatrix result;
   int r, c, i;

   for( r=0; r<4; r++ )
      for( c=0; c<4; c++ )
         for( i=0; i<4; i++ )
            result[c][r] += m1[i][r] * m2[c][i];
   return result;
}

PMMatrix operator* ( const PMMatrix& m1, const double d )
{
   PMMatrix result( m1 );
   result *= d;
   return result;
}

PMMatrix operator/ ( const PMMatrix& m1, const double d )
{
   PMMatrix result( m1 );
   result /= d ;
   return result;
}

PMMatrix operator* ( const double d, const PMMatrix& m1 )
{
   PMMatrix result( m1 );
   result *= d;
   return result;
}

#include <stdio.h>
void PMMatrix::testOutput( )
{
   int r, c;

   printf( "\n" );
   for( r=0; r<4; r++ )
   {
      for( c=0; c<4; c++ )
         printf( "% 20.18f ", (*this)[c][r] );
      printf( "\n" );
   }     
}

TQString PMMatrix::serializeXML( ) const
{
   TQString result;
   TQTextStream str( &result, IO_WriteOnly );
   int i;

   for( i = 0; i < 16; i++ )
   {
      if( i > 0 )
         str << ' ';
      str << m_elements[i];
   }
   
   return result;
}

bool PMMatrix::loadXML( const TQString& str )
{
   int i;
   TQString tmp( str );
   TQTextStream s( &tmp, IO_ReadOnly );
   TQString val;
   bool ok;
   
   for( i = 0; i < 16; i++ )
   {
      s >> val;
      m_elements[i] = val.toDouble( &ok );
      if( !ok )
         return false;
   }
   return true;
}