source: orxonox.OLD/orxonox/trunk/src/lib/math/vector.cc @ 4537

/* 
   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
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH

#include "vector.h"
#include "debug.h"

using namespace std;

/////////////
/* VECTORS */
/////////////
/**
   \brief returns the this-vector normalized to length 1.0
*/
Vector Vector::getNormalized() const
{
  float l = len();
  if(unlikely(l != 1.0))
    {
      return *this;
    }
  else if(unlikely(l == 0.0))
    {
      return *this;
    }

  return *this / l;
}

/**
   \brief Vector is looking in the positive direction on all axes after this
*/
Vector Vector::abs() 
{
  Vector v(fabs(x), fabs(y), fabs(z));
  return v;
}



/**
   \brief Outputs the values of the Vector
*/
void Vector::debug(void) const
{
  PRINT(0)("Vector Debug information\n");
  PRINT(0)("x: %f; y: %f; z: %f", x, y, z);
  PRINT(3)(" lenght: %f", len());
  PRINT(0)("\n");
}

/////////////////
/* QUATERNIONS */
/////////////////
/**
   \brief 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.
*/
Quaternion::Quaternion (const Vector& dir, const Vector& up)
{
  Vector z = dir;
  z.normalize(); 
  Vector x = up.cross(z);
  x.normalize(); 
  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);
}

/**
   \brief 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;
}

/**
   \brief rotates one Quaternion by another
   \param q: another Quaternion to rotate this by
   \return a quaternion that represents the first one rotated by the second one (WARUNING: this operation is not commutative! e.g. (A*B) != (B*A))
*/
Quaternion Quaternion::operator*(const Quaternion& q) const
{
  float A, B, C, D, E, F, G, H;
  
  A = (w   + v.x)*(q.w   + q.v.x);
  B = (v.z - v.y)*(q.v.y - q.v.z);
  C = (w   - v.x)*(q.v.y + q.v.z); 
  D = (v.y + v.z)*(q.w   - q.v.x);
  E = (v.x + v.z)*(q.v.x + q.v.y);
  F = (v.x - v.z)*(q.v.x - q.v.y);
  G = (w   + v.y)*(q.w   - q.v.z);
  H = (w   - v.y)*(q.w   + q.v.z);
  
  Quaternion r;
  r.v.x = A - (E + F + G + H)/2;
  r.v.y = C + (E - F + G - H)/2;
  r.v.z = D + (E - F - G + H)/2;
  r.w = B +  (-E - F + G + H)/2;

  return r;
}

/**
   \brief rotate a Vector by a Quaternion
   \param v: the Vector
   \return a new Vector representing v rotated by the Quaternion
*/

Vector Quaternion::apply (const Vector& v) const
{
  Quaternion q;
  q.v = v;
  q.w = 0;
  q = *this * q * conjugate();
  return q.v;
}


/**
   \brief multiply a Quaternion with a real value
   \param f: a real value
   \return a new Quaternion containing the product
*/
Quaternion Quaternion::operator*(const float& f) const
{
  Quaternion r(*this);
  r.w = r.w*f;
  r.v = r.v*f;
  return r;
}

/**
   \brief divide a Quaternion by a real value
   \param f: a real value
   \return a new Quaternion containing the quotient
*/
Quaternion Quaternion::operator/(const float& f) const
{
  if( f == 0) return Quaternion();
  Quaternion r(*this);
  r.w = r.w/f;
  r.v = r.v/f;
  return r;
}

/**
   \brief calculate the conjugate value of the Quaternion
   \return the conjugate Quaternion
*/
/*
Quaternion Quaternion::conjugate() const
{
  Quaternion r(*this);
  r.v = Vector() - r.v;
  return r;
}
*/

/**
   \brief calculate the norm of the Quaternion
   \return the norm of The Quaternion
*/
float Quaternion::norm() const
{
  return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
}

/**
   \brief calculate the inverse value of the Quaternion
   \return the inverse Quaternion
   
        Note that this is equal to conjugate() if the Quaternion's norm is 1
*/
Quaternion Quaternion::inverse() const
{
  float n = norm();
  if (n != 0)
    {
      return conjugate() / norm();
    }
  else return Quaternion();
}

/**
   \brief 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;
}

/**
   \brief 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 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;
    }
  
  //if( (1.0 - cosom) > DELTA )
  //{
  omega = acos(cosom);
  sinom = sin(omega);
  scale0 = sin((1.0 - t) * omega) / sinom;
  scale1 = sin(t * omega) / sinom;
  //}
  /*
    else
    {
    scale0 = 1.0 - t;
    scale1 = t;
    }
  */


  /*
    Quaternion res;
    res.v.x = scale0 * from.v.x + scale1 * tol[0];
    res.v.y = scale0 * from.v.y + scale1 * tol[1];
    res.v.z = scale0 * from.v.z + scale1 * tol[2];
    res.w = scale0 * from.w + scale1 * tol[3];
  */
  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]);
}


/**
   \brief 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];
  }
}

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

/**
   \brief create a rotation from a vector
   \param v: a vector
*/
Rotation::Rotation (const Vector& v)
{
  Vector x = Vector( 1, 0, 0);
  Vector axis = x.cross( v);
  axis.normalize();
  float angle = angleRad( x, v);
  float ca = cos(angle);
  float sa = sin(angle);
  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
}

/**
   \brief creates a rotation from an axis and an angle (radians!)
   \param axis: the rotational axis
   \param angle: the angle in radians
*/
Rotation::Rotation (const Vector& axis, float angle)
{
  float ca, sa;
  ca = cos(angle);
  sa = sin(angle);
  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
}

/**
   \brief creates a rotation from euler angles (pitch/yaw/roll)
   \param pitch: rotation around z (in radians)
   \param yaw: rotation around y (in radians)
   \param roll: rotation around x (in radians)
*/
Rotation::Rotation ( float pitch, float yaw, float roll)
{
  float cy, sy, cr, sr, cp, sp;
  cy = cos(yaw);
  sy = sin(yaw);
  cr = cos(roll);
  sr = sin(roll);
  cp = cos(pitch);
  sp = sin(pitch);
  m[0] = cy*cr;
  m[1] = -cy*sr;
  m[2] = sy;
  m[3] = cp*sr+sp*sy*cr;
  m[4] = cp*cr-sp*sr*sy;
  m[5] = -sp*cy;
  m[6] = sp*sr-cp*sy*cr;
  m[7] = sp*cr+cp*sy*sr;
  m[8] = cp*cy;
}

/**
   \brief creates a nullrotation (an identity rotation)
*/
Rotation::Rotation ()
{
  m[0] = 1.0f;
  m[1] = 0.0f;
  m[2] = 0.0f;
  m[3] = 0.0f;
  m[4] = 1.0f;
  m[5] = 0.0f;
  m[6] = 0.0f;
  m[7] = 0.0f;
  m[8] = 1.0f;
}

/**
   \brief fills the specified buffer with a 4x4 glmatrix
   \param buffer: Pointer to an array of 16 floats
   
   Use this to get the rotation in a gl-compatible format
*/
void Rotation::glmatrix (float* buffer)
{
        buffer[0] = m[0];
        buffer[1] = m[3];
        buffer[2] = m[6];
        buffer[3] = m[0];
        buffer[4] = m[1];
        buffer[5] = m[4];
        buffer[6] = m[7];
        buffer[7] = m[0];
        buffer[8] = m[2];
        buffer[9] = m[5];
        buffer[10] = m[8];
        buffer[11] = m[0];
        buffer[12] = m[0];
        buffer[13] = m[0];
        buffer[14] = m[0];
        buffer[15] = m[1];
}

/**
   \brief multiplies two rotational matrices
   \param r: another Rotation
   \return the matrix product of the Rotations
   
   Use this to rotate one rotation by another
*/
Rotation Rotation::operator* (const Rotation& r)
{
        Rotation p;
        
        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
        
        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];

        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
        
        return p;
}


/**
   \brief rotates the vector by the given rotation
   \param v: a vector
   \param r: a rotation
   \return the rotated vector
*/
Vector rotateVector( const Vector& v, const Rotation& r)
{
  Vector t;
  
  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];

  return t;
}

/**
   \brief calculate the distance between two lines
   \param l: the other line
   \return the distance between the lines
*/
float Line::distance (const Line& l) const
{
  float q, d;
  Vector n = a.cross(l.a);
  q = n.dot(r-l.r);
  d = n.len();
  if( d == 0.0) return 0.0;
  return q/d;
}

/**
   \brief calculate the distance between a line and a point
   \param v: the point
   \return the distance between the Line and the point
*/
float Line::distancePoint (const Vector& v) const
{
  Vector d = v-r;
  Vector u = a * d.dot( a);
  return (d - u).len();
}

/**
   \brief calculate the two points of minimal distance of two lines
   \param l: the other line
   \return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
*/
Vector* Line::footpoints (const Line& l) const
{
  Vector* fp = new Vector[2];
  Plane p = Plane (r + a.cross(l.a), r, r + a);
  fp[1] = p.intersectLine (l);
  p = Plane (fp[1], l.a);
  fp[0] = p.intersectLine (*this);
  return fp;
}

/**
  \brief calculate the length of a line
  \return the lenght of the line 
*/
float Line::len() const
{
  return a.len();
}

/**
   \brief rotate the line by given rotation
   \param rot: a rotation
*/
void Line::rotate (const Rotation& rot)
{
  Vector t = a + r;
  t = rotateVector( t, rot);
  r = rotateVector( r, rot),
  a = t - r;
}

/**
   \brief create a plane from three points
   \param a: first point
   \param b: second point
   \param c: third point
*/
Plane::Plane (Vector a, Vector b, Vector c)
{
  n = (a-b).cross(c-b);
  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
}

/**
   \brief create a plane from anchor point and normal
   \param norm: normal vector
   \param p: anchor point
*/
Plane::Plane (Vector norm, Vector p)
{
  n = norm;
  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
}

/**
   \brief returns the intersection point between the plane and a line
   \param l: a line
*/
Vector Plane::intersectLine (const Line& l) const
{
  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
  float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z);
  return l.r + (l.a * t);
}

/**
   \brief returns the distance between the plane and a point
   \param p: a Point
   \return the distance between the plane and the point (can be negative)
*/
float Plane::distancePoint (const Vector& p) const
{
  float l = n.len();
  if( l == 0.0) return 0.0;
  return (n.dot(p) + k) / n.len();
}

/**
   \brief returns the side a point is located relative to a Plane
   \param p: a Point
   \return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane
*/
float Plane::locatePoint (const Vector& p) const
{
  return (n.dot(p) + k);
}