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

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

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