source: orxonox.OLD/branches/shared_lib/src/lib/math/quaternion.cc @ 8097

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Christian Meyer
   co-programmer: Patrick Boenzli : Vector::scale()
                                    Vector::abs()

   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake

   2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now)
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH

#include "quaternion.h"
#ifdef DEBUG
  #include "debug.h"
#else
  #include <stdio.h>
  #define PRINT(x) printf
#endif

using namespace std;

/////////////////
/* QUATERNIONS */
/////////////////
/**
 *  calculates a lookAt rotation
 * @param dir: the direction you want to look
 * @param up: specify what direction up should be

   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
   the same way as dir. If you want to use this with cameras, you'll have to reverse the
   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
   can use this for meshes as well (then you do not have to reverse the vector), but keep
   in mind that if you do that, the model's front has to point in +z direction, and left
   and right should be -x or +x respectively or the mesh wont rotate correctly.
 *
 * @TODO !!! OPTIMIZE THIS !!!
 */
Quaternion::Quaternion (const Vector& dir, const Vector& up)
{
  Vector z = dir.getNormalized();
  Vector x = up.cross(z).getNormalized();
  Vector y = z.cross(x);

  float m[4][4];
  m[0][0] = x.x;
  m[0][1] = x.y;
  m[0][2] = x.z;
  m[0][3] = 0;
  m[1][0] = y.x;
  m[1][1] = y.y;
  m[1][2] = y.z;
  m[1][3] = 0;
  m[2][0] = z.x;
  m[2][1] = z.y;
  m[2][2] = z.z;
  m[2][3] = 0;
  m[3][0] = 0;
  m[3][1] = 0;
  m[3][2] = 0;
  m[3][3] = 1;

  *this = Quaternion (m);
}

/**
 *  calculates a rotation from euler angles
 * @param roll: the roll in radians
 * @param pitch: the pitch in radians
 * @param yaw: the yaw in radians
 */
Quaternion::Quaternion (float roll, float pitch, float yaw)
{
  float cr, cp, cy, sr, sp, sy, cpcy, spsy;

  // calculate trig identities
  cr = cos(roll/2);
  cp = cos(pitch/2);
  cy = cos(yaw/2);

  sr = sin(roll/2);
  sp = sin(pitch/2);
  sy = sin(yaw/2);

  cpcy = cp * cy;
  spsy = sp * sy;

  w = cr * cpcy + sr * spsy;
  v.x = sr * cpcy - cr * spsy;
  v.y = cr * sp * cy + sr * cp * sy;
  v.z = cr * cp * sy - sr * sp * cy;
}

/**
 *  convert the Quaternion to a 4x4 rotational glMatrix
 * @param m: a buffer to store the Matrix in
 */
void Quaternion::matrix (float m[4][4]) const
{
  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

  // calculate coefficients
  x2 = v.x + v.x;
  y2 = v.y + v.y;
  z2 = v.z + v.z;
  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
  wx = w * x2; wy = w * y2; wz = w * z2;

  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
  m[2][0] = xz + wy; m[3][0] = 0.0;

  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
  m[2][1] = yz - wx; m[3][1] = 0.0;

  m[0][2] = xz - wy; m[1][2] = yz + wx;
  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;

  m[0][3] = 0; m[1][3] = 0;
  m[2][3] = 0; m[3][3] = 1;
}


/**
 * Slerps this QUaternion performs a smooth move.
 * @param toQuat to this Quaternion
 * @param t \% inth the the direction[0..1]
 */
void Quaternion::slerpTo(const Quaternion& toQuat, float t)
{
  float tol[4];
  double omega, cosom, sinom, scale0, scale1;
  //  float DELTA = 0.2;

  cosom = this->v.x * toQuat.v.x + this->v.y * toQuat.v.y + this->v.z * toQuat.v.z + this->w * toQuat.w;

  if( cosom < 0.0 )
  {
    cosom = -cosom;
    tol[0] = -toQuat.v.x;
    tol[1] = -toQuat.v.y;
    tol[2] = -toQuat.v.z;
    tol[3] = -toQuat.w;
  }
  else
  {
    tol[0] = toQuat.v.x;
    tol[1] = toQuat.v.y;
    tol[2] = toQuat.v.z;
    tol[3] = toQuat.w;
  }

  omega = acos(cosom);
  sinom = sin(omega);
  scale0 = sin((1.0 - t) * omega) / sinom;
  scale1 = sin(t * omega) / sinom;
  this->v = Vector(scale0 * this->v.x + scale1 * tol[0],
                    scale0 * this->v.y + scale1 * tol[1],
                    scale0 * this->v.z + scale1 * tol[2]);
  this->w = scale0 * this->w + scale1 * tol[3];
}


/**
 *  performs a smooth move.
 * @param from  where
 * @param to where
 * @param t the time this transformation should take value [0..1]
 * @returns the Result of the smooth move
 */
Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
{
  float tol[4];
  double omega, cosom, sinom, scale0, scale1;
  //  float DELTA = 0.2;

  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;

  if( cosom < 0.0 )
    {
      cosom = -cosom;
      tol[0] = -to.v.x;
      tol[1] = -to.v.y;
      tol[2] = -to.v.z;
      tol[3] = -to.w;
    }
  else
    {
      tol[0] = to.v.x;
      tol[1] = to.v.y;
      tol[2] = to.v.z;
      tol[3] = to.w;
    }

  omega = acos(cosom);
  sinom = sin(omega);
  scale0 = sin((1.0 - t) * omega) / sinom;
  scale1 = sin(t * omega) / sinom;
  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
                    scale0 * from.v.y + scale1 * tol[1],
                    scale0 * from.v.z + scale1 * tol[2]),
                    scale0 * from.w + scale1 * tol[3]);
}


/**
 *  convert a rotational 4x4 glMatrix into a Quaternion
 * @param m: a 4x4 matrix in glMatrix order
 */
Quaternion::Quaternion (float m[4][4])
{

  float  tr, s, q[4];
  int    i, j, k;

  int nxt[3] = {1, 2, 0};

  tr = m[0][0] + m[1][1] + m[2][2];

        // check the diagonal
  if (tr > 0.0)
  {
    s = sqrt (tr + 1.0);
    w = s / 2.0;
    s = 0.5 / s;
    v.x = (m[1][2] - m[2][1]) * s;
    v.y = (m[2][0] - m[0][2]) * s;
    v.z = (m[0][1] - m[1][0]) * s;
        }
        else
        {
                // diagonal is negative
        i = 0;
        if (m[1][1] > m[0][0]) i = 1;
    if (m[2][2] > m[i][i]) i = 2;
    j = nxt[i];
    k = nxt[j];

    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);

    q[i] = s * 0.5;

    if (s != 0.0) s = 0.5 / s;

          q[3] = (m[j][k] - m[k][j]) * s;
    q[j] = (m[i][j] + m[j][i]) * s;
    q[k] = (m[i][k] + m[k][i]) * s;

        v.x = q[0];
        v.y = q[1];
        v.z = q[2];
        w = q[3];
  }
}

/**
 *  outputs some nice formated debug information about this quaternion
*/
void Quaternion::debug() const
{
  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
}

void Quaternion::debug2() const
{
  Vector axis = this->getSpacialAxis();
  PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
}