source: orxonox.OLD/branches/collision_detection/src/lib/math/vector.cc @ 6694

/*
   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 "vector.h"
#ifdef DEBUG
  #include "debug.h"
#else
  #include <stdio.h>
  #define PRINT(x) printf
#endif

using namespace std;

/////////////
/* VECTORS */
/////////////
/**
 *  returns the this-vector normalized to length 1.0
 * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.
 */
Vector Vector::getNormalized() const
{
  float l = this->len();
  if(unlikely(l == 1.0 || l == 0.0))
    return *this;
  else
    return (*this / l);
}

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



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

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

/**
 *  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()
{
  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
}

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

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

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

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

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

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

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


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

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

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

/**
 *  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 sVec3D& v) const
{
  Vector s(v[0], v[1], v[2]);
  Vector d = s - r;
  Vector u = a * d.dot( a);
  return (d - u).len();
}

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

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

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

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


/**
  *  create a plane from anchor point and normal
  * @param norm: normal vector
  * @param p: anchor point
*/
Plane::Plane (const Vector& norm, const sVec3D& g)
{
  Vector p(g[0], g[1], g[2]);
  n = norm;
  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
}


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

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


/**
 *  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 sVec3D& p) const
{
  Vector s(p[0], p[1], p[2]);
  float l = n.len();
  if( l == 0.0) return 0.0;
  return (n.dot(s) + k) / n.len();
}


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