/*
 ******************************************************************************
 * Title: Matrix4f
 * Project: ColDetection Library
 ******************************************************************************
 * File: Matrix4f.h
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-01-08
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * 4x4 Matrices
 ******************************************************************************
 * Copyright (c) 2003, INRIA CYBERMOVE
 *
 * This library 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 library 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 library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 ******************************************************************************
 */

#ifndef MATRIX4F_H
#define MATRIX4F_H

#include "tools.h"
#include <iostream>
// #define ZERO 0
// #define ONE 1
/*
 ******************************************************************************
 * 4x4 matrices
 *
 *  Transformation matrix is in column major order and used to multiply
 *  column vector on the left as in OpenGL (ie  v' = M.v).
 *  Coordinate system and rotations are assumed to be right-handed.
 *  
 *       x   y   z   w           x   y   z   w
 *     ---------------         ---------------
 *   x | 0   4   8  12       x | r.s r.s r.s t
 *   y | 1   5   9  13       y | r.s r.s r.s t  (for affine transformations)
 *   z | 2   6  10  14       z | r.s r.s r.s t
 *   w | 3   7  11  15       w | 0   0   0   1
 *  
 *  element (i,j) (raw i, column j) of matrix M is accessed by M[4*j+i]
 *  The second figure is for transformation matrices. It implies that
 *  M = T.R.S, i.e scaling is applied first, then rotation and last
 *  translation.
 ******************************************************************************
 */

// Transformation matrix type
typedef double Matrix4f[16];

// Identity matrix
static const Matrix4f MATRIX4F_IDENTITY = {
  ONE,  ZERO, ZERO, ZERO,
  ZERO, ONE,  ZERO, ZERO,
  ZERO, ZERO, ONE,  ZERO,
  ZERO, ZERO, ZERO, ONE
};

// Null matrix
static const Matrix4f MATRIX4F_ZERO = {
  ZERO, ZERO, ZERO, ZERO,
  ZERO, ZERO, ZERO, ZERO,
  ZERO, ZERO, ZERO, ZERO,
  ZERO, ZERO, ZERO, ZERO
};

/**
 * Print matrix on standard output
 */
 inline void matrix4fPrint(const Matrix4f m)
{
	 std::cout << m[0] << "\t" << m[4] << "\t" << m[8]  << "\t" << m[12] << std::endl
		<< m[1] << "\t" << m[5] << "\t" << m[9]  << "\t" << m[13] << std::endl
		<< m[2] << "\t" << m[6] << "\t" << m[10] << "\t" << m[14] << std::endl
		<< m[3] << "\t" << m[7] << "\t" << m[11] << "\t" << m[15] << std::endl;
}

/**
 * Matrix Copy
 */
inline void matrix4fCopy(Matrix4f dest, const Matrix4f orig)
{
  dest[0] = orig[0]; dest[4] = orig[4];
  dest[8] = orig[8]; dest[12] = orig[12];
  dest[1] = orig[1]; dest[5] = orig[5];
  dest[9] = orig[9]; dest[13] = orig[13];
  dest[2] = orig[2]; dest[6] = orig[6];
  dest[10] = orig[10]; dest[14] = orig[14];
  dest[3] = orig[3]; dest[7] = orig[7];
  dest[11] = orig[11]; dest[15] = orig[15];
}

/** 
 * Cheap matrix inversion for transformation matrices. This will NOT
 * work for a "real" 4x4 matrix.
 */
inline void matrix4fInverse(Matrix4f dest, const Matrix4f orig)
{
  // rotation values, transposed matrix
  dest[0] = orig[0]; dest[4] = orig[1]; dest[8]  = orig[2];
  dest[1] = orig[4]; dest[5] = orig[5]; dest[9]  = orig[6];
  dest[2] = orig[8]; dest[6] = orig[9]; dest[10] = orig[10];

  // translation, transposed rotation applied to opposed translation
  dest[12] = - orig[0] * orig[12] - orig[1] * orig[13] - orig[2]  * orig[14];
  dest[13] = - orig[4] * orig[12] - orig[5] * orig[13] - orig[6]  * orig[14];
  dest[14] = - orig[8] * orig[12] - orig[9] * orig[13] - orig[10] * orig[14];
  
  // this is the end...
  dest[3] = dest[7] = dest[11] = ZERO;
  dest[15] = ONE / orig[15];
}

/**
 * Vector * Matrix product: U := MV 
 * Note: U and V should be 2 distinct variables
 */
inline void matrix4fXformvtkVector3f(vtkVector3f& U,
				  const Matrix4f M,
				  const vtkVector3f& V)
{
  U.x = M[0] * V.x + M[4] * V.y + M[8]  * V.z + M[12];
  U.y = M[1] * V.x + M[5] * V.y + M[9]  * V.z + M[13];
  U.z = M[2] * V.x + M[6] * V.y + M[10] * V.z + M[14];
}

/**
 * Rotate a vector
 * Can be used to transform a vector (normal, vlocity, force...)
 * Note: U and V should be 2 distinct variables
 */
inline void matrix4fRotatevtkVector3f(vtkVector3f& U,
				   const Matrix4f M,
				   const vtkVector3f& V)
{
  U.x = M[0] * V.x + M[4] * V.y + M[8]  * V.z;
  U.y = M[1] * V.x + M[5] * V.y + M[9]  * V.z;
  U.z = M[2] * V.x + M[6] * V.y + M[10] * V.z;
}
  
/**
 * Compute the transformation by the matrix M of the vector V
 */
inline void transform(vtkVector3f& U,
		      const Matrix4f M)
{
  vtkVector3f tmp;
  tmp.x = M[0] * U.x + M[4] * U.y + M[8]  * U.z + M[12];
  tmp.y = M[1] * U.x + M[5] * U.y + M[9]  * U.z + M[13];
  tmp.z = M[2] * U.x + M[6] * U.y + M[10] * U.z + M[14];
  U = tmp;
}

#endif /* ifndef MATRIX4F_H */

/* Matrix4f.h ends here */