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

/* 
   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"
#include "stdincl.h"

using namespace std;

/**
   \brief add two vectors
   \param v: the other vector
   \return the sum of both vectors
*/

//Vector Vector::operator+ (const Vector& v) const


/**
   \brief subtract a vector from another
   \param v: the other vector
   \return the difference between the vectors
*/
//Vector Vector::operator- (const Vector& v) const


/**
   \brief calculate the dot product of two vectors
   \param v: the other vector
   \return the dot product of the vectors
*/
//float Vector::operator* (const Vector& v) const


/**
   \brief multiply a vector with a float
   \param f: the factor
   \return the vector multipied by f
*/
//Vector Vector::operator* (float f) const


/**
   \brief divide a vector with a float
   \param f: the divisor
   \return the vector divided by f   
*/
Vector Vector::operator/ (float f) const
{
  __UNLIKELY_IF( f == 0.0)
  {
    // Prevent divide by zero
    return Vector (0,0,0);
  }
  return Vector(x / f, y / f, z / f);
}

/**
   \brief calculate the dot product of two vectors
   \param v: the other vector
   \return the dot product of the vectors
*/
float Vector::dot (const Vector& v) const
{
  return x*v.x+y*v.y+z*v.z;
}

/**
  \brief calculate the cross product of two vectors
  \param v: the other vector
        \return the cross product of the vectors
*/
Vector Vector::cross (const Vector& v) const
{
  return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x );
}
  
/**
   \brief normalizes the vector to lenght 1.0
*/
void Vector::normalize ()
{
  float l = len();
  
  __UNLIKELY_IF( l == 0.0)
  {
    // Prevent divide by zero
    return;
  }
  
  x = x / l;
  y = y / l;
  z = z / l;
}


/**
   \brief returns the voctor normalized to length 1.0
*/

Vector* Vector::getNormalized()
{
  float l = len();
  __UNLIKELY_IF(l != 1.0)
    {
      return this;
    }
  else __UNLIKELY_IF(l == 0.0)
    {
      return 0;
    }

  return new Vector(x / l, y /l, z / l);
}

/**
   \brief scales this Vector with Vector v.
   \param v the vector to scale this vector with
*/
void Vector::scale(const Vector& v)
{
  x *= v.x;
  y *= v.y;
  z *= v.z;
}

 
/**
   \brief calculates the lenght of the vector
   \return the lenght of the vector
*/
float Vector::len () const
{
  return sqrt (x*x+y*y+z*z);
}


/**
   \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 calculate the angle between two vectors in radiances
   \param v1: a vector
   \param v2: another vector
   \return the angle between the vectors in radians
*/
float angleRad (const Vector& v1, const Vector& v2)
{
  return acos( v1 * v2 / (v1.len() * v2.len()));
}


/**
   \brief calculate the angle between two vectors in degrees
   \param v1: a vector
   \param v2: another vector
   \return the angle between the vectors in degrees
*/
float angleDeg (const Vector& v1, const Vector& v2)
{
  float f;
  f = acos( v1 * v2 / (v1.len() * v2.len()));
  return f * 180 / PI;
}


/**
   \brief Outputs the values of the Vector
*/
void Vector::debug(void)
{
  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");
}

/**
        \brief creates a multiplicational identity Quaternion
*/
Quaternion::Quaternion ()
{
        w = 1;
        v = Vector(0,0,0);
}

/**
        \brief turns a rotation along an axis into a Quaternion
        \param angle: the amount of radians to rotate
        \param axis: the axis to rotate around
*/
Quaternion::Quaternion (float angle, const Vector& axis)
{
        w = cos(angle/2);
        v = axis * sin(angle/2);
}

/**
   \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
        
        I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know 
        in what order they are applied, I just copy-pasted the code.
*/
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;
        Quaternion r;
        
        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);
        
        r.w = B +  (-E - F + G + H)/2;
        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;
        
        return r;
}

/**
        \brief add two Quaternions
        \param q: another Quaternion
        \return the sum of both Quaternions
*/
Quaternion Quaternion::operator+(const Quaternion& q) const
{
  Quaternion r(*this);
  r.w = r.w + q.w;
  r.v = r.v + q.v;
  return r;
}

/**
   \brief subtract two Quaternions
   \param q: another Quaternion
   \return the difference of both Quaternions
*/
Quaternion Quaternion::operator- (const Quaternion& q) const
{
  Quaternion r(*this);
  r.w = r.w - q.w;
  r.v = r.v - q.v;
  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 (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 from where
   \param to to where
   \param t the time this transformation should take
   \param res The approximation-density
*/
void Quaternion::quatSlerp(const Quaternion* from, const Quaternion* to, float t, Quaternion* res)
{
  float tol[4];
  double omega, cosom, sinom, scale0, scale1;
  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;
    }
      */
  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];
}


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