/*
 * Miscellaneous useful math functions' templates
 */

#ifndef __OGL2_MISCELLANEOUS_MATH__
#define __OGL2_MISCELLANEOUS_MATH__

#include "vector3.h"
#include "glmatrix.h"
#include "quaternion.h"

#ifndef PI
#define PI 3.14159265358979324
#endif

extern "C++" {


/*
 * Converts an angle&axis representation of rotations to corresponding
 * quaternion representation.
 */

template <class __type> inline quaternion<__type>
AngleAxis2Quaternion (__type angle, __type x, __type y, __type z) {
  __type s = (__type)sin(angle/2.0);
  return quaternion<__type> ((__type)cos(angle/2.0), x*s, y*s, z*s);
}

/*
 * Converts euler angles to its corresponding matrix representation, where
 * the result represents a rotation about the x-axis by the angle 'alpha'
 * followed by rotation about the y-axis by the angle 'beta' concluded by
 * rotation about the z-axis by the angle 'gamma'.
 */

template <class __type> inline GLmatrix_template<__type>
Euler2Matrix (__type alpha, __type beta, __type gamma) {
  GLmatrix_template<__type> M;
  __type* m = M();
  __type sin_alpha = (__type)(sin(alpha));
  __type cos_alpha = (__type)(cos(alpha));
  __type sin_beta = (__type)(sin(beta));
  __type cos_beta = (__type)(cos(beta));
  __type sin_gamma = (__type)(sin(gamma));
  __type cos_gamma = (__type)(cos(gamma));

  *m = (__type)(cos(beta)*cos(gamma)); m++;
  *m = (__type)(cos(beta)*sin(gamma)); m++;
  *m = (__type)(-sin(beta)); m++;
  *m = (__type)0; m++;

  *m = (__type)(cos_gamma*sin_alpha*sin_beta - cos_alpha*sin_gamma); m++;
  *m = (__type)(cos_alpha*cos_gamma + sin_alpha*sin_beta*sin_gamma); m++;
  *m = (__type)(cos_beta*sin_alpha); m++;
  *m = (__type)0; m++;

  *m = (__type)(cos_alpha*cos_gamma*sin_beta + sin_alpha*sin_gamma); m++;
  *m = (__type)(cos_alpha*sin_beta*sin_gamma - cos_gamma*sin_alpha); m++;
  *m = (__type)(cos_alpha*cos_beta); m++;
  *m = (__type)0; m++;

  *m = (__type)0; m++;
  *m = (__type)0; m++;
  *m = (__type)0; m++;
  *m = (__type)1;

  return M;
}

/*
 * Converts a rotation matrix to the corresponding unit quaternion.
 * The matrix argument must not represent nothing more than a rotation,
 * except for translation data which will be left unnoticed.
 */

template <class __type> inline quaternion<__type>
Matrix2Quaternion (const GLmatrix_template<__type>& mat) {
  __type* m = mat();
  __type s, x, y, z;
  s = (__type)(-0.5*sqrt(m[0] + m[5] + m[10] + m[15]));
  __type c = (__type)(0.25/s);
  x = (m[6] - m[9])*c;
  y = (m[8] - m[2])*c;
  z = (m[1] - m[4])*c;
  return quaternion<__type> (s, x, y, z);
}

/*
 * Converts an unit quaternion to the corresponding rotation matrix.
 */

template <class __type> inline GLmatrix_template<__type>
Quaternion2Matrix(const quaternion<__type>& q) {
  __type w = re(q);
  __type x = im1(q);
  __type y = im2(q);
  __type z = im3(q);

  __type wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
  x2 = x + x; y2 = y + y; 
  z2 = z + z;
  xx = x * x2;   xy = x * y2;   xz = x * z2;
  yy = y * y2;   yz = y * z2;   zz = z * z2;
  wx = w * x2;   wy = w * y2;   wz = w * z2;

  GLmatrix_template<__type> m;
  m[0] = 1.0 - (yy + zz); m[4] = xy - wz;         m[8] = xz + wy;          m[12] = 0.0;
  m[1] = xy + wz;         m[5] = 1.0 - (xx + zz); m[9] = yz - wx;          m[13] = 0.0;
  m[2] = xz - wy;         m[6] = yz + wx;         m[10] = 1.0 - (xx + yy); m[14] = 0.0;
  m[3] = 0.0;             m[7] = 0.0;             m[11] = 0.0;             m[15] = 1.0;

  return m; 
}


/*
 * Linear matrix interpolation. The input matrices must not represent
 * anything more than a rotation and a translation. The result is a matrix
 * representing the rotation and the translation on the way from 'm1' to 'm2'
 * according to the time parameter 't' which should lie between [0,1]
 * boundaries. Internally, the matrices are converted to the corresponding
 * quaternions, then they are interpolated through the slerp function and
 * finally the result is converted back to a matrix, where the translation
 * is computed using linear interpolation on the translation vectors from
 * the original two matrices.
 */
template <class __type> inline GLmatrix_template<__type>
Mslerp (__type t, const GLmatrix_template<__type>& m1,
                  const GLmatrix_template<__type>& m2)
{
  quaternion<__type> q = slerp(t, Matrix2Quaternion(m1), Matrix2Quaternion(m2));
  GLmatrix_template<__type> m = Quaternion2Matrix(q);

  m[12] = m1[12] + t*(m2[12]-m1[12]);
  m[13] = m1[13] + t*(m2[13]-m1[13]);
  m[14] = m1[14] + t*(m2[14]-m1[14]);

  return m;
}


} // extern "C++"

#endif
