Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 6617 in orxonox.OLD for trunk/src/lib/math


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

trunk: split Rotation/Line/Quaternion/Plane(Rectangle) into seperate files

Location:
trunk/src/lib/math
Files:
2 edited
6 copied

Legend:

Unmodified
Added
Removed

  • trunk/src/lib/math/line.cc

    r6616 r6617  
    1313   co-programmer: Patrick Boenzli : Vector::scale()
    1414                                    Vector::abs()
    15 
    16    Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
    17 
    18    2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now)
    1915*/
    2016
    2117#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
    2218
    23 #include "vector.h"
     19#include "line.h"
     20#include "plane.h"
    2421#ifdef DEBUG
    2522  #include "debug.h"
    … …  
    3027
    3128using 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 }
    69 
    70 /**
    71  *  create a rotation from a vector
    72  * @param v: a vector
    73 */
    74 Rotation::Rotation (const Vector& v)
    75 {
    76   Vector x = Vector( 1, 0, 0);
    77   Vector axis = x.cross( v);
    78   axis.normalize();
    79   float angle = angleRad( x, v);
    80   float ca = cos(angle);
    81   float sa = sin(angle);
    82   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    83   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    84   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    85   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    86   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    87   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    88   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    89   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    90   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    91 }
    92 
    93 /**
    94  *  creates a rotation from an axis and an angle (radians!)
    95  * @param axis: the rotational axis
    96  * @param angle: the angle in radians
    97 */
    98 Rotation::Rotation (const Vector& axis, float angle)
    99 {
    100   float ca, sa;
    101   ca = cos(angle);
    102   sa = sin(angle);
    103   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    104   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    105   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    106   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    107   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    108   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    109   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    110   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    111   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    112 }
    113 
    114 /**
    115  *  creates a rotation from euler angles (pitch/yaw/roll)
    116  * @param pitch: rotation around z (in radians)
    117  * @param yaw: rotation around y (in radians)
    118  * @param roll: rotation around x (in radians)
    119 */
    120 Rotation::Rotation ( float pitch, float yaw, float roll)
    121 {
    122   float cy, sy, cr, sr, cp, sp;
    123   cy = cos(yaw);
    124   sy = sin(yaw);
    125   cr = cos(roll);
    126   sr = sin(roll);
    127   cp = cos(pitch);
    128   sp = sin(pitch);
    129   m[0] = cy*cr;
    130   m[1] = -cy*sr;
    131   m[2] = sy;
    132   m[3] = cp*sr+sp*sy*cr;
    133   m[4] = cp*cr-sp*sr*sy;
    134   m[5] = -sp*cy;
    135   m[6] = sp*sr-cp*sy*cr;
    136   m[7] = sp*cr+cp*sy*sr;
    137   m[8] = cp*cy;
    138 }
    139 
    140 /**
    141  *  creates a nullrotation (an identity rotation)
    142 */
    143 Rotation::Rotation ()
    144 {
    145   m[0] = 1.0f;
    146   m[1] = 0.0f;
    147   m[2] = 0.0f;
    148   m[3] = 0.0f;
    149   m[4] = 1.0f;
    150   m[5] = 0.0f;
    151   m[6] = 0.0f;
    152   m[7] = 0.0f;
    153   m[8] = 1.0f;
    154 }
    155 
    156 /**
    157  *  fills the specified buffer with a 4x4 glmatrix
    158  * @param buffer: Pointer to an array of 16 floats
    159 
    160    Use this to get the rotation in a gl-compatible format
    161 */
    162 void Rotation::glmatrix (float* buffer)
    163 {
    164         buffer[0] = m[0];
    165         buffer[1] = m[3];
    166         buffer[2] = m[6];
    167         buffer[3] = m[0];
    168         buffer[4] = m[1];
    169         buffer[5] = m[4];
    170         buffer[6] = m[7];
    171         buffer[7] = m[0];
    172         buffer[8] = m[2];
    173         buffer[9] = m[5];
    174         buffer[10] = m[8];
    175         buffer[11] = m[0];
    176         buffer[12] = m[0];
    177         buffer[13] = m[0];
    178         buffer[14] = m[0];
    179         buffer[15] = m[1];
    180 }
    181 
    182 /**
    183  *  multiplies two rotational matrices
    184  * @param r: another Rotation
    185  * @return the matrix product of the Rotations
    186 
    187    Use this to rotate one rotation by another
    188 */
    189 Rotation Rotation::operator* (const Rotation& r)
    190 {
    191         Rotation p;
    192 
    193         p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
    194         p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
    195         p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
    196 
    197         p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
    198         p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
    199         p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
    200 
    201         p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
    202         p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
    203         p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
    204 
    205         return p;
    206 }
    207 
    208 
    209 /**
    210  *  rotates the vector by the given rotation
    211  * @param v: a vector
    212  * @param r: a rotation
    213  * @return the rotated vector
    214 */
    215 Vector rotateVector( const Vector& v, const Rotation& r)
    216 {
    217   Vector t;
    218 
    219   t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
    220   t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
    221   t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
    222 
    223   return t;
    224 }
    22529
    22630/**
    … …  
    299103  a = t - r;
    300104}
    301 
    302 /**
    303  *  create a plane from three points
    304  * @param a: first point
    305  * @param b: second point
    306  * @param c: third point
    307 */
    308 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)
    309 {
    310   n = (a-b).cross(c-b);
    311   k = -(n.x*b.x+n.y*b.y+n.z*b.z);
    312 }
    313 
    314 /**
    315  *  create a plane from anchor point and normal
    316  * @param norm: normal vector
    317  * @param p: anchor point
    318 */
    319 Plane::Plane (const Vector& norm, const Vector& p)
    320 {
    321   n = norm;
    322   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    323 }
    324 
    325 
    326 /**
    327   *  create a plane from anchor point and normal
    328   * @param norm: normal vector
    329   * @param p: anchor point
    330 */
    331 Plane::Plane (const Vector& norm, const sVec3D& g)
    332 {
    333   Vector p(g[0], g[1], g[2]);
    334   n = norm;
    335   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    336 }
    337 
    338 
    339 /**
    340  *  returns the intersection point between the plane and a line
    341  * @param l: a line
    342 */
    343 Vector Plane::intersectLine (const Line& l) const
    344 {
    345   if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
    346   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);
    347   return l.r + (l.a * t);
    348 }
    349 
    350 /**
    351  *  returns the distance between the plane and a point
    352  * @param p: a Point
    353  * @return the distance between the plane and the point (can be negative)
    354 */
    355 float Plane::distancePoint (const Vector& p) const
    356 {
    357   float l = n.len();
    358   if( l == 0.0) return 0.0;
    359   return (n.dot(p) + k) / n.len();
    360 }
    361 
    362 
    363 /**
    364  *  returns the distance between the plane and a point
    365  * @param p: a Point
    366  * @return the distance between the plane and the point (can be negative)
    367  */
    368 float Plane::distancePoint (const sVec3D& p) const
    369 {
    370   Vector s(p[0], p[1], p[2]);
    371   float l = n.len();
    372   if( l == 0.0) return 0.0;
    373   return (n.dot(s) + k) / n.len();
    374 }
    375 
    376 
    377 /**
    378  *  returns the side a point is located relative to a Plane
    379  * @param p: a Point
    380  * @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
    381 */
    382 float Plane::locatePoint (const Vector& p) const
    383 {
    384   return (n.dot(p) + k);
    385 }
    386 

  • trunk/src/lib/math/line.h

    r6616 r6617  
    1515
    1616/*!
    17  * @file vector.h
    18  * A basic 3D math framework
     17 * @file line.h
     18 * A basic 3D math line framework
    1919 *
    20  * Contains classes to handle vectors, lines, rotations and planes
     20 * Contains class to handle lines
    2121*/
    2222
    23 #ifndef __VECTOR_H_
    24 #define __VECTOR_H_
     23#ifndef __LINE_H_
     24#define __LINE_H_
    2525
    2626#include <math.h>
    2727#include "compiler.h"
    28 //! PI the circle-constant
    29 #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 
    126 
    127 //! 3D rotation (OBSOLETE)
    128 /**
    129   Class to handle 3-dimensional rotations.
    130   Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix
    131 */
    132 class Rotation {
    133   public:
    134 
    135   float m[9]; //!< 3x3 Rotation Matrix
    136 
    137   Rotation ( const Vector& v);
    138   Rotation ( const Vector& axis, float angle);
    139   Rotation ( float pitch, float yaw, float roll);
    140   Rotation ();
    141   ~Rotation () {}
    142 
    143   Rotation operator* (const Rotation& r);
    144 
    145   void glmatrix (float* buffer);
    146 };
    147 
    148 //!< Apply a rotation to a vector
    149 Vector rotateVector( const Vector& v, const Rotation& r);
     28#include "vector.h"
     29#include "rotation_OBSOLETE.h"
    15030
    15131//! 3D line
    … …  
    17555};
    17656
    177 //! 3D plane
    178 /**
    179   Class to handle planes in 3-dimensional space
    18057
    181   Critical for polygon-based collision detection
    182 */
    183 class Plane
    184 {
    185   public:
     58#endif /* __LINE_H_ */
    18659
    187   Vector n;   //!< Normal vector
    188   float k;    //!< Offset constant
    189 
    190   Plane (const Vector& a, const Vector& b, const Vector& c);
    191   Plane (const Vector& norm, const Vector& p);
    192   Plane (const Vector& norm, const sVec3D& p);
    193   Plane (const Vector& n, float k) : n(n), k(k) {} //!< assignment constructor
    194   Plane () : n(Vector(1,1,1)), k(0) {}
    195   ~Plane () {}
    196 
    197   Vector intersectLine (const Line& l) const;
    198   float distancePoint (const Vector& p) const;
    199   float distancePoint (const sVec3D& p) const;
    200   float locatePoint (const Vector& p) const;
    201 };
    202 
    203 
    204 
    205 //! A class that represents a rectangle, this is needed for SpatialSeparation
    206 class Rectangle
    207 {
    208 
    209   public:
    210     Rectangle() { this->center = Vector(); }
    211     Rectangle(const Vector &center, float len) { this->center = Vector(center.x, center.y, center.z); this->axis[0] = len; this->axis[1] = len; }
    212     virtual ~Rectangle() {}
    213 
    214     /** \brief sets the center of the rectangle to a defined vector @param center the new center */
    215    inline void setCenter(const Vector &center) { this->center = center;}
    216     /** \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 */
    217    inline void setCenter(float x, float y, float z) { this->center.x = x; this->center.y = y; this->center.z = z; }
    218    /** \brief returns the center of the rectangle to a defined vector @returns center the new center */
    219    inline const Vector& getCenter() const { return this->center; }
    220 
    221    /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */
    222    inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }
    223    /** \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*/
    224    inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }
    225    /** \brief gets one axis length of the rectangle  @returns the length of the axis 0 */
    226    inline float getAxis() { return this-> axis[0]; }
    227 
    228   private:
    229     Vector          center;
    230     float           axis[2];
    231 };
    232 
    233 
    234 #endif /* __VECTOR_H_ */
    235 

  • trunk/src/lib/math/plane.cc

    r6616 r6617  
    1111   ### File Specific:
    1212   main-programmer: Christian Meyer
    13    co-programmer: Patrick Boenzli : Vector::scale()
    14                                     Vector::abs()
    15 
    16    Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
    17 
    18    2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now)
     13   co-programmer: Patrick Boenzli
    1914*/
    2015
    2116#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
    2217
    23 #include "vector.h"
     18#include "plane.h"
    2419#ifdef DEBUG
    2520  #include "debug.h"
    … …  
    3025
    3126using 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 }
    6927
    7028/**

  • trunk/src/lib/math/plane.h

    r6616 r6617  
    1515
    1616/*!
    17  * @file vector.h
    18  * A basic 3D math framework
     17 * @file plane.h
     18 * A basic 3D plane math framework
    1919 *
    20  * Contains classes to handle vectors, lines, rotations and planes
     20 * Contains class to handle planes
    2121*/
    2222
    23 #ifndef __VECTOR_H_
    24 #define __VECTOR_H_
     23#ifndef __PLANE_H_
     24#define __PLANE_H_
    2525
    2626#include <math.h>
    2727#include "compiler.h"
    28 //! PI the circle-constant
    29 #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 
    126 
    127 //! 3D rotation (OBSOLETE)
    128 /**
    129   Class to handle 3-dimensional rotations.
    130   Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix
    131 */
    132 class Rotation {
    133   public:
    134 
    135   float m[9]; //!< 3x3 Rotation Matrix
    136 
    137   Rotation ( const Vector& v);
    138   Rotation ( const Vector& axis, float angle);
    139   Rotation ( float pitch, float yaw, float roll);
    140   Rotation ();
    141   ~Rotation () {}
    142 
    143   Rotation operator* (const Rotation& r);
    144 
    145   void glmatrix (float* buffer);
    146 };
    147 
    148 //!< Apply a rotation to a vector
    149 Vector rotateVector( const Vector& v, const Rotation& r);
    150 
    151 //! 3D line
    152 /**
    153   Class to store Lines in 3-dimensional space
    154 
    155   Supports line-to-line distance measurements and rotation
    156 */
    157 class Line
    158 {
    159   public:
    160 
    161   Vector r;   //!< Offset
    162   Vector a;   //!< Direction
    163 
    164   Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
    165   Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
    166   ~Line () {}
    167 
    168   float distance (const Line& l) const;
    169   float distancePoint (const Vector& v) const;
    170   float distancePoint (const sVec3D& v) const;
    171   Vector* footpoints (const Line& l) const;
    172   float len () const;
    173 
    174   void rotate(const Rotation& rot);
    175 };
     28#include "vector.h"
     29#include "line.h"
    17630
    17731//! 3D plane
    … …  
    23286
    23387
    234 #endif /* __VECTOR_H_ */
     88#endif /* __PLANE_H_ */
    23589

  • trunk/src/lib/math/rotation_OBSOLETE.cc

    r6616 r6617  
    1111   ### File Specific:
    1212   main-programmer: Christian Meyer
    13    co-programmer: Patrick Boenzli : Vector::scale()
    14                                     Vector::abs()
    15 
    16    Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
    17 
    18    2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now)
     13   co-programmer: ...
    1914*/
    2015
    2116#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
    2217
    23 #include "vector.h"
     18#include "rotation_OBSOLETE.h"
    2419#ifdef DEBUG
    2520  #include "debug.h"
    … …  
    3025
    3126using 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 }
    6927
    7028/**
    … …  
    223181  return t;
    224182}
    225 
    226 /**
    227  *  calculate the distance between two lines
    228  * @param l: the other line
    229  * @return the distance between the lines
    230 */
    231 float Line::distance (const Line& l) const
    232 {
    233   float q, d;
    234   Vector n = a.cross(l.a);
    235   q = n.dot(r-l.r);
    236   d = n.len();
    237   if( d == 0.0) return 0.0;
    238   return q/d;
    239 }
    240 
    241 /**
    242  *  calculate the distance between a line and a point
    243  * @param v: the point
    244  * @return the distance between the Line and the point
    245 */
    246 float Line::distancePoint (const Vector& v) const
    247 {
    248   Vector d = v-r;
    249   Vector u = a * d.dot( a);
    250   return (d - u).len();
    251 }
    252 
    253 /**
    254  *  calculate the distance between a line and a point
    255  * @param v: the point
    256  * @return the distance between the Line and the point
    257  */
    258 float Line::distancePoint (const sVec3D& v) const
    259 {
    260   Vector s(v[0], v[1], v[2]);
    261   Vector d = s - r;
    262   Vector u = a * d.dot( a);
    263   return (d - u).len();
    264 }
    265 
    266 /**
    267  *  calculate the two points of minimal distance of two lines
    268  * @param l: the other line
    269  * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
    270 */
    271 Vector* Line::footpoints (const Line& l) const
    272 {
    273   Vector* fp = new Vector[2];
    274   Plane p = Plane (r + a.cross(l.a), r, r + a);
    275   fp[1] = p.intersectLine (l);
    276   p = Plane (fp[1], l.a);
    277   fp[0] = p.intersectLine (*this);
    278   return fp;
    279 }
    280 
    281 /**
    282   \brief calculate the length of a line
    283   \return the lenght of the line
    284 */
    285 float Line::len() const
    286 {
    287   return a.len();
    288 }
    289 
    290 /**
    291  *  rotate the line by given rotation
    292  * @param rot: a rotation
    293 */
    294 void Line::rotate (const Rotation& rot)
    295 {
    296   Vector t = a + r;
    297   t = rotateVector( t, rot);
    298   r = rotateVector( r, rot),
    299   a = t - r;
    300 }
    301 
    302 /**
    303  *  create a plane from three points
    304  * @param a: first point
    305  * @param b: second point
    306  * @param c: third point
    307 */
    308 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)
    309 {
    310   n = (a-b).cross(c-b);
    311   k = -(n.x*b.x+n.y*b.y+n.z*b.z);
    312 }
    313 
    314 /**
    315  *  create a plane from anchor point and normal
    316  * @param norm: normal vector
    317  * @param p: anchor point
    318 */
    319 Plane::Plane (const Vector& norm, const Vector& p)
    320 {
    321   n = norm;
    322   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    323 }
    324 
    325 
    326 /**
    327   *  create a plane from anchor point and normal
    328   * @param norm: normal vector
    329   * @param p: anchor point
    330 */
    331 Plane::Plane (const Vector& norm, const sVec3D& g)
    332 {
    333   Vector p(g[0], g[1], g[2]);
    334   n = norm;
    335   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    336 }
    337 
    338 
    339 /**
    340  *  returns the intersection point between the plane and a line
    341  * @param l: a line
    342 */
    343 Vector Plane::intersectLine (const Line& l) const
    344 {
    345   if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
    346   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);
    347   return l.r + (l.a * t);
    348 }
    349 
    350 /**
    351  *  returns the distance between the plane and a point
    352  * @param p: a Point
    353  * @return the distance between the plane and the point (can be negative)
    354 */
    355 float Plane::distancePoint (const Vector& p) const
    356 {
    357   float l = n.len();
    358   if( l == 0.0) return 0.0;
    359   return (n.dot(p) + k) / n.len();
    360 }
    361 
    362 
    363 /**
    364  *  returns the distance between the plane and a point
    365  * @param p: a Point
    366  * @return the distance between the plane and the point (can be negative)
    367  */
    368 float Plane::distancePoint (const sVec3D& p) const
    369 {
    370   Vector s(p[0], p[1], p[2]);
    371   float l = n.len();
    372   if( l == 0.0) return 0.0;
    373   return (n.dot(s) + k) / n.len();
    374 }
    375 
    376 
    377 /**
    378  *  returns the side a point is located relative to a Plane
    379  * @param p: a Point
    380  * @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
    381 */
    382 float Plane::locatePoint (const Vector& p) const
    383 {
    384   return (n.dot(p) + k);
    385 }
    386 

  • trunk/src/lib/math/rotation_OBSOLETE.h

    r6616 r6617  
    1515
    1616/*!
    17  * @file vector.h
    18  * A basic 3D math framework
     17 * @file rotation_OBSOLETE.h
     18 * A basic 3D rotation framework
     19 *
     20 * THIS IS AN OBSOLETE FRAMEWORK, AND WILL BE DELETED SOON
     21 *
    1922 *
    2023 * Contains classes to handle vectors, lines, rotations and planes
    2124*/
    2225
    23 #ifndef __VECTOR_H_
    24 #define __VECTOR_H_
     26#ifndef __ROTATION_H_
     27#define __ROTATION_H_
    2528
    26 #include <math.h>
    27 #include "compiler.h"
    28 //! PI the circle-constant
    29 #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 
     29#include "vector.h"
    12630
    12731//! 3D rotation (OBSOLETE)
    … …  
    14953Vector rotateVector( const Vector& v, const Rotation& r);
    15054
    151 //! 3D line
    152 /**
    153   Class to store Lines in 3-dimensional space
    15455
    155   Supports line-to-line distance measurements and rotation
    156 */
    157 class Line
    158 {
    159   public:
     56#endif /* __ROTATION_H_ */
    16057
    161   Vector r;   //!< Offset
    162   Vector a;   //!< Direction
    163 
    164   Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
    165   Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
    166   ~Line () {}
    167 
    168   float distance (const Line& l) const;
    169   float distancePoint (const Vector& v) const;
    170   float distancePoint (const sVec3D& v) const;
    171   Vector* footpoints (const Line& l) const;
    172   float len () const;
    173 
    174   void rotate(const Rotation& rot);
    175 };
    176 
    177 //! 3D plane
    178 /**
    179   Class to handle planes in 3-dimensional space
    180 
    181   Critical for polygon-based collision detection
    182 */
    183 class Plane
    184 {
    185   public:
    186 
    187   Vector n;   //!< Normal vector
    188   float k;    //!< Offset constant
    189 
    190   Plane (const Vector& a, const Vector& b, const Vector& c);
    191   Plane (const Vector& norm, const Vector& p);
    192   Plane (const Vector& norm, const sVec3D& p);
    193   Plane (const Vector& n, float k) : n(n), k(k) {} //!< assignment constructor
    194   Plane () : n(Vector(1,1,1)), k(0) {}
    195   ~Plane () {}
    196 
    197   Vector intersectLine (const Line& l) const;
    198   float distancePoint (const Vector& p) const;
    199   float distancePoint (const sVec3D& p) const;
    200   float locatePoint (const Vector& p) const;
    201 };
    202 
    203 
    204 
    205 //! A class that represents a rectangle, this is needed for SpatialSeparation
    206 class Rectangle
    207 {
    208 
    209   public:
    210     Rectangle() { this->center = Vector(); }
    211     Rectangle(const Vector &center, float len) { this->center = Vector(center.x, center.y, center.z); this->axis[0] = len; this->axis[1] = len; }
    212     virtual ~Rectangle() {}
    213 
    214     /** \brief sets the center of the rectangle to a defined vector @param center the new center */
    215    inline void setCenter(const Vector &center) { this->center = center;}
    216     /** \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 */
    217    inline void setCenter(float x, float y, float z) { this->center.x = x; this->center.y = y; this->center.z = z; }
    218    /** \brief returns the center of the rectangle to a defined vector @returns center the new center */
    219    inline const Vector& getCenter() const { return this->center; }
    220 
    221    /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */
    222    inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }
    223    /** \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*/
    224    inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }
    225    /** \brief gets one axis length of the rectangle  @returns the length of the axis 0 */
    226    inline float getAxis() { return this-> axis[0]; }
    227 
    228   private:
    229     Vector          center;
    230     float           axis[2];
    231 };
    232 
    233 
    234 #endif /* __VECTOR_H_ */
    235 

  • trunk/src/lib/math/vector.cc

    r6616 r6617  
    6767  PRINT(0)("\n");
    6868}
    69 
    70 /**
    71  *  create a rotation from a vector
    72  * @param v: a vector
    73 */
    74 Rotation::Rotation (const Vector& v)
    75 {
    76   Vector x = Vector( 1, 0, 0);
    77   Vector axis = x.cross( v);
    78   axis.normalize();
    79   float angle = angleRad( x, v);
    80   float ca = cos(angle);
    81   float sa = sin(angle);
    82   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    83   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    84   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    85   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    86   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    87   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    88   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    89   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    90   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    91 }
    92 
    93 /**
    94  *  creates a rotation from an axis and an angle (radians!)
    95  * @param axis: the rotational axis
    96  * @param angle: the angle in radians
    97 */
    98 Rotation::Rotation (const Vector& axis, float angle)
    99 {
    100   float ca, sa;
    101   ca = cos(angle);
    102   sa = sin(angle);
    103   m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
    104   m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    105   m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    106   m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
    107   m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
    108   m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    109   m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
    110   m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
    111   m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
    112 }
    113 
    114 /**
    115  *  creates a rotation from euler angles (pitch/yaw/roll)
    116  * @param pitch: rotation around z (in radians)
    117  * @param yaw: rotation around y (in radians)
    118  * @param roll: rotation around x (in radians)
    119 */
    120 Rotation::Rotation ( float pitch, float yaw, float roll)
    121 {
    122   float cy, sy, cr, sr, cp, sp;
    123   cy = cos(yaw);
    124   sy = sin(yaw);
    125   cr = cos(roll);
    126   sr = sin(roll);
    127   cp = cos(pitch);
    128   sp = sin(pitch);
    129   m[0] = cy*cr;
    130   m[1] = -cy*sr;
    131   m[2] = sy;
    132   m[3] = cp*sr+sp*sy*cr;
    133   m[4] = cp*cr-sp*sr*sy;
    134   m[5] = -sp*cy;
    135   m[6] = sp*sr-cp*sy*cr;
    136   m[7] = sp*cr+cp*sy*sr;
    137   m[8] = cp*cy;
    138 }
    139 
    140 /**
    141  *  creates a nullrotation (an identity rotation)
    142 */
    143 Rotation::Rotation ()
    144 {
    145   m[0] = 1.0f;
    146   m[1] = 0.0f;
    147   m[2] = 0.0f;
    148   m[3] = 0.0f;
    149   m[4] = 1.0f;
    150   m[5] = 0.0f;
    151   m[6] = 0.0f;
    152   m[7] = 0.0f;
    153   m[8] = 1.0f;
    154 }
    155 
    156 /**
    157  *  fills the specified buffer with a 4x4 glmatrix
    158  * @param buffer: Pointer to an array of 16 floats
    159 
    160    Use this to get the rotation in a gl-compatible format
    161 */
    162 void Rotation::glmatrix (float* buffer)
    163 {
    164         buffer[0] = m[0];
    165         buffer[1] = m[3];
    166         buffer[2] = m[6];
    167         buffer[3] = m[0];
    168         buffer[4] = m[1];
    169         buffer[5] = m[4];
    170         buffer[6] = m[7];
    171         buffer[7] = m[0];
    172         buffer[8] = m[2];
    173         buffer[9] = m[5];
    174         buffer[10] = m[8];
    175         buffer[11] = m[0];
    176         buffer[12] = m[0];
    177         buffer[13] = m[0];
    178         buffer[14] = m[0];
    179         buffer[15] = m[1];
    180 }
    181 
    182 /**
    183  *  multiplies two rotational matrices
    184  * @param r: another Rotation
    185  * @return the matrix product of the Rotations
    186 
    187    Use this to rotate one rotation by another
    188 */
    189 Rotation Rotation::operator* (const Rotation& r)
    190 {
    191         Rotation p;
    192 
    193         p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
    194         p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
    195         p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
    196 
    197         p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
    198         p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
    199         p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
    200 
    201         p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
    202         p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
    203         p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
    204 
    205         return p;
    206 }
    207 
    208 
    209 /**
    210  *  rotates the vector by the given rotation
    211  * @param v: a vector
    212  * @param r: a rotation
    213  * @return the rotated vector
    214 */
    215 Vector rotateVector( const Vector& v, const Rotation& r)
    216 {
    217   Vector t;
    218 
    219   t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
    220   t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
    221   t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
    222 
    223   return t;
    224 }
    225 
    226 /**
    227  *  calculate the distance between two lines
    228  * @param l: the other line
    229  * @return the distance between the lines
    230 */
    231 float Line::distance (const Line& l) const
    232 {
    233   float q, d;
    234   Vector n = a.cross(l.a);
    235   q = n.dot(r-l.r);
    236   d = n.len();
    237   if( d == 0.0) return 0.0;
    238   return q/d;
    239 }
    240 
    241 /**
    242  *  calculate the distance between a line and a point
    243  * @param v: the point
    244  * @return the distance between the Line and the point
    245 */
    246 float Line::distancePoint (const Vector& v) const
    247 {
    248   Vector d = v-r;
    249   Vector u = a * d.dot( a);
    250   return (d - u).len();
    251 }
    252 
    253 /**
    254  *  calculate the distance between a line and a point
    255  * @param v: the point
    256  * @return the distance between the Line and the point
    257  */
    258 float Line::distancePoint (const sVec3D& v) const
    259 {
    260   Vector s(v[0], v[1], v[2]);
    261   Vector d = s - r;
    262   Vector u = a * d.dot( a);
    263   return (d - u).len();
    264 }
    265 
    266 /**
    267  *  calculate the two points of minimal distance of two lines
    268  * @param l: the other line
    269  * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
    270 */
    271 Vector* Line::footpoints (const Line& l) const
    272 {
    273   Vector* fp = new Vector[2];
    274   Plane p = Plane (r + a.cross(l.a), r, r + a);
    275   fp[1] = p.intersectLine (l);
    276   p = Plane (fp[1], l.a);
    277   fp[0] = p.intersectLine (*this);
    278   return fp;
    279 }
    280 
    281 /**
    282   \brief calculate the length of a line
    283   \return the lenght of the line
    284 */
    285 float Line::len() const
    286 {
    287   return a.len();
    288 }
    289 
    290 /**
    291  *  rotate the line by given rotation
    292  * @param rot: a rotation
    293 */
    294 void Line::rotate (const Rotation& rot)
    295 {
    296   Vector t = a + r;
    297   t = rotateVector( t, rot);
    298   r = rotateVector( r, rot),
    299   a = t - r;
    300 }
    301 
    302 /**
    303  *  create a plane from three points
    304  * @param a: first point
    305  * @param b: second point
    306  * @param c: third point
    307 */
    308 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)
    309 {
    310   n = (a-b).cross(c-b);
    311   k = -(n.x*b.x+n.y*b.y+n.z*b.z);
    312 }
    313 
    314 /**
    315  *  create a plane from anchor point and normal
    316  * @param norm: normal vector
    317  * @param p: anchor point
    318 */
    319 Plane::Plane (const Vector& norm, const Vector& p)
    320 {
    321   n = norm;
    322   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    323 }
    324 
    325 
    326 /**
    327   *  create a plane from anchor point and normal
    328   * @param norm: normal vector
    329   * @param p: anchor point
    330 */
    331 Plane::Plane (const Vector& norm, const sVec3D& g)
    332 {
    333   Vector p(g[0], g[1], g[2]);
    334   n = norm;
    335   k = -(n.x*p.x+n.y*p.y+n.z*p.z);
    336 }
    337 
    338 
    339 /**
    340  *  returns the intersection point between the plane and a line
    341  * @param l: a line
    342 */
    343 Vector Plane::intersectLine (const Line& l) const
    344 {
    345   if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
    346   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);
    347   return l.r + (l.a * t);
    348 }
    349 
    350 /**
    351  *  returns the distance between the plane and a point
    352  * @param p: a Point
    353  * @return the distance between the plane and the point (can be negative)
    354 */
    355 float Plane::distancePoint (const Vector& p) const
    356 {
    357   float l = n.len();
    358   if( l == 0.0) return 0.0;
    359   return (n.dot(p) + k) / n.len();
    360 }
    361 
    362 
    363 /**
    364  *  returns the distance between the plane and a point
    365  * @param p: a Point
    366  * @return the distance between the plane and the point (can be negative)
    367  */
    368 float Plane::distancePoint (const sVec3D& p) const
    369 {
    370   Vector s(p[0], p[1], p[2]);
    371   float l = n.len();
    372   if( l == 0.0) return 0.0;
    373   return (n.dot(s) + k) / n.len();
    374 }
    375 
    376 
    377 /**
    378  *  returns the side a point is located relative to a Plane
    379  * @param p: a Point
    380  * @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
    381 */
    382 float Plane::locatePoint (const Vector& p) const
    383 {
    384   return (n.dot(p) + k);
    385 }
    386 

  • trunk/src/lib/math/vector.h

    r6616 r6617  
    124124#define VECTOR_RAND(sideLength)  (Vector((float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5) * sideLength)
    125125
    126 
    127 //! 3D rotation (OBSOLETE)
    128 /**
    129   Class to handle 3-dimensional rotations.
    130   Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix
    131 */
    132 class Rotation {
    133   public:
    134 
    135   float m[9]; //!< 3x3 Rotation Matrix
    136 
    137   Rotation ( const Vector& v);
    138   Rotation ( const Vector& axis, float angle);
    139   Rotation ( float pitch, float yaw, float roll);
    140   Rotation ();
    141   ~Rotation () {}
    142 
    143   Rotation operator* (const Rotation& r);
    144 
    145   void glmatrix (float* buffer);
    146 };
    147 
    148 //!< Apply a rotation to a vector
    149 Vector rotateVector( const Vector& v, const Rotation& r);
    150 
    151 //! 3D line
    152 /**
    153   Class to store Lines in 3-dimensional space
    154 
    155   Supports line-to-line distance measurements and rotation
    156 */
    157 class Line
    158 {
    159   public:
    160 
    161   Vector r;   //!< Offset
    162   Vector a;   //!< Direction
    163 
    164   Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
    165   Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
    166   ~Line () {}
    167 
    168   float distance (const Line& l) const;
    169   float distancePoint (const Vector& v) const;
    170   float distancePoint (const sVec3D& v) const;
    171   Vector* footpoints (const Line& l) const;
    172   float len () const;
    173 
    174   void rotate(const Rotation& rot);
    175 };
    176 
    177 //! 3D plane
    178 /**
    179   Class to handle planes in 3-dimensional space
    180 
    181   Critical for polygon-based collision detection
    182 */
    183 class Plane
    184 {
    185   public:
    186 
    187   Vector n;   //!< Normal vector
    188   float k;    //!< Offset constant
    189 
    190   Plane (const Vector& a, const Vector& b, const Vector& c);
    191   Plane (const Vector& norm, const Vector& p);
    192   Plane (const Vector& norm, const sVec3D& p);
    193   Plane (const Vector& n, float k) : n(n), k(k) {} //!< assignment constructor
    194   Plane () : n(Vector(1,1,1)), k(0) {}
    195   ~Plane () {}
    196 
    197   Vector intersectLine (const Line& l) const;
    198   float distancePoint (const Vector& p) const;
    199   float distancePoint (const sVec3D& p) const;
    200   float locatePoint (const Vector& p) const;
    201 };
    202 
    203 
    204 
    205 //! A class that represents a rectangle, this is needed for SpatialSeparation
    206 class Rectangle
    207 {
    208 
    209   public:
    210     Rectangle() { this->center = Vector(); }
    211     Rectangle(const Vector &center, float len) { this->center = Vector(center.x, center.y, center.z); this->axis[0] = len; this->axis[1] = len; }
    212     virtual ~Rectangle() {}
    213 
    214     /** \brief sets the center of the rectangle to a defined vector @param center the new center */
    215    inline void setCenter(const Vector &center) { this->center = center;}
    216     /** \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 */
    217    inline void setCenter(float x, float y, float z) { this->center.x = x; this->center.y = y; this->center.z = z; }
    218    /** \brief returns the center of the rectangle to a defined vector @returns center the new center */
    219    inline const Vector& getCenter() const { return this->center; }
    220 
    221    /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */
    222    inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }
    223    /** \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*/
    224    inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }
    225    /** \brief gets one axis length of the rectangle  @returns the length of the axis 0 */
    226    inline float getAxis() { return this-> axis[0]; }
    227 
    228   private:
    229     Vector          center;
    230     float           axis[2];
    231 };
    232 
    233 
    234126#endif /* __VECTOR_H_ */
    235127
Note: See TracChangeset for help on using the changeset viewer.