source: orxonox.OLD/orxonox/branches/chris/src/vector.cc @ 2115

/* 
   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: ...
   
   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
*/


#include "vector.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
{
  Vector r;
  
  r.x = x + v.x;
  r.y = y + v.y;
  r.z = z + v.z;
  
  return r;
}

/**
   \brief subtract a vector from another
   \param v: the other vector
   \return the difference between the vectors
*/
Vector Vector::operator- (const Vector& v) const
{
  Vector r;
  
  r.x = x - v.x;
  r.y = y - v.y;
  r.z = z - v.z;
  
  return r;
}

/**
   \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
{
  return x*v.x+y*v.y+z*v.z;
}

/**
   \brief multiply a vector with a float
   \param f: the factor
   \return the vector multipied by f
*/
Vector Vector::operator* (float f) const
{  
  Vector r;
  
  r.x = x * f;
  r.y = y * f;
  r.z = z * f;
  
  return r;
}

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

/**
   \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
{
  Vector r;
  
  r.x = y * v.z - z * v.y;
  r.y = z * v.x - x * v.z;
  r.z = x * v.y - y * v.x;
  
  return r;
}
  
/**
   \brief normalizes the vector to lenght 1.0
*/
void Vector::normalize ()
{
  float l = len();
  
  if( l == 0.0)
  {
    // Prevent divide by zero
    return;
  }
  
  x = x / l;
  y = y / l;
  z = z / l;
}
 
/**
   \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 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 angle_rad (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 angle_deg (const Vector& v1, const Vector& v2)
{
  float f;
  f = acos( v1 * v2 / (v1.len() * v2.len()));
  return f * 180 / PI;
}

Quaternion::Quaternion ()
{
        w = 1;
        v = Vector(0,0,0);
}

Quaternion::Quaternion (float angle, const Vector& axis)
{
        w = cos(angle/2);
        v = axis * sin(angle/2);
}

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);
}

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;
}

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;
}

Quaternion Quaternion::operator+(const Quaternion& q) const
{
        Quaternion r(*this);
        r.w = r.w + q.w;
        r.v = r.v + q.v;
        return r;
}

Quaternion Quaternion::operator- (const Quaternion& q) const
{
        Quaternion r(*this);
        r.w = r.w - q.w;
        r.v = r.v - q.v;
        return r;
}

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

Quaternion Quaternion::operator*(const float& f) const
{
        Quaternion r(*this);
        r.w = r.w*f;
        r.v = r.v*f;
        return r;
}

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;
}

Quaternion Quaternion::conjugate() const
{
        Quaternion r(*this);
        r.v = Vector() - r.v;
        return r;
}

float Quaternion::norm() const
{
        return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
}

Quaternion Quaternion::inverse() const
{
        float n = norm();
        if (n != 0)
        {
                return conjugate() / norm();
        }
        else return Quaternion();
}

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;
}

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 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 = angle_rad( 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 rotate_vector( 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::distance_point (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.intersect_line (l);
  p = Plane (fp[1], l.a);
  fp[0] = p.intersect_line (*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 = rotate_vector( t, rot);
  r = rotate_vector( 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 n: 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::intersect_line (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::distance_point (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::locate_point (const Vector& p) const
{
  return (n.dot(p) + k);
}