source: orxonox.OLD/orxonox/trunk/src/lib/math/vector.h @ 4861

/*!
    \file vector.h
  *  A basic 3D math framework

    Contains classes to handle vectors, lines, rotations and planes
*/

#ifndef _VECTOR_H
#define _VECTOR_H

#include <math.h>
#include "compiler.h"
#include "abstract_model.h"
//! PI the circle-constant
#define PI 3.14159265359f

//! 3D Vector
/**
        Class to handle 3D Vectors
*/
class Vector {


 public:
  Vector (float x, float y, float z) : x(x), y(y), z(z) {}  //!< assignment constructor
  Vector () : x(0), y(0), z(0) {}
  ~Vector () {}

  /** @param index The index of the "array" @returns the x/y/z coordinate */
  inline float operator[] (float index) const {if( index == 0) return this->x; if( index == 1) return this->y; if( index == 2) return this->z; }
  /*** @param v The vector to add @returns the addition between two vectors (this + v) */
  inline Vector operator+ (const Vector& v) const { return Vector(x + v.x, y + v.y, z + v.z); };
  /*** @param v The vector to add @returns the addition between two vectors (this + v) */
  inline Vector operator+ (const sVec3D& v) const { return Vector(x + v[0], y + v[1], z + v[2]); };
  /** @param v The vector to add  @returns the addition between two vectors (this += v) */
  inline const Vector& operator+= (const Vector& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; };
  /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
  inline const Vector& operator+= (const sVec3D& v) { this->x += v[0]; this->y += v[1]; this->z += v[2]; return *this; };
  /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
  inline Vector operator- (const Vector& v) const { return Vector(x - v.x, y - v.y, z - v.z); }
  /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
  inline Vector operator- (const sVec3D& v) const { return Vector(x - v[0], y - v[1], z - v[2]); }
  /** @param v The vector to substract  @returns the substraction between two vectors (this -= v) */
  inline const Vector& operator-= (const Vector& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; };
  /** @param v The vector to substract  @returns the substraction between two vectors (this -= v) */
  inline const Vector& operator-= (const sVec3D& v) { this->x -= v[0]; this->y -= v[1]; this->z -= v[2]; return *this; };
  /** @param v the second vector  @returns The dotProduct between two vector (this (dot) v) */
  inline float operator* (const Vector& v) const { return x * v.x + y * v.y + z * v.z; };
  /** @todo strange */
  inline const Vector& operator*= (const Vector& v) { this->x *= v.x; this->y *= v.y; this->z *= v.z; return *this; };
  /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this * f) */
  inline Vector operator* (float f) const { return Vector(x * f, y * f, z * f); };
  /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this *= f) */
  inline const Vector& operator*= (float f) { this->x *= f; this->y *= f; this->z *= f; return *this; };
  /** @param f a factor to divide the vector with @returns the vector divided by f (this / f) */
  inline Vector operator/ (float f) const {if (unlikely(f == 0.0)) return Vector(0,0,0); else return Vector(this->x / f, this->y / f, this->z / f); };
  /** @param f a factor to divide the vector with @returns the vector divided by f (this /= f) */
  inline const Vector& operator/= (float f) {if (unlikely(f == 0.0)) {this->x=0;this->y=0;this->z=0;} else {this->x /= f; this->y /= f; this->z /= f;} return *this; };
  /** \brief copy constructor @todo (i do not know it this is faster) @param v the vector to assign to this vector. @returns the vector v */
  inline const Vector& operator= (const Vector& v) { this->x = v.x; this->y = v.y; this->z = v.z; return *this; };
  /** \brief copy constructor* @param v the sVec3D to assign to this vector. @returns the vector v */
  inline const Vector& operator= (const sVec3D& v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; }
  /** @param v: the other vector \return the dot product of the vectors */
  float dot (const Vector& v) const { return x*v.x+y*v.y+z*v.z; };
  /** @param v: the corss-product partner @returns the cross-product between this and v (this (x) v) */
  inline 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 scales the this vector with v* @param v the vector to scale this with */
  void scale(const Vector& v) {   x *= v.x;  y *= v.y; z *= v.z; };
  /** @returns the length of the vector */
  inline float len() const { return sqrt (x*x+y*y+z*z); }
  /** \brief normalizes the vector */
  inline void normalize() {
                      float l = len();
                      if( unlikely(l == 0.0))
                        {
                          // Prevent divide by zero
                          return;
                        }
                      x = x / l;
                      y = y / l;
                      z = z / l;
                    }
  Vector getNormalized() const;
  Vector abs();

  void debug() const;

 public:
  float    x;     //!< The x Coordinate of the Vector.
  float    y;     //!< The y Coordinate of the Vector.
  float    z;     //!< The z Coordinate of the Vector.
};

/**
 *  calculate the angle between two vectors in radiances
 * @param v1: a vector
 * @param v2: another vector
 * @return the angle between the vectors in radians
*/
inline float angleDeg (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())); };
/**
 *  calculate the angle between two vectors in degrees
 * @param v1: a vector
 * @param v2: another vector
 * @return the angle between the vectors in degrees
*/
inline float angleRad (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())) * 180/M_PI; };


//! Quaternion
/**
   Class to handle 3-dimensional rotation efficiently
*/
class Quaternion
{
 public:
  /** \brief creates a Default quaternion (multiplicational identity Quaternion)*/
  inline Quaternion () { w = 1; v = Vector(0,0,0); }
  /** \brief creates a Quaternion looking into the direction v @param v: the direction @param f: the value */
  inline Quaternion (const Vector& v, float f) { this->w = f; this->v = v; }
  Quaternion (float m[4][4]);
  /** \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 */
  inline Quaternion (float angle, const Vector& axis) { w = cos(angle/2); v = axis * sin(angle/2); }
  Quaternion (const Vector& dir, const Vector& up);
  Quaternion (float roll, float pitch, float yaw);
  Quaternion operator/ (const float& f) const;
  /** @param f: the value to divide by @returns the quaternion devided by f (this /= f) */
  inline const Quaternion& operator/= (const float& f) {*this = *this / f; return *this;}
  Quaternion operator* (const float& f) const;
  /** @param f: the value to multiply by @returns the quaternion multiplied by f (this *= f) */
  inline const Quaternion& operator*= (const float& f) {*this = *this * f; return *this;}
  Quaternion operator* (const Quaternion& q) const;
  /** @param q: the Quaternion to multiply by @returns the quaternion multiplied by q (this *= q) */
  inline const Quaternion operator*= (const Quaternion& q) {*this = *this * q; return *this; };
  /** @param q the Quaternion to add to this @returns the quaternion added with q (this + q) */
  inline Quaternion operator+ (const Quaternion& q) const { return Quaternion(q.v + v, q.w + w); };
  /** @param q the Quaternion to add to this @returns the quaternion added with q (this += q) */
  inline const Quaternion& operator+= (const Quaternion& q) { this->v += q.v; this->w += q.w; return *this; };
  /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this - q) */
  inline Quaternion operator- (const Quaternion& q) const { return Quaternion(q.v - v, q.w - w); }
  /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this -= q) */
  inline const Quaternion& operator-= (const Quaternion& q) { this->v -= q.v; this->w -= q.w; return *this; };
  /** \brief copy constructor @param q: the Quaternion to set this to. @returns the Quaternion q (or this) */
  inline Quaternion operator= (const Quaternion& q) {this->v = q.v; this->w = q.w; return *this;}
  /** \brief conjugates this Quaternion @returns the conjugate */
  inline Quaternion conjugate () const {  Quaternion r(*this);  r.v = Vector() - r.v;  return r;}
  Quaternion inverse () const;
  Vector apply (const Vector& f) const;
  float norm () const;
  void matrix (float m[4][4]) const;

  void debug();

 public:
  Vector    v;        //!< Imaginary Vector
  float     w;        //!< Real part of the number

};

Quaternion quatSlerp(const Quaternion& from, const Quaternion& to, float t);



//! 3D rotation (OBSOLETE)
/**
  Class to handle 3-dimensional rotations.
  Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix
*/
class Rotation {
  public:

  float m[9]; //!< 3x3 Rotation Matrix

  Rotation ( const Vector& v);
  Rotation ( const Vector& axis, float angle);
  Rotation ( float pitch, float yaw, float roll);
  Rotation ();
  ~Rotation () {}

  Rotation operator* (const Rotation& r);

  void glmatrix (float* buffer);
};

//!< Apply a rotation to a vector
Vector rotateVector( const Vector& v, const Rotation& r);

//! 3D line
/**
  Class to store Lines in 3-dimensional space

  Supports line-to-line distance measurements and rotation
*/
class Line
{
  public:

  Vector r;   //!< Offset
  Vector a;   //!< Direction

  Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
  Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
  ~Line () {}

  float distance (const Line& l) const;
  float distancePoint (const Vector& v) const;
  float distancePoint (const sVec3D& v) const;
  Vector* footpoints (const Line& l) const;
  float len () const;

  void rotate(const Rotation& rot);
};

//! 3D plane
/**
  Class to handle planes in 3-dimensional space

  Critical for polygon-based collision detection
*/
class Plane
{
  public:

  Vector n;   //!< Normal vector
  float k;    //!< Offset constant

  Plane (Vector a, Vector b, Vector c);
  Plane (Vector norm, Vector p);
  Plane (Vector norm, sVec3D p);
  Plane (Vector n, float k) : n(n), k(k) {} //!< assignment constructor
  Plane () : n(Vector(1,1,1)), k(0) {}
  ~Plane () {}

  Vector intersectLine (const Line& l) const;
  float distancePoint (const Vector& p) const;
  float distancePoint (const sVec3D& p) const;
  float locatePoint (const Vector& p) const;
};



//! A class that represents a rectangle, this is needed for SpatialSeparation
class Rectangle
{

  public:
    Rectangle() { this->center = new Vector(); }
    virtual ~Rectangle() {}

    /** \brief sets the center of the rectangle to a defined vector @param center the new center */
   inline void setCenter(const Vector &center) { *this->center = center;}
    /** \brief sets the center of the rectangle to a defined vector @param x coord of the center @param y coord of the center @param z coord of the center */
   inline void setCenter(float x, float y, float z) { this->center->x = x; this->center->y = y; this->center->z = z; }
   /** \brief returns the center of the rectangle to a defined vector @returns center the new center */
   inline const Vector* getCenter() const { return this->center; }

   /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */
   inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }
   /** \brief sets both axis of the rectangle to a defined vector @param v1 the length of the x axis @param v2 the length of the z axis*/
   inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }
   /** \brief gets one axis length of the rectangle  @returns the length of the axis 0 */
   inline float getAxis() { return this-> axis[0]; }

  private:
    Vector*         center;
    float           axis[2];
};


#endif /* _VECTOR_H */