#include "Matrix4f.h"
/**
 * 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;
// }