Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 6616 in orxonox.OLD for trunk/src


Ignore:
Timestamp:
Jan 19, 2006, 12:27:45 PM (19 years ago)
Author:
bensch
Message:

orxonox/trunk: taken the quaternion outside of Vector.cc to quaternion.cc/h

Location:
trunk/src
Files:
9 edited
2 copied

Legend:

Unmodified
Added
Removed
  • trunk/src/lib/Makefile.am

    r6615 r6616  
    4343                        util/executor/executor.cc \
    4444                        math/vector.cc \
     45                        math/quaternion.cc \
    4546                        math/vector2D.cc \
    4647                        math/matrix.cc \
     
    6465                        util/executor/functor_list.h \
    6566                        math/vector.h \
     67                        math/quaternion.h \
    6668                        math/vector2D.h \
    6769                        math/matrix.h \
  • trunk/src/lib/coord/p_node.cc

    r6512 r6616  
    214214
    215215  *this->toCoordinate = relCoordSoft;
     216  this->toStep = 0.0f;
    216217  this->bias = bias;
    217218}
     
    299300      *this->toCoordinate = absCoordSoft;
    300301  }
     302  this->toStep = 0.0f;
    301303}
    302304
     
    370372  *this->toDirection = relDirSoft;
    371373  this->bias = bias;
     374  this->toStep = 0.0f;
    372375  this->bRelDirChanged = true;
    373376}
     
    439442
    440443  this->bias = bias;
     444  this->toStep = 0.0f;
    441445  this->bRelDirChanged = true;
    442446}
     
    614618  }
    615619  this->bias = bias;
    616 
     620  this->toStep = 0.0f;
    617621
    618622  Vector tmpV = this->getAbsCoor();
     
    743747        if (unlikely(this->toDirection != NULL))
    744748        {
    745           Quaternion rotQuat = Quaternion::quatSlerp(this->relDirection,*this->toDirection, fabsf(dt)*this->bias);
     749          this->toStep += fabs(dt) * bias;
     750          printf("%s::%s %f\n", this->getClassName(), this->getName(), this->toStep );
     751          Quaternion rotQuat = Quaternion::quatSlerp(this->relDirection,*this->toDirection, toStep);
    746752          if (this->relDirection.distance(rotQuat) >PNODE_ITERATION_DELTA)
    747753          {
  • trunk/src/lib/coord/p_node.h

    r6512 r6616  
    2424
    2525#include "vector.h"
     26#include "quaternion.h"
    2627#include <list>
    2728
     
    218219  Quaternion*        toDirection;        //!< a direction to which to iterate. (This is used in conjunction with setParentSoft and set*DirSoft)
    219220  float              bias;               //!< how fast to iterate to the given position (default is 1)
     221  float              toStep;
    220222
    221223  PNode*             parent;             //!< a pointer to the parent node.
  • trunk/src/lib/math/curve.h

    r5231 r6616  
    1111
    1212#include "vector.h"
     13#include "quaternion.h"
    1314
    1415template<class T> class tList;
  • trunk/src/lib/math/quaternion.cc

    r6614 r6616  
    2121#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
    2222
    23 #include "vector.h"
     23#include "quaternion.h"
    2424#ifdef DEBUG
    2525  #include "debug.h"
     
    3030
    3131using namespace std;
    32 
    33 /////////////
    34 /* VECTORS */
    35 /////////////
    36 /**
    37  *  returns the this-vector normalized to length 1.0
    38  * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.
    39  */
    40 Vector Vector::getNormalized() const
    41 {
    42   float l = this->len();
    43   if(unlikely(l == 1.0 || l == 0.0))
    44     return *this;
    45   else
    46     return (*this / l);
    47 }
    48 
    49 /**
    50  *  Vector is looking in the positive direction on all axes after this
    51 */
    52 Vector Vector::abs()
    53 {
    54   Vector v(fabs(x), fabs(y), fabs(z));
    55   return v;
    56 }
    57 
    58 
    59 
    60 /**
    61  *  Outputs the values of the Vector
    62  */
    63 void Vector::debug() const
    64 {
    65   PRINT(0)("x: %f; y: %f; z: %f", x, y, z);
    66   PRINT(0)(" lenght: %f", len());
    67   PRINT(0)("\n");
    68 }
    6932
    7033/////////////////
     
    274237  PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
    275238}
    276 
    277 /**
    278  *  create a rotation from a vector
    279  * @param v: a vector
    280 */
    281 Rotation::Rotation (const Vector& v)
    282 {
    283   Vector x = Vector( 1, 0, 0);
    284   Vector axis = x.cross( v);
    285   axis.normalize();
    286   float angle = angleRad( x, v);
    287   float ca = cos(angle);
    288   float sa = sin(angle);
    289   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    290   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    291   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    292   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    293   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    294   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    295   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    296   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    297   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    298 }
    299 
    300 /**
    301  *  creates a rotation from an axis and an angle (radians!)
    302  * @param axis: the rotational axis
    303  * @param angle: the angle in radians
    304 */
    305 Rotation::Rotation (const Vector& axis, float angle)
    306 {
    307   float ca, sa;
    308   ca = cos(angle);
    309   sa = sin(angle);
    310   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    311   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    312   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    313   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    314   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    315   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    316   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    317   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    318   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    319 }
    320 
    321 /**
    322  *  creates a rotation from euler angles (pitch/yaw/roll)
    323  * @param pitch: rotation around z (in radians)
    324  * @param yaw: rotation around y (in radians)
    325  * @param roll: rotation around x (in radians)
    326 */
    327 Rotation::Rotation ( float pitch, float yaw, float roll)
    328 {
    329   float cy, sy, cr, sr, cp, sp;
    330   cy = cos(yaw);
    331   sy = sin(yaw);
    332   cr = cos(roll);
    333   sr = sin(roll);
    334   cp = cos(pitch);
    335   sp = sin(pitch);
    336   m[0] = cy*cr;
    337   m[1] = -cy*sr;
    338   m[2] = sy;
    339   m[3] = cp*sr+sp*sy*cr;
    340   m[4] = cp*cr-sp*sr*sy;
    341   m[5] = -sp*cy;
    342   m[6] = sp*sr-cp*sy*cr;
    343   m[7] = sp*cr+cp*sy*sr;
    344   m[8] = cp*cy;
    345 }
    346 
    347 /**
    348  *  creates a nullrotation (an identity rotation)
    349 */
    350 Rotation::Rotation ()
    351 {
    352   m[0] = 1.0f;
    353   m[1] = 0.0f;
    354   m[2] = 0.0f;
    355   m[3] = 0.0f;
    356   m[4] = 1.0f;
    357   m[5] = 0.0f;
    358   m[6] = 0.0f;
    359   m[7] = 0.0f;
    360   m[8] = 1.0f;
    361 }
    362 
    363 /**
    364  *  fills the specified buffer with a 4x4 glmatrix
    365  * @param buffer: Pointer to an array of 16 floats
    366 
    367    Use this to get the rotation in a gl-compatible format
    368 */
    369 void Rotation::glmatrix (float* buffer)
    370 {
    371         buffer[0] = m[0];
    372         buffer[1] = m[3];
    373         buffer[2] = m[6];
    374         buffer[3] = m[0];
    375         buffer[4] = m[1];
    376         buffer[5] = m[4];
    377         buffer[6] = m[7];
    378         buffer[7] = m[0];
    379         buffer[8] = m[2];
    380         buffer[9] = m[5];
    381         buffer[10] = m[8];
    382         buffer[11] = m[0];
    383         buffer[12] = m[0];
    384         buffer[13] = m[0];
    385         buffer[14] = m[0];
    386         buffer[15] = m[1];
    387 }
    388 
    389 /**
    390  *  multiplies two rotational matrices
    391  * @param r: another Rotation
    392  * @return the matrix product of the Rotations
    393 
    394    Use this to rotate one rotation by another
    395 */
    396 Rotation Rotation::operator* (const Rotation& r)
    397 {
    398         Rotation p;
    399 
    400         p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
    401         p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
    402         p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
    403 
    404         p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
    405         p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
    406         p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
    407 
    408         p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
    409         p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
    410         p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
    411 
    412         return p;
    413 }
    414 
    415 
    416 /**
    417  *  rotates the vector by the given rotation
    418  * @param v: a vector
    419  * @param r: a rotation
    420  * @return the rotated vector
    421 */
    422 Vector rotateVector( const Vector& v, const Rotation& r)
    423 {
    424   Vector t;
    425 
    426   t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
    427   t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
    428   t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
    429 
    430   return t;
    431 }
    432 
    433 /**
    434  *  calculate the distance between two lines
    435  * @param l: the other line
    436  * @return the distance between the lines
    437 */
    438 float Line::distance (const Line& l) const
    439 {
    440   float q, d;
    441   Vector n = a.cross(l.a);
    442   q = n.dot(r-l.r);
    443   d = n.len();
    444   if( d == 0.0) return 0.0;
    445   return q/d;
    446 }
    447 
    448 /**
    449  *  calculate the distance between a line and a point
    450  * @param v: the point
    451  * @return the distance between the Line and the point
    452 */
    453 float Line::distancePoint (const Vector& v) const
    454 {
    455   Vector d = v-r;
    456   Vector u = a * d.dot( a);
    457   return (d - u).len();
    458 }
    459 
    460 /**
    461  *  calculate the distance between a line and a point
    462  * @param v: the point
    463  * @return the distance between the Line and the point
    464  */
    465 float Line::distancePoint (const sVec3D& v) const
    466 {
    467   Vector s(v[0], v[1], v[2]);
    468   Vector d = s - r;
    469   Vector u = a * d.dot( a);
    470   return (d - u).len();
    471 }
    472 
    473 /**
    474  *  calculate the two points of minimal distance of two lines
    475  * @param l: the other line
    476  * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
    477 */
    478 Vector* Line::footpoints (const Line& l) const
    479 {
    480   Vector* fp = new Vector[2];
    481   Plane p = Plane (r + a.cross(l.a), r, r + a);
    482   fp[1] = p.intersectLine (l);
    483   p = Plane (fp[1], l.a);
    484   fp[0] = p.intersectLine (*this);
    485   return fp;
    486 }
    487 
    488 /**
    489   \brief calculate the length of a line
    490   \return the lenght of the line
    491 */
    492 float Line::len() const
    493 {
    494   return a.len();
    495 }
    496 
    497 /**
    498  *  rotate the line by given rotation
    499  * @param rot: a rotation
    500 */
    501 void Line::rotate (const Rotation& rot)
    502 {
    503   Vector t = a + r;
    504   t = rotateVector( t, rot);
    505   r = rotateVector( r, rot),
    506   a = t - r;
    507 }
    508 
    509 /**
    510  *  create a plane from three points
    511  * @param a: first point
    512  * @param b: second point
    513  * @param c: third point
    514 */
    515 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)
    516 {
    517   n = (a-b).cross(c-b);
    518   k = -(n.x*b.x+n.y*b.y+n.z*b.z);
    519 }
    520 
    521 /**
    522  *  create a plane from anchor point and normal
    523  * @param norm: normal vector
    524  * @param p: anchor point
    525 */
    526 Plane::Plane (const Vector& norm, const Vector& p)
    527 {
    528   n = norm;
    529   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    530 }
    531 
    532 
    533 /**
    534   *  create a plane from anchor point and normal
    535   * @param norm: normal vector
    536   * @param p: anchor point
    537 */
    538 Plane::Plane (const Vector& norm, const sVec3D& g)
    539 {
    540   Vector p(g[0], g[1], g[2]);
    541   n = norm;
    542   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    543 }
    544 
    545 
    546 /**
    547  *  returns the intersection point between the plane and a line
    548  * @param l: a line
    549 */
    550 Vector Plane::intersectLine (const Line& l) const
    551 {
    552   if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
    553   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);
    554   return l.r + (l.a * t);
    555 }
    556 
    557 /**
    558  *  returns the distance between the plane and a point
    559  * @param p: a Point
    560  * @return the distance between the plane and the point (can be negative)
    561 */
    562 float Plane::distancePoint (const Vector& p) const
    563 {
    564   float l = n.len();
    565   if( l == 0.0) return 0.0;
    566   return (n.dot(p) + k) / n.len();
    567 }
    568 
    569 
    570 /**
    571  *  returns the distance between the plane and a point
    572  * @param p: a Point
    573  * @return the distance between the plane and the point (can be negative)
    574  */
    575 float Plane::distancePoint (const sVec3D& p) const
    576 {
    577   Vector s(p[0], p[1], p[2]);
    578   float l = n.len();
    579   if( l == 0.0) return 0.0;
    580   return (n.dot(s) + k) / n.len();
    581 }
    582 
    583 
    584 /**
    585  *  returns the side a point is located relative to a Plane
    586  * @param p: a Point
    587  * @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
    588 */
    589 float Plane::locatePoint (const Vector& p) const
    590 {
    591   return (n.dot(p) + k);
    592 }
    593 
  • trunk/src/lib/math/quaternion.h

    r6614 r6616  
    1515
    1616/*!
    17  * @file vector.h
    18  * A basic 3D math framework
     17 * @file quaternion.h
     18 * A basic 3D quaternion math framework
    1919 *
    2020 * Contains classes to handle vectors, lines, rotations and planes
    2121*/
    2222
    23 #ifndef __VECTOR_H_
    24 #define __VECTOR_H_
     23#ifndef __QUATERNION_H_
     24#define __QUATERNION_H_
    2525
    2626#include <math.h>
     
    2828//! PI the circle-constant
    2929#define PI 3.14159265359f
    30 
    31 
    32 //! this is a small and performant 3D vector
    33 typedef float sVec3D[3];
    34 
    35 
    36 //! small and performant 2D vector
    37 typedef float sVec2D[2];
    38 
    39 
    40 
    41 //! 3D Vector
    42 /**
    43         Class to handle 3D Vectors
    44 */
    45 class Vector {
    46  public:
    47   Vector (float x, float y, float z) : x(x), y(y), z(z) {}  //!< assignment constructor
    48   Vector () : x(0), y(0), z(0) {}
    49   ~Vector () {}
    50 
    51   /** @param v: the Vecor to compare with this one @returns true, if the Vecors are the same, false otherwise */
    52   inline bool operator== (const Vector& v) const { return (this->x==v.x&&this->y==v.y&&this->z==v.z)?true:false; };
    53   /** @param index The index of the "array" @returns the x/y/z coordinate */
    54   inline float operator[] (float index) const {if( index == 0) return this->x; if( index == 1) return this->y; if( index == 2) return this->z; }
    55   /** @param v The vector to add @returns the addition between two vectors (this + v) */
    56   inline Vector operator+ (const Vector& v) const { return Vector(x + v.x, y + v.y, z + v.z); };
    57   /** @param v The vector to add @returns the addition between two vectors (this + v) */
    58   inline Vector operator+ (const sVec3D& v) const { return Vector(x + v[0], y + v[1], z + v[2]); };
    59   /** @param v The vector to add  @returns the addition between two vectors (this += v) */
    60   inline const Vector& operator+= (const Vector& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; };
    61   /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
    62   inline const Vector& operator+= (const sVec3D& v) { this->x += v[0]; this->y += v[1]; this->z += v[2]; return *this; };
    63   /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
    64   inline Vector operator- (const Vector& v) const { return Vector(x - v.x, y - v.y, z - v.z); }
    65   /** @param v The vector to substract  @returns the substraction between two vectors (this - v) */
    66   inline Vector operator- (const sVec3D& v) const { return Vector(x - v[0], y - v[1], z - v[2]); }
    67   /** @param v The vector to substract  @returns the substraction between two vectors (this -= v) */
    68   inline const Vector& operator-= (const Vector& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; };
    69   /** @param v The vector to substract  @returns the substraction between two vectors (this -= v) */
    70   inline const Vector& operator-= (const sVec3D& v) { this->x -= v[0]; this->y -= v[1]; this->z -= v[2]; return *this; };
    71   /** @param v the second vector  @returns The dotProduct between two vector (this (dot) v) */
    72   inline float operator* (const Vector& v) const { return x * v.x + y * v.y + z * v.z; };
    73   /** @todo strange */
    74   inline const Vector& operator*= (const Vector& v) { this->x *= v.x; this->y *= v.y; this->z *= v.z; return *this; };
    75   /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this * f) */
    76   inline Vector operator* (float f) const { return Vector(x * f, y * f, z * f); };
    77   /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this *= f) */
    78   inline const Vector& operator*= (float f) { this->x *= f; this->y *= f; this->z *= f; return *this; };
    79   /** @param f a factor to divide the vector with @returns the vector divided by f (this / f) */
    80   inline Vector operator/ (float f) const { return (unlikely(f == 0.0))?Vector(0,0,0):Vector(this->x / f, this->y / f, this->z / f); };
    81   /** @param f a factor to divide the vector with @returns the vector divided by f (this /= f) */
    82   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; };
    83   /**  copy constructor @todo (i do not know it this is faster) @param v the vector to assign to this vector. @returns the vector v */
    84   inline const Vector& operator= (const Vector& v) { this->x = v.x; this->y = v.y; this->z = v.z; return *this; };
    85   /** copy constructor* @param v the sVec3D to assign to this vector. @returns the vector v */
    86   inline const Vector& operator= (const sVec3D& v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; }
    87   /** @param v: the other vector \return the dot product of the vectors */
    88   float dot (const Vector& v) const { return x*v.x+y*v.y+z*v.z; };
    89   /** @param v: the corss-product partner @returns the cross-product between this and v (this (x) v) */
    90   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 ); }
    91   /** scales the this vector with v* @param v the vector to scale this with */
    92   void scale(const Vector& v) {   x *= v.x;  y *= v.y; z *= v.z; };
    93   /** @returns the length of the vector */
    94   inline float len() const { return sqrt (x*x+y*y+z*z); }
    95   /** normalizes the vector */
    96   inline void normalize() { float l = len(); if( unlikely(l == 0.0))return; this->x=this->x/l; this->y=this->y/l; this->z=this->z/l; };
    97   Vector getNormalized() const;
    98   Vector abs();
    99 
    100   void debug() const;
    101 
    102  public:
    103   float    x;     //!< The x Coordinate of the Vector.
    104   float    y;     //!< The y Coordinate of the Vector.
    105   float    z;     //!< The z Coordinate of the Vector.
    106 };
    107 
    108 /**
    109  *  calculate the angle between two vectors in radiances
    110  * @param v1: a vector
    111  * @param v2: another vector
    112  * @return the angle between the vectors in radians
    113 */
    114 inline float angleDeg (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())); };
    115 /**
    116  *  calculate the angle between two vectors in degrees
    117  * @param v1: a vector
    118  * @param v2: another vector
    119  * @return the angle between the vectors in degrees
    120 */
    121 inline float angleRad (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())) * 180/M_PI; };
    122 
    123 /** an easy way to create a Random Vector @param sideLength the length of the Vector (x not sqrt(x^2...)) */
    124 #define VECTOR_RAND(sideLength)  (Vector((float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5) * sideLength)
    125 
     30#include "vector.h"
    12631
    12732//! Quaternion
     
    205110  Vector    v;        //!< Imaginary Vector
    206111  float     w;        //!< Real part of the number
    207 
    208112};
    209113
    210114
     115#endif /* __QUATERNION_H_ */
    211116
    212 
    213 //! 3D rotation (OBSOLETE)
    214 /**
    215   Class to handle 3-dimensional rotations.
    216   Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix
    217 */
    218 class Rotation {
    219   public:
    220 
    221   float m[9]; //!< 3x3 Rotation Matrix
    222 
    223   Rotation ( const Vector& v);
    224   Rotation ( const Vector& axis, float angle);
    225   Rotation ( float pitch, float yaw, float roll);
    226   Rotation ();
    227   ~Rotation () {}
    228 
    229   Rotation operator* (const Rotation& r);
    230 
    231   void glmatrix (float* buffer);
    232 };
    233 
    234 //!< Apply a rotation to a vector
    235 Vector rotateVector( const Vector& v, const Rotation& r);
    236 
    237 //! 3D line
    238 /**
    239   Class to store Lines in 3-dimensional space
    240 
    241   Supports line-to-line distance measurements and rotation
    242 */
    243 class Line
    244 {
    245   public:
    246 
    247   Vector r;   //!< Offset
    248   Vector a;   //!< Direction
    249 
    250   Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
    251   Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
    252   ~Line () {}
    253 
    254   float distance (const Line& l) const;
    255   float distancePoint (const Vector& v) const;
    256   float distancePoint (const sVec3D& v) const;
    257   Vector* footpoints (const Line& l) const;
    258   float len () const;
    259 
    260   void rotate(const Rotation& rot);
    261 };
    262 
    263 //! 3D plane
    264 /**
    265   Class to handle planes in 3-dimensional space
    266 
    267   Critical for polygon-based collision detection
    268 */
    269 class Plane
    270 {
    271   public:
    272 
    273   Vector n;   //!< Normal vector
    274   float k;    //!< Offset constant
    275 
    276   Plane (const Vector& a, const Vector& b, const Vector& c);
    277   Plane (const Vector& norm, const Vector& p);
    278   Plane (const Vector& norm, const sVec3D& p);
    279   Plane (const Vector& n, float k) : n(n), k(k) {} //!< assignment constructor
    280   Plane () : n(Vector(1,1,1)), k(0) {}
    281   ~Plane () {}
    282 
    283   Vector intersectLine (const Line& l) const;
    284   float distancePoint (const Vector& p) const;
    285   float distancePoint (const sVec3D& p) const;
    286   float locatePoint (const Vector& p) const;
    287 };
    288 
    289 
    290 
    291 //! A class that represents a rectangle, this is needed for SpatialSeparation
    292 class Rectangle
    293 {
    294 
    295   public:
    296     Rectangle() { this->center = Vector(); }
    297     Rectangle(const Vector &center, float len) { this->center = Vector(center.x, center.y, center.z); this->axis[0] = len; this->axis[1] = len; }
    298     virtual ~Rectangle() {}
    299 
    300     /** \brief sets the center of the rectangle to a defined vector @param center the new center */
    301    inline void setCenter(const Vector &center) { this->center = center;}
    302     /** \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 */
    303    inline void setCenter(float x, float y, float z) { this->center.x = x; this->center.y = y; this->center.z = z; }
    304    /** \brief returns the center of the rectangle to a defined vector @returns center the new center */
    305    inline const Vector& getCenter() const { return this->center; }
    306 
    307    /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */
    308    inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }
    309    /** \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*/
    310    inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }
    311    /** \brief gets one axis length of the rectangle  @returns the length of the axis 0 */
    312    inline float getAxis() { return this-> axis[0]; }
    313 
    314   private:
    315     Vector          center;
    316     float           axis[2];
    317 };
    318 
    319 
    320 #endif /* __VECTOR_H_ */
    321 
  • trunk/src/lib/math/vector.cc

    r5688 r6616  
    6666  PRINT(0)(" lenght: %f", len());
    6767  PRINT(0)("\n");
    68 }
    69 
    70 /////////////////
    71 /* QUATERNIONS */
    72 /////////////////
    73 /**
    74  *  calculates a lookAt rotation
    75  * @param dir: the direction you want to look
    76  * @param up: specify what direction up should be
    77 
    78    Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
    79    the same way as dir. If you want to use this with cameras, you'll have to reverse the
    80    dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
    81    can use this for meshes as well (then you do not have to reverse the vector), but keep
    82    in mind that if you do that, the model's front has to point in +z direction, and left
    83    and right should be -x or +x respectively or the mesh wont rotate correctly.
    84  *
    85  * @TODO !!! OPTIMIZE THIS !!!
    86  */
    87 Quaternion::Quaternion (const Vector& dir, const Vector& up)
    88 {
    89   Vector z = dir.getNormalized();
    90   Vector x = up.cross(z).getNormalized();
    91   Vector y = z.cross(x);
    92 
    93   float m[4][4];
    94   m[0][0] = x.x;
    95   m[0][1] = x.y;
    96   m[0][2] = x.z;
    97   m[0][3] = 0;
    98   m[1][0] = y.x;
    99   m[1][1] = y.y;
    100   m[1][2] = y.z;
    101   m[1][3] = 0;
    102   m[2][0] = z.x;
    103   m[2][1] = z.y;
    104   m[2][2] = z.z;
    105   m[2][3] = 0;
    106   m[3][0] = 0;
    107   m[3][1] = 0;
    108   m[3][2] = 0;
    109   m[3][3] = 1;
    110 
    111   *this = Quaternion (m);
    112 }
    113 
    114 /**
    115  *  calculates a rotation from euler angles
    116  * @param roll: the roll in radians
    117  * @param pitch: the pitch in radians
    118  * @param yaw: the yaw in radians
    119  */
    120 Quaternion::Quaternion (float roll, float pitch, float yaw)
    121 {
    122   float cr, cp, cy, sr, sp, sy, cpcy, spsy;
    123 
    124   // calculate trig identities
    125   cr = cos(roll/2);
    126   cp = cos(pitch/2);
    127   cy = cos(yaw/2);
    128 
    129   sr = sin(roll/2);
    130   sp = sin(pitch/2);
    131   sy = sin(yaw/2);
    132 
    133   cpcy = cp * cy;
    134   spsy = sp * sy;
    135 
    136   w = cr * cpcy + sr * spsy;
    137   v.x = sr * cpcy - cr * spsy;
    138   v.y = cr * sp * cy + sr * cp * sy;
    139   v.z = cr * cp * sy - sr * sp * cy;
    140 }
    141 
    142 /**
    143  *  convert the Quaternion to a 4x4 rotational glMatrix
    144  * @param m: a buffer to store the Matrix in
    145  */
    146 void Quaternion::matrix (float m[4][4]) const
    147 {
    148   float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
    149 
    150   // calculate coefficients
    151   x2 = v.x + v.x;
    152   y2 = v.y + v.y;
    153   z2 = v.z + v.z;
    154   xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
    155   yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
    156   wx = w * x2; wy = w * y2; wz = w * z2;
    157 
    158   m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
    159   m[2][0] = xz + wy; m[3][0] = 0.0;
    160 
    161   m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
    162   m[2][1] = yz - wx; m[3][1] = 0.0;
    163 
    164   m[0][2] = xz - wy; m[1][2] = yz + wx;
    165   m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
    166 
    167   m[0][3] = 0; m[1][3] = 0;
    168   m[2][3] = 0; m[3][3] = 1;
    169 }
    170 
    171 /**
    172  *  performs a smooth move.
    173  * @param from  where
    174  * @param to where
    175  * @param t the time this transformation should take value [0..1]
    176  * @returns the Result of the smooth move
    177  */
    178 Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
    179 {
    180   float tol[4];
    181   double omega, cosom, sinom, scale0, scale1;
    182   //  float DELTA = 0.2;
    183 
    184   cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
    185 
    186   if( cosom < 0.0 )
    187     {
    188       cosom = -cosom;
    189       tol[0] = -to.v.x;
    190       tol[1] = -to.v.y;
    191       tol[2] = -to.v.z;
    192       tol[3] = -to.w;
    193     }
    194   else
    195     {
    196       tol[0] = to.v.x;
    197       tol[1] = to.v.y;
    198       tol[2] = to.v.z;
    199       tol[3] = to.w;
    200     }
    201 
    202   omega = acos(cosom);
    203   sinom = sin(omega);
    204   scale0 = sin((1.0 - t) * omega) / sinom;
    205   scale1 = sin(t * omega) / sinom;
    206   return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
    207                     scale0 * from.v.y + scale1 * tol[1],
    208                     scale0 * from.v.z + scale1 * tol[2]),
    209                     scale0 * from.w + scale1 * tol[3]);
    210 }
    211 
    212 
    213 /**
    214  *  convert a rotational 4x4 glMatrix into a Quaternion
    215  * @param m: a 4x4 matrix in glMatrix order
    216  */
    217 Quaternion::Quaternion (float m[4][4])
    218 {
    219 
    220   float  tr, s, q[4];
    221   int    i, j, k;
    222 
    223   int nxt[3] = {1, 2, 0};
    224 
    225   tr = m[0][0] + m[1][1] + m[2][2];
    226 
    227         // check the diagonal
    228   if (tr > 0.0)
    229   {
    230     s = sqrt (tr + 1.0);
    231     w = s / 2.0;
    232     s = 0.5 / s;
    233     v.x = (m[1][2] - m[2][1]) * s;
    234     v.y = (m[2][0] - m[0][2]) * s;
    235     v.z = (m[0][1] - m[1][0]) * s;
    236         }
    237         else
    238         {
    239                 // diagonal is negative
    240         i = 0;
    241         if (m[1][1] > m[0][0]) i = 1;
    242     if (m[2][2] > m[i][i]) i = 2;
    243     j = nxt[i];
    244     k = nxt[j];
    245 
    246     s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
    247 
    248     q[i] = s * 0.5;
    249 
    250     if (s != 0.0) s = 0.5 / s;
    251 
    252           q[3] = (m[j][k] - m[k][j]) * s;
    253     q[j] = (m[i][j] + m[j][i]) * s;
    254     q[k] = (m[i][k] + m[k][i]) * s;
    255 
    256         v.x = q[0];
    257         v.y = q[1];
    258         v.z = q[2];
    259         w = q[3];
    260   }
    261 }
    262 
    263 /**
    264  *  outputs some nice formated debug information about this quaternion
    265 */
    266 void Quaternion::debug()
    267 {
    268   PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
    269 }
    270 
    271 void Quaternion::debug2()
    272 {
    273   Vector axis = this->getSpacialAxis();
    274   PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
    27568}
    27669
  • trunk/src/lib/math/vector.h

    r5982 r6616  
    125125
    126126
    127 //! Quaternion
    128 /**
    129    Class to handle 3-dimensional rotation efficiently
    130 */
    131 class Quaternion
    132 {
    133  public:
    134   /** creates a Default quaternion (multiplicational identity Quaternion)*/
    135   inline Quaternion () { w = 1; v = Vector(0,0,0); }
    136   /** creates a Quaternion looking into the direction v @param v: the direction @param f: the value */
    137   inline Quaternion (const Vector& v, float f) { this->w = f; this->v = v; }
    138   Quaternion (float m[4][4]);
    139   /** turns a rotation along an axis into a Quaternion @param angle: the amount of radians to rotate @param axis: the axis to rotate around */
    140   inline Quaternion (float angle, const Vector& axis) { w = cos(angle/2); v = axis * sin(angle/2); }
    141   Quaternion (const Vector& dir, const Vector& up);
    142   Quaternion (float roll, float pitch, float yaw);
    143 
    144   /** @param q: the Quaternion to compare with this one. @returns true if the Quaternions are the same, false otherwise */
    145   inline bool operator== (const Quaternion& q) const { return (unlikely(this->v==q.v&&this->w==q.w))?true:false; };
    146   /** @param f: a real value @return a Quaternion containing the quotient */
    147   inline Quaternion operator/ (const float& f) const { return (unlikely(f==0.0)) ? Quaternion() : Quaternion(this->v/f, this->w/f); };
    148   /** @param f: the value to divide by @returns the quaternion devided by f (this /= f) */
    149   inline const Quaternion& operator/= (const float& f) {*this = *this / f; return *this;}
    150   /** @param f: a real value @return a Quaternion containing the product */
    151   inline Quaternion operator* (const float& f) const { return Quaternion(this->v*f, this->w*f); };
    152   /** @param f: the value to multiply by @returns the quaternion multiplied by f (this *= f) */
    153   inline const Quaternion& operator*= (const float& f) {*this = *this * f; return *this;}
    154   /** @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)) */
    155   Quaternion operator* (const Quaternion& q) const { return Quaternion(Vector(this->w*q.v.x + this->v.x*q.w + this->v.y*q.v.z - this->v.z*q.v.y,
    156                                                                          this->w*q.v.y + this->v.y*q.w + this->v.z*q.v.x - this->v.x*q.v.z,
    157                                                                          this->w*q.v.z + this->v.z*q.w + this->v.x*q.v.y - this->v.y*q.v.x),
    158                                                                          this->w*q.w - this->v.x*q.v.x - this->v.y*q.v.y - this->v.z*q.v.z); };
    159   /** @param q: the Quaternion to multiply by @returns the quaternion multiplied by q (this *= q) */
    160   inline const Quaternion& operator*= (const Quaternion& q) {*this = *this * q; return *this; };
    161   /** @param q the Quaternion by which to devide @returns the division from this by q (this / q) */
    162   inline Quaternion operator/ (const Quaternion& q) const { return *this * q.inverse(); };
    163   /** @param q the Quaternion by which to devide @returns the division from this by q (this /= q) */
    164   inline const Quaternion& operator/= (const Quaternion& q) { *this = *this * q.inverse(); return *this; };
    165   /** @param q the Quaternion to add to this @returns the quaternion added with q (this + q) */
    166   inline Quaternion operator+ (const Quaternion& q) const { return Quaternion(q.v + v, q.w + w); };
    167   /** @param q the Quaternion to add to this @returns the quaternion added with q (this += q) */
    168   inline const Quaternion& operator+= (const Quaternion& q) { this->v += q.v; this->w += q.w; return *this; };
    169   /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this - q) */
    170   inline Quaternion operator- (const Quaternion& q) const { return Quaternion(q.v - v, q.w - w); }
    171   /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this -= q) */
    172   inline const Quaternion& operator-= (const Quaternion& q) { this->v -= q.v; this->w -= q.w; return *this; };
    173   /** copy constructor @param q: the Quaternion to set this to. @returns the Quaternion q (or this) */
    174   inline Quaternion operator= (const Quaternion& q) {this->v = q.v; this->w = q.w; return *this;}
    175   /** conjugates this Quaternion @returns the conjugate */
    176   inline Quaternion conjugate () const { return Quaternion(Vector(-v.x, -v.y, -v.z), this->w); };
    177   /** @returns the norm of The Quaternion */
    178   inline float norm () const { return sqrt(w*w + v.x*v.x + v.y*v.y + v.z*v.z); };
    179   /** @returns the inverted Quaterntion of this */
    180   inline Quaternion inverse () const { return conjugate() / (w*w + v.x*v.x + v.y*v.y + v.z*v.z); };
    181   /** @returns the dot Product of a Quaternion */
    182   inline float dot (const Quaternion& q) const { return v.x*q.v.x + v.y*q.v.y + v.z*q.v.z + w*q.w; };
    183   /** @retuns the Distance between two Quaternions */
    184   inline float distance(const Quaternion& q) const { return 2*acos(fabsf(this->dot(q))); };
    185   /** @param v: the Vector  @return a new Vector representing v rotated by the Quaternion */
    186   inline Vector apply (const Vector& v) const { return (*this * Quaternion(v, 0) * conjugate()).v; };
    187   void matrix (float m[4][4]) const;
    188   /** @returns the normalized Quaternion (|this|) */
    189   inline Quaternion getNormalized() const { float n = this->norm(); return Quaternion(this->v/n, this->w/n); };
    190   /** normalizes the current Quaternion */
    191   inline void normalize() { float n = this->norm(); this->v /= n; this->w/=n; };
    192 
    193   /** @returns the rotational axis of this Quaternion */
    194   inline Vector getSpacialAxis() const { return this->v / sin(acos(w));/*sqrt(v.x*v.x + v.y*v.y + v.z+v.z);*/ };
    195   /** @returns the rotational angle of this Quaternion around getSpacialAxis()  !! IN DEGREE !! */
    196   inline float getSpacialAxisAngle() const { return 360.0 / M_PI * acos( this->w ); };
    197 
    198   static Quaternion quatSlerp(const Quaternion& from, const Quaternion& to, float t);
    199 
    200   void debug();
    201   void debug2();
    202 
    203 
    204  public:
    205   Vector    v;        //!< Imaginary Vector
    206   float     w;        //!< Real part of the number
    207 
    208 };
    209 
    210 
    211 
    212 
    213127//! 3D rotation (OBSOLETE)
    214128/**
  • trunk/src/lib/math/vector2D.h

    r6615 r6616  
    1010
    1111### File Specific:
    12    main-programmer: Christian Meyer
     12   main-programmer: Benjamin Grauer
    1313   co-programmer: ...
    1414*/
    1515
    1616/*!
    17  * @file vector.h
    18  * A basic 3D math framework
     17 * @file vector2D.h
     18 * A basic 2D Vector math framework
    1919 *
    2020 * Contains classes to handle vectors, lines, rotations and planes
  • trunk/src/lib/physics/physics_interface.h

    r5257 r6616  
    1010
    1111#include "vector.h"
    12 
     12#include "quaternion.h"
    1313#ifndef NULL
    1414#define NULL 0          //!< NULL
  • trunk/src/util/animation/animation3d.h

    r5039 r6616  
    77
    88#include "vector.h"
     9#include "quaternion.h"
    910class PNode;
    1011
Note: See TracChangeset for help on using the changeset viewer.