/*=========================================================================
  This code provided as part of 
  Brooks, R., Arbel, T. "Improving the OrientedImage class for use in the
  Registration Framework", Insight Journal, October, 2007

  Author: Rupert Brooks
  Institution: Centre for Intelligent Machines, McGill University

  This class based on itkOrientedImage

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

#ifndef __itkFastOrientedImage_txx
#define __itkFastOrientedImage_txx

#include "itkFastOrientedImage.h"

namespace itk
{

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetIndexFunctions() {
		m_DiagonalDirections=true;
		m_UnitDirections=true;
		m_ZeroOrigin=true;
    for (unsigned int i=0; i < VImageDimension; i++)
      {
		  if(this->m_Origin[i]!=0) { m_ZeroOrigin=false; }
		  if(this->m_IndexToPhysicalPoint[i][i]!=1) { m_UnitDirections=false; }
    for (unsigned int j=0; j < VImageDimension; j++)
      {
		  if(i!=j &&this->m_IndexToPhysicalPoint[i][j]!=0) { m_DiagonalDirections=false; m_UnitDirections=false;}
	  }
	  }
	  if(m_UnitDirections) {
		  m_SelectedTransformImageGradientToSpatialGradientFunction=
			  &Self::TransformImageGradientToSpatialGradientUD;
		  if(m_ZeroOrigin) {
			  //UDZO
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointUDZO;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointUDZO;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexUDZO;
		  } else {
			  //UD
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointUD;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointUD;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexUD;
		  }
	  } else if(m_DiagonalDirections) {
		  m_SelectedTransformImageGradientToSpatialGradientFunction=
			  &Self::TransformImageGradientToSpatialGradientDD;
		  if(m_ZeroOrigin) {
			  //DDZO
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointDDZO;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointDDZO;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexDDZO;
		  } else {
			  //DD
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointDD;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointDD;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexDD;
		  }
	  } else {
		  m_SelectedTransformImageGradientToSpatialGradientFunction=
			  &Self::TransformImageGradientToSpatialGradientGeneric;
		  if(m_ZeroOrigin) {
			  //ZO
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointZO;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointZO;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexZO;
		  } else {
			  //Generic
	  m_SelectedTransformIndexToPhysicalPointFunction=
		  &Self::TransformIndexToPhysicalPointGeneric;
	  m_SelectedTransformContinuousIndexToPhysicalPointFunction=
		  &Self::TransformContinuousIndexToPhysicalPointGeneric;
	  m_SelectedTransformPhysicalPointToContinuousIndexFunction=
		  &Self::TransformPhysicalPointToContinuousIndexGeneric;
		  }
	  }
	}

    /** Set the spacing of the image and precompute the transforms for
   * the image. */
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetOrigin (const PointType origin)
    {
    Superclass::SetOrigin(origin);
	SetIndexFunctions();
	}
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetOrigin (const double origin[VImageDimension])
    {
    Superclass::SetOrigin(origin);
	SetIndexFunctions();
	}
  template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetOrigin (const float origin[VImageDimension])
    {
    Superclass::SetOrigin(origin);
	SetIndexFunctions();
	}
  template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetSpacing (const SpacingType spacing)
    {
    Superclass::SetSpacing(spacing);

    DirectionType scale;
    for (unsigned int i=0; i < VImageDimension; i++)
      {
      scale[i][i] = this->m_Spacing[i];
      }
    m_IndexToPhysicalPoint = this->m_Direction * scale;
    m_PhysicalPointToIndex = m_IndexToPhysicalPoint.GetInverse();
	SetIndexFunctions();

    }

  template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetSpacing (const double spacing[VImageDimension])
    {
    Superclass::SetSpacing(spacing);

    DirectionType scale;
    for (unsigned int i=0; i < VImageDimension; i++)
      {
      scale[i][i] = this->m_Spacing[i];
      }
    m_IndexToPhysicalPoint = this->m_Direction * scale;
    m_PhysicalPointToIndex = m_IndexToPhysicalPoint.GetInverse();
	SetIndexFunctions();
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetSpacing (const float spacing[VImageDimension])
    {
    Superclass::SetSpacing(spacing);

    DirectionType scale;
    for (unsigned int i=0; i < VImageDimension; i++)
      {
      scale[i][i] = this->m_Spacing[i];
      }
    m_IndexToPhysicalPoint = this->m_Direction * scale;
    m_PhysicalPointToIndex = m_IndexToPhysicalPoint.GetInverse();
	SetIndexFunctions();
    }

  /** Set the direction of the image and precompute the transforms for
   * the image. */
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::SetDirection (const DirectionType direction)
    {
    Superclass::SetDirection(direction);

    DirectionType scale;
    for (unsigned int i=0; i < VImageDimension; i++)
      {
      scale[i][i] = this->m_Spacing[i];
      }
    m_IndexToPhysicalPoint = this->m_Direction * scale;
    m_PhysicalPointToIndex = m_IndexToPhysicalPoint.GetInverse();
    m_DirectionInverse = this->m_Direction.GetInverse(); //This ought to be just the transpose
														 // but lets keep it general
	SetIndexFunctions();
    }

template <class TPixel, unsigned int VImageDimension>
typename FastOrientedImage<TPixel,VImageDimension>::GradientPixelType 
FastOrientedImage<TPixel,VImageDimension>::TransformImageGradientToSpatialGradientGeneric(const GradientPixelType & din) const {
#ifdef PRINTFUNCS
		std::cout<<"TIGTSG Generic"<<std::endl;
#endif
	GradientPixelType output;
	// we premultiply a horizontal vector onto the matrix
	for(unsigned int i=0;i<VImageDimension;i++) {
		output[i]=this->m_DirectionInverse[0][i]*din[0];
		for(unsigned int j=1;j<VImageDimension;j++) {
		output[i]+=this->m_DirectionInverse[j][i]*din[j];
		}
	}
	return output;
	}

template <class TPixel, unsigned int VImageDimension>
typename FastOrientedImage<TPixel,VImageDimension>::GradientPixelType 
FastOrientedImage<TPixel,VImageDimension>::TransformImageGradientToSpatialGradientUD(const GradientPixelType & din) const {
#ifdef PRINTFUNCS
		std::cout<<"TIGTSG UD"<<std::endl;
#endif
	return din;
	}

template <class TPixel, unsigned int VImageDimension>
typename FastOrientedImage<TPixel,VImageDimension>::GradientPixelType 
FastOrientedImage<TPixel,VImageDimension>::TransformImageGradientToSpatialGradientDD(const GradientPixelType & din) const {
#ifdef PRINTFUNCS
		std::cout<<"TIGTSG DD"<<std::endl;
#endif
	GradientPixelType output;
	for(unsigned int i=0;i<VImageDimension;i++) {
		output[i]=this->m_Direction[i][i]*din[i];
	}
	return output;
	}

 
// In the following, numerous loops are partially unrolled for performance reasons.
// The pattern is as follows for a 2 dimensional loop.  For simpler loops, the
// corresponding parts can be dropped
//
// If dimension larger than 0
//    Perform loop elements for [0,0]
//    If dimension larger than 1
//       Perform loop elements for [0,1],[1,0]  (leftovers)
//       Perform loop elements for [1,1]        (main part)
//       If dimension larger than 2
//          Perform loop elements for [0,1],[0,2],[2,0],[2,1] (leftovers)
//          Perform loop elements for [2,2] (main part)
//          If dimension larger than 3
//              Need loops to handle both "leftovers" and "mainpart"

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexGeneric(TPPTCIARGS ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI Generic"<<std::endl;
#endif
		double temp[VImageDimension];
		if (VImageDimension>0) {
			temp[0]=point[0]-this->m_Origin[0];
			index[0]=m_PhysicalPointToIndex[0][0]*(temp[0]);
			if (VImageDimension>1) {
				temp[1]=point[1]-this->m_Origin[1];
				index[0]+=m_PhysicalPointToIndex[0][1]*(temp[1]);
				index[1]=m_PhysicalPointToIndex[1][0]*(temp[0]);
				index[1]+=m_PhysicalPointToIndex[1][1]*(temp[1]);
				if (VImageDimension > 2) {
					temp[2]=point[2]-this->m_Origin[2];
					index[0]+=m_PhysicalPointToIndex[0][2]*temp[2];
					index[1]+=m_PhysicalPointToIndex[1][2]*temp[2];
					index[2]=m_PhysicalPointToIndex[2][0]*temp[0];
					index[2]+=m_PhysicalPointToIndex[2][1]*temp[1];
					index[2]+=m_PhysicalPointToIndex[2][2]*temp[2];
					if (VImageDimension > 3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							index[i]=0.0;
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								index[i]+=m_PhysicalPointToIndex[i][j]*(point[j]-this->m_Origin[j]);
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								index[i]+=m_PhysicalPointToIndex[i][j]*(point[j]-this->m_Origin[j]);
							}
						}
					}
				}
			}
		}

	}

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexZO(TPPTCIARGS  ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI ZO"<<std::endl;
#endif
		if (VImageDimension>0) {
			index[0]=m_PhysicalPointToIndex[0][0]*(point[0]);
			if (VImageDimension>1) {
				index[0]+=m_PhysicalPointToIndex[0][1]*(point[1]);
				index[1]=m_PhysicalPointToIndex[1][0]*(point[0]);
				index[1]+=m_PhysicalPointToIndex[1][1]*(point[1]);
				if (VImageDimension > 2){
					index[0]+=m_PhysicalPointToIndex[0][2]*point[2];
					index[1]+=m_PhysicalPointToIndex[1][2]*point[2];
					index[2]=m_PhysicalPointToIndex[2][0]*point[0];
					index[2]+=m_PhysicalPointToIndex[2][1]*point[1];
					index[2]+=m_PhysicalPointToIndex[2][2]*point[2];
					if (VImageDimension > 3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							index[i]=0.0;
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								index[i]+=m_PhysicalPointToIndex[i][j]*(point[j]);
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								index[i]+=m_PhysicalPointToIndex[i][j]*(point[j]);
							}
						}
					}
				}
			}
		}



    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexDD(TPPTCIARGS ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI DD"<<std::endl;
#endif
		if(VImageDimension>0) {
			index[0]=m_PhysicalPointToIndex[0][0]*(point[0]-this->m_Origin[0]);
			if(VImageDimension>1) {
				index[1]=m_PhysicalPointToIndex[1][1]*(point[1]-this->m_Origin[1]);
				if(VImageDimension>2) {
					index[2]=m_PhysicalPointToIndex[2][2]*(point[2]-this->m_Origin[2]);
					if(VImageDimension>3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)      {
							index[i] = m_PhysicalPointToIndex[i][i]*(point[i]-this->m_Origin[i]);
						}
					}
				}
			}
		}

    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexUD(TPPTCIARGS ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI UD"<<std::endl;
#endif
/*    for (unsigned int i = 0 ; i < VImageDimension ; i++)
      {
      index[i] = static_cast<double>(point[i]-this->m_Origin[i]);
      }
*/
		if(VImageDimension>0) {
			index[0]=point[0]-this->m_Origin[0];
			if(VImageDimension>1) {
				index[1]=point[1]-this->m_Origin[1];
				if(VImageDimension>2) {
					index[2]=point[2]-this->m_Origin[2];
					if(VImageDimension>3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)      {
							index[i] = point[i]-this->m_Origin[i];
						}
					}
				}
			}
		}


    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexDDZO(TPPTCIARGS  ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI DDZO"<<std::endl;
#endif
 /*   for (unsigned int i = 0 ; i < VImageDimension ; i++)
      {
      index[i] = static_cast<double>(m_PhysicalPointToIndex[i][i]*point[i]);
      }
	  */
		if(VImageDimension>0) {
			index[0]=m_PhysicalPointToIndex[0][0]*(point[0]);
			if(VImageDimension>1) {
				index[1]=m_PhysicalPointToIndex[1][1]*(point[1]);
				if(VImageDimension>2) {
					index[2]=m_PhysicalPointToIndex[2][2]*(point[2]);
					if(VImageDimension>3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)      {
							index[i] = m_PhysicalPointToIndex[i][i]*(point[i]);
						}
					}
				}
			}
		}

    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformPhysicalPointToContinuousIndexUDZO(TPPTCIARGS ) const
    {
#ifdef PRINTFUNCS
		std::cout<<"TPPTCI UDZO"<<std::endl;
#endif
		if(VImageDimension>0) {
			index[0]=point[0];
			if(VImageDimension>1) {
				index[1]=point[1];
				if(VImageDimension>2) {
					index[2]=point[2];
					if(VImageDimension>3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)      {
							index[i] = point[i];
						}
					}
				}
			}
		}


    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointGeneric(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP Generic"<<std::endl;
#endif
		if (VImageDimension>0) {
			point[0]=this->m_Origin[0];
			point[0]+=m_IndexToPhysicalPoint[0][0]*(index[0]);
			if (VImageDimension>1) {
				point[1]=this->m_Origin[1];
				point[0]+=m_IndexToPhysicalPoint[0][1]*(index[1]);
				point[1]+=m_IndexToPhysicalPoint[1][0]*(index[0]);
				point[1]+=m_IndexToPhysicalPoint[1][1]*(index[1]);
				if (VImageDimension > 2) {
					point[2]=this->m_Origin[2];
					point[0]+=m_IndexToPhysicalPoint[0][2]*index[2];
					point[1]+=m_IndexToPhysicalPoint[1][2]*index[2];
					point[2]+=m_IndexToPhysicalPoint[2][0]*index[0];
					point[2]+=m_IndexToPhysicalPoint[2][1]*index[1];
					point[2]+=m_IndexToPhysicalPoint[2][2]*index[2];
					if (VImageDimension > 3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							point[i]=this->m_Origin[i];
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(index[j]);
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(index[j]);
							}
						}
					}
				}
			}
		}

  
  
  
  
  }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointZO(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP ZO"<<std::endl;
#endif
		if (VImageDimension>0) {
			point[0]=m_IndexToPhysicalPoint[0][0]*(index[0]);
			if (VImageDimension>1) {
				point[0]+=m_IndexToPhysicalPoint[0][1]*(index[1]);
				point[1]=m_IndexToPhysicalPoint[1][0]*(index[0]);
				point[1]+=m_IndexToPhysicalPoint[1][1]*(index[1]);
				if (VImageDimension > 2) {
					point[0]+=m_IndexToPhysicalPoint[0][2]*index[2];
					point[1]+=m_IndexToPhysicalPoint[1][2]*index[2];
					point[2]=m_IndexToPhysicalPoint[2][0]*index[0];
					point[2]+=m_IndexToPhysicalPoint[2][1]*index[1];
					point[2]+=m_IndexToPhysicalPoint[2][2]*index[2];
					if (VImageDimension > 3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							point[i]=0.0;
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(index[j]);
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(index[j]);
							}
						}
					}
				}
			}
		}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointDDZO(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP DDZO"<<std::endl;
#endif
	if(VImageDimension>0) {
		point[0]=m_IndexToPhysicalPoint[0][0]*index[0];
		if(VImageDimension>1) {
			point[1]=m_IndexToPhysicalPoint[1][1]*index[1];
			if(VImageDimension>2) {
				point[2]=m_IndexToPhysicalPoint[2][2]*index[2];
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = m_IndexToPhysicalPoint[i][i] * index[i];
					}
				}
			}
		}
	}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointDD(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP DD"<<std::endl;
#endif
	if(VImageDimension>0) {
		point[0]=this->m_Origin[0] + m_IndexToPhysicalPoint[0][0]*index[0];
		if(VImageDimension>1) {
			point[1]=this->m_Origin[1] + m_IndexToPhysicalPoint[1][1]*index[1];
			if(VImageDimension>2) {
				point[2]=this->m_Origin[2] + m_IndexToPhysicalPoint[2][2]*index[2];
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = this->m_Origin[i] + m_IndexToPhysicalPoint[i][i] * index[i];
					}
				}
			}
		}
	}
    }
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointUDZO(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP UDZO"<<std::endl;
#endif
	if(VImageDimension>0) {
		point[0]=index[0];
		if(VImageDimension>1) {
			point[1]=index[1];
			if(VImageDimension>2) {
				point[2]=index[2];
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = index[i];
					}
				}
			}
		}
	}
    }
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformContinuousIndexToPhysicalPointUD(TCITPPARGS) const    {
#ifdef PRINTFUNCS
	std::cout<<"TCITPP UD"<<std::endl;
#endif
 	if(VImageDimension>0) {
		point[0]=this->m_Origin[0] + index[0];
		if(VImageDimension>1) {
			point[1]=this->m_Origin[1] + index[1];
			if(VImageDimension>2) {
				point[2]=this->m_Origin[2] + index[2];
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = this->m_Origin[i] + index[i];
					}
				}
			}
		}
	}
    }

 
template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointGeneric(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP Generic"<<std::endl;
#endif

			if (VImageDimension>0) {
			point[0]=this->m_Origin[0];
			point[0]+=m_IndexToPhysicalPoint[0][0]*(index[0]);
			if (VImageDimension>1) {
				point[1]=this->m_Origin[1];
				point[0]+=m_IndexToPhysicalPoint[0][1]*(index[1]);
				point[1]+=m_IndexToPhysicalPoint[1][0]*(index[0]);
				point[1]+=m_IndexToPhysicalPoint[1][1]*(index[1]);
				if (VImageDimension > 2) {
					point[2]=this->m_Origin[2];
					point[0]+=m_IndexToPhysicalPoint[0][2]*index[2];
					point[1]+=m_IndexToPhysicalPoint[1][2]*index[2];
					point[2]+=m_IndexToPhysicalPoint[2][0]*index[0];
					point[2]+=m_IndexToPhysicalPoint[2][1]*index[1];
					point[2]+=m_IndexToPhysicalPoint[2][2]*index[2];
					if (VImageDimension > 3) {
						for (unsigned int j = 3 ; j < VImageDimension ; j++)
						{
							point[j]=this->m_Origin[j];
						}
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*index[j];
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*index[j];
							}
						}
					}
				}
			}
		}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointZO(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP ZO"<<std::endl;
#endif

		if (VImageDimension>0) {
			point[0]=m_IndexToPhysicalPoint[0][0]*(static_cast<double>(index[0]));
			if (VImageDimension>1) {
				point[0]+=m_IndexToPhysicalPoint[0][1]*(static_cast<double>(index[1]));
				point[1]=m_IndexToPhysicalPoint[1][0]*(static_cast<double>(index[0]));
				point[1]+=m_IndexToPhysicalPoint[1][1]*(static_cast<double>(index[1]));
				if (VImageDimension > 2) {
					point[0]+=m_IndexToPhysicalPoint[0][2]*static_cast<double>(index[2]);
					point[1]+=m_IndexToPhysicalPoint[1][2]*static_cast<double>(index[2]);
					point[2]=m_IndexToPhysicalPoint[2][0]*static_cast<double>(index[0]);
					point[2]+=m_IndexToPhysicalPoint[2][1]*static_cast<double>(index[1]);
					point[2]+=m_IndexToPhysicalPoint[2][2]*static_cast<double>(index[2]);
					if (VImageDimension > 3) {
						for (unsigned int i = 3 ; i < VImageDimension ; i++)
						{
							point[i]=0.0;
							for (unsigned int j = 0 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(static_cast<double>(index[j]));
							}
						}

						for (unsigned int i = 0 ; i < 3 ; i++)
						{
							for (unsigned int j = 3 ; j < VImageDimension ; j++)
							{
								point[i]+=m_IndexToPhysicalPoint[i][j]*(static_cast<double>(index[j]));
							}
						}
					}
				}
			}
		}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointDDZO(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP DDZO"<<std::endl;
#endif

			if(VImageDimension>0) {
		point[0]=m_IndexToPhysicalPoint[0][0]*static_cast<double>(index[0]);
		if(VImageDimension>1) {
			point[1]=m_IndexToPhysicalPoint[1][1]*static_cast<double>(index[1]);
			if(VImageDimension>2) {
				point[2]=m_IndexToPhysicalPoint[2][2]*static_cast<double>(index[2]);
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = m_IndexToPhysicalPoint[i][i] * static_cast<double>(index[i]);
					}
				}
			}
		}
	}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointUDZO(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP UDZO"<<std::endl;
#endif
			if(VImageDimension>0) {
		point[0]=static_cast<double>(index[0]);
		if(VImageDimension>1) {
			point[1]=static_cast<double>(index[1]);
			if(VImageDimension>2) {
				point[2]=static_cast<double>(index[2]);
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = static_cast<double>(index[i]);
					}
				}
			}
		}
	}
    }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointDD(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP DD"<<std::endl;
#endif
		if(VImageDimension>0) {
		point[0]=this->m_Origin[0] + m_IndexToPhysicalPoint[0][0]*static_cast<double>(index[0]);
		if(VImageDimension>1) {
			point[1]=this->m_Origin[1] + m_IndexToPhysicalPoint[1][1]*static_cast<double>(index[1]);
			if(VImageDimension>2) {
				point[2]=this->m_Origin[2] + m_IndexToPhysicalPoint[2][2]*static_cast<double>(index[2]);
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = this->m_Origin[i] + m_IndexToPhysicalPoint[i][i] * static_cast<double>(index[i]);
					}
				}
			}
		}
	}
  }

template <class TPixel, unsigned int VImageDimension>
void FastOrientedImage<TPixel,VImageDimension>::TransformIndexToPhysicalPointUD(TITPPARGS ) const    {
#ifdef PRINTFUNCS
			std::cout<<"TITPP UD"<<std::endl;
#endif
		if(VImageDimension>0) {
		point[0]=this->m_Origin[0] + static_cast<double>(index[0]);
		if(VImageDimension>1) {
			point[1]=this->m_Origin[1] + static_cast<double>(index[1]);
			if(VImageDimension>2) {
				point[2]=this->m_Origin[2] + static_cast<double>(index[2]);
				if(VImageDimension>3) {
				    for (unsigned int i = 3 ; i < VImageDimension ; i++) {
						point[i] = this->m_Origin[i] + static_cast<double>(index[i]);
					}
				}
			}
		}
	}
    }
} // end namespace itk



#endif