/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkHomogeneousTransform.txx,v $
  Language:  C++
  Date:      $Date: 2007-03-14 12:37:08 $
  Version:   $Revision: 1.1 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#ifndef _itkHomogeneousTransform_txx
#define _itkHomogeneousTransform_txx

#include "itkNumericTraits.h"
#include "itkHomogeneousTransform.h"
#include "vnl/algo/vnl_matrix_inverse.h"


namespace itk
{

// Constructor with default arguments
template<class TScalarType, unsigned int NDimensions>
HomogeneousTransform<TScalarType, NDimensions>::
HomogeneousTransform():Superclass(SpaceDimension,ParametersDimension)
{
  m_Matrix.SetIdentity();
  m_Center.Fill( 0 );
  m_Singular = false;
  m_InverseMatrix.SetIdentity();
}


// Constructor with default arguments
template<class TScalarType, unsigned int NDimensions>
HomogeneousTransform<TScalarType, NDimensions>::
HomogeneousTransform( unsigned int outputSpaceDimension, 
                 unsigned int parametersDimension   ):
  Superclass(outputSpaceDimension,parametersDimension)
{
  m_Matrix.SetIdentity();
  m_Center.Fill( 0 );
  m_Singular = false;
}



// Constructor with explicit arguments
template<class TScalarType, unsigned int NDimensions>
HomogeneousTransform<TScalarType, NDimensions>::
HomogeneousTransform(const MatrixType &matrix, const OutputPointType &center)
{
  m_Matrix = matrix;
  m_Center = center;
  m_MatrixMTime.Modified();
  this->Modified();
}




// Destructor
template<class TScalarType, unsigned int NDimensions>
HomogeneousTransform<TScalarType, NDimensions>::
~HomogeneousTransform()
{
  return;
}



// Print self
template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
PrintSelf(std::ostream &os, Indent indent) const
{
  Superclass::PrintSelf(os,indent);

  unsigned int i, j;
  
  os << indent << "Matrix: " << std::endl;
  for (i = 0; i < MatrixDimension; i++) 
    {
    os << indent.GetNextIndent();
    for (j = 0; j < MatrixDimension; j++)
      {
      os << m_Matrix[i][j] << " ";
      }
    os << std::endl;
    }

  os << indent << "Center: " << m_Center << std::endl;

  os << indent << "Inverse: " << std::endl;
  for (i = 0; i < MatrixDimension; i++) 
    {
    os << indent.GetNextIndent();
    for (j = 0; j < MatrixDimension; j++)
      {
      os << this->GetInverseMatrix()[i][j] << " ";
      }
    os << std::endl;
    }
  os << indent << "Singular: " << m_Singular << std::endl;

}

// Get the translation part of the matrix
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::OutputVectorType 
HomogeneousTransform<TScalarType, NDimensions>::
GetTranslation() const
{
   OutputVectorType translation;
   for (unsigned int i=0;i<SpaceDimension;i++) {
    	translation[i]=m_Matrix[i][SpaceDimension];
   }
   return translation;
}

// Set the translation part of the matrix
template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
SetTranslation(const OutputVectorType & translation)
{
   for (unsigned int i=0;i<SpaceDimension;i++) {
    	m_Matrix[i][SpaceDimension]=translation[i];
   }
   return;
}

template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
SetCenter(const InputPointType & center)
{
	m_Center=center;
    return;
}


  /** Utility methods for centering transform */
  template<class TScalarType, unsigned int NDimensions>
  typename HomogeneousTransform<TScalarType, NDimensions>::MatrixType HomogeneousTransform<TScalarType, NDimensions>::DeCenter(MatrixType H, OutputPointType C) const
  {
	  MatrixType T,Tinv,Hout;
	  T.SetIdentity();
	  Tinv.SetIdentity();
	  for (unsigned int i=0;i<SpaceDimension;i++) {
		  T[i][SpaceDimension]=-C[i];
		  Tinv[i][SpaceDimension]=C[i];
	  }
	  Hout=Tinv*H*T;
	  Hout=Hout*(1/Hout[SpaceDimension][SpaceDimension]);
	  return Hout;
  }
// with no args do it on this transform
  template<class TScalarType, unsigned int NDimensions>
  typename HomogeneousTransform<TScalarType, NDimensions>::MatrixType 
  HomogeneousTransform<TScalarType, NDimensions>::DeCenter() const
  {
	  return DeCenter(m_Matrix,m_Center);
  }

  template<class TScalarType, unsigned int NDimensions>
  typename HomogeneousTransform<TScalarType, NDimensions>::MatrixType 
  HomogeneousTransform<TScalarType, NDimensions>::ReCenter(MatrixType H, OutputPointType C) const
  {
	  // recentering is just decentering off the negative of the point
	  OutputPointType C2;
	  for (unsigned int i=0;i<SpaceDimension;i++) {
		C2[i]=-C[i];
	  }
	  return DeCenter(H,C2);
  }


// Compose with another homogeneous transformation
template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
Compose(const Self * other, bool pre)
{
	MatrixType Hother,Hself,Hint;
	Hother=other->DeCenter();
	Hself=DeCenter();
  if (pre) 
    {
		Hint=Hself*Hother;
    }
  else 
    {
		Hint=Hother*Hself;
    }
	m_Matrix=ReCenter(Hint,m_Center);
    m_MatrixMTime.Modified();
    this->Modified();
    return;
}

// Compose with another homogeneous transformation
template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
Compose(const MatrixTransformType * other, bool pre)
{
	MatrixType Hother,Hself,Hint;
	AffineMatrixType otherMatrix;
	OutputVectorType otherTranslation;
	otherMatrix=other->GetMatrix();
	otherTranslation=other->GetOffset();
	for (unsigned int i=0;i<SpaceDimension;i++) {
		for (unsigned int j=0;j<SpaceDimension;j++) {
			Hother[i][j]=otherMatrix[i][j];
		}
		Hother[i][SpaceDimension]=otherTranslation[i];
	}
	Hother[SpaceDimension][SpaceDimension]=1;
	Hself=DeCenter();
  if (pre) 
    {
		Hint=Hself*Hother;
    }
  else 
    {
		Hint=Hother*Hself;
    }
	m_Matrix=ReCenter(Hint,m_Center);
    m_MatrixMTime.Modified();
    this->Modified();
    return;
}



// Transform a point
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::OutputPointType
HomogeneousTransform<TScalarType, NDimensions>::
TransformPoint(const InputPointType &point) const 
{
  HomogeneousPointType Hpoint,Hpoint2;
  OutputPointType Opoint;
  for (unsigned int i=0;i<SpaceDimension;i++) {
	  Hpoint[i]=point[i]-m_Center[i];
  }
  Hpoint[SpaceDimension]=1;
  Hpoint2=m_Matrix*Hpoint;
  for (unsigned int i=0;i<SpaceDimension;i++) {
	  Opoint[i]=Hpoint2[i]/Hpoint2[SpaceDimension]+m_Center[i];
  }
  return Opoint;
}




// Back transform a point
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::InputPointType
HomogeneousTransform<TScalarType, NDimensions>::
BackTransform(const OutputPointType &point) const 
{
  HomogeneousPointType Hpoint,Hpoint2;
  OutputPointType Opoint;
  MatrixType iMatrix;
  iMatrix=this->GetInverseMatrix();
  for (unsigned int i=0;i<SpaceDimension;i++) {
	  Hpoint[i]=point[i]-m_Center[i];
  }
  Hpoint[SpaceDimension]=1;
  Hpoint2=iMatrix*Hpoint;
  for (unsigned int i=0;i<SpaceDimension;i++) {
	  Opoint[i]=Hpoint2[i]/Hpoint2[SpaceDimension]+m_Center[i];
  }
  return Opoint;
}

	
// Back transform a given point which is represented as type PointType
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::InputPointType
HomogeneousTransform<TScalarType, NDimensions>::
BackTransformPoint(const OutputPointType &point) const
{
	return BackTransform(point);
}


// return an inverse transformation
template<class TScalarType, unsigned int NDimensions>
bool
HomogeneousTransform<TScalarType, NDimensions>::
GetInverse( Self* inverse) const
{
  if(!inverse)
    {
    return false;
    }

  this->GetInverseMatrix();
  if(m_Singular)
    {
    return false;
    }

  inverse->m_Matrix         = this->GetInverseMatrix();
  inverse->m_InverseMatrix  = m_Matrix;
  inverse->m_Center = m_Center;
  return true;
}

// Compute a distance between two homogeneous transforms
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::ScalarType
HomogeneousTransform<TScalarType, NDimensions>::
Metric(const Self * other) const
{
  ScalarType result = 0.0, term;

  for (unsigned int i = 0; i < MatrixDimension; i++) 
    {
    for (unsigned int j = 0; j < MatrixDimension; j++) 
      {
      term = m_Matrix[i][j] - other->m_Matrix[i][j];
      result += term * term;
      }
	}
  for (unsigned int i = 0; i < SpaceDimension; i++) 
    {
    term = m_Center[i] - other->m_Center[i];
    result += term * term;
    }
  return sqrt(result);
}



// Compute a distance between self and the identity transform
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::ScalarType
HomogeneousTransform<TScalarType, NDimensions>::
Metric(void) const
{
  ScalarType result = 0.0, term;

  for (unsigned int i = 0; i < NDimensions; i++) 
    {
    for (unsigned int j = 0; j < NDimensions; j++) 
      {
      if (i == j)
        {
        term = m_Matrix[i][j] - 1.0;
        }
      else
        {
        term = m_Matrix[i][j];
        }
      result += term * term;
      }
    term = m_Center[i];
    result += term * term;
    }

  return sqrt(result);
}





// Recompute the inverse matrix (internal)
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::MatrixType
HomogeneousTransform<TScalarType, NDimensions>::
GetInverseMatrix( void ) const
{
  // If the transform has been modified we recompute the inverse
  if(m_InverseMatrixMTime != m_MatrixMTime)
    {
    m_Singular = false;
    try 
      {
      m_InverseMatrix  = m_Matrix.GetInverse();
      }
    catch(...) 
      {
      m_Singular = true;
      }
     m_InverseMatrixMTime = m_MatrixMTime;
    }

  return m_InverseMatrix;
}





// Get parameters
template<class TScalarType, unsigned int NDimensions>
const typename HomogeneousTransform<TScalarType, NDimensions>::ParametersType &
HomogeneousTransform<TScalarType, NDimensions>::
GetParameters( void ) const
{

   // Transfer the linear part
  unsigned int par = 0;

  // parameter vector is in this order
  // | 0 1 4 |
  // | 2 3 5 |
  // | 6 7 x |
  
  for(unsigned int row=0; row<NDimensions; row++) 
    {
    for(unsigned int col=0; col<NDimensions; col++) 
      {
      this->m_Parameters[par]=m_Matrix[row][col];
      ++par;
      }
    }

  // Transfer the translation part
  for(unsigned int i=0; i<NDimensions; i++) 
    {
    this->m_Parameters[par]=m_Matrix[i][NDimensions];
    ++par;
    }

  // Transfer the perspective part
  for(unsigned int i=0; i<NDimensions; i++) 
    {
    this->m_Parameters[par] = m_Matrix[NDimensions][i];
    ++par;
    }

  return this->m_Parameters;

}




// Set parameters
template<class TScalarType, unsigned int NDimensions>
void
HomogeneousTransform<TScalarType, NDimensions>::
SetParameters( const ParametersType & parameters )
{

  // Transfer the linear part
  unsigned int par = 0;

  this->m_Parameters = parameters;
  // parameter vector is in this order
  // | 0 1 4 |
  // | 2 3 5 |
  // | 6 7 x |
  
  for(unsigned int row=0; row<NDimensions; row++) 
    {
    for(unsigned int col=0; col<NDimensions; col++) 
      {
      m_Matrix[row][col] = this->m_Parameters[par];
      ++par;
      }
    }

  // Transfer the translation part
  for(unsigned int i=0; i<NDimensions; i++) 
    {
    m_Matrix[i][NDimensions] = this->m_Parameters[par];
    ++par;
    }

  // Transfer the perspective part
  for(unsigned int i=0; i<NDimensions; i++) 
    {
    m_Matrix[NDimensions][i] = this->m_Parameters[par];
    ++par;
    }
 
  // Recompute the inverse
  m_MatrixMTime.Modified();
  this->Modified();
}


// Compute the Jacobian in one position 
template<class TScalarType, unsigned int NDimensions>
const typename HomogeneousTransform<TScalarType, NDimensions>::JacobianType & 
HomogeneousTransform<TScalarType, NDimensions>::
GetJacobian( const InputPointType & p ) const
{
  // jacobian is of size [dim][par]
  this->m_Jacobian.Fill( 0.0 );

  //compute the homogeneous points before and after the transform, minus the center
  HomogeneousPointType Hpoint,Hpoint2;
  for (unsigned int i=0;i<SpaceDimension;i++) {
	  Hpoint[i]=p[i]-m_Center[i];
  }
  Hpoint[SpaceDimension]=1;
  Hpoint2=m_Matrix*Hpoint;

  for(unsigned int par=0;par<SpaceDimension;par++) {
      for(unsigned int dim=0;dim<SpaceDimension;dim++) {
	  m_Jacobian[dim][dim*SpaceDimension+par]=Hpoint[par]/Hpoint2[SpaceDimension];
	  m_Jacobian[dim][MatrixDimension*SpaceDimension+par]=
		  -Hpoint2[dim]*Hpoint[par]/Hpoint2[SpaceDimension]/Hpoint2[SpaceDimension];
	  }
	  m_Jacobian[par][SpaceDimension*SpaceDimension+par]=1/Hpoint2[SpaceDimension];
  }

  return this->m_Jacobian;

}

// Create and return an inverse transformation
template<class TScalarType, unsigned int NDimensions>
typename HomogeneousTransform<TScalarType, NDimensions>::Pointer
HomogeneousTransform<TScalarType, NDimensions>::
Inverse( void ) const
{
  itkWarningMacro("Inverse() is deprecated.  Please use GetInverse() instead.");
  Pointer result           = New();
  result->m_Matrix         = this->GetInverseMatrix();
  result->m_InverseMatrix  = m_Matrix;
  result->m_Center         = this->m_Center;
  result->m_Singular       = false;
  return result;
}



} // namespace

#endif