Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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


Ignore:
Timestamp:
Jun 10, 2005, 2:52:46 AM (20 years ago)
Author:
patrick
Message:

orxonox/trunk: box dimension alg implemented, got some incontinuities

Location:
orxonox/trunk/src/lib/math
Files:
2 edited

Legend:

Unmodified
Added
Removed

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

    r4477 r4578  
    1 /* 
     1/*
    22   orxonox - the future of 3D-vertical-scrollers
    33
    … …  
    1010
    1111   ### File Specific:
    12    main-programmer: Christian Meyer 
     12   main-programmer: Christian Meyer
    1313   co-programmer: Patrick Boenzli : Vector::scale()
    1414                                    Vector::abs()
    15    
     15
    1616   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
    1717*/
    … …  
    4848   \brief Vector is looking in the positive direction on all axes after this
    4949*/
    50 Vector Vector::abs() 
     50Vector Vector::abs()
    5151{
    5252  Vector v(fabs(x), fabs(y), fabs(z));
    … …  
    7474   \param dir: the direction you want to look
    7575   \param up: specify what direction up should be
    76    
     76
    7777   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
    7878   the same way as dir. If you want to use this with cameras, you'll have to reverse the
    7979   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
    80    can use this for meshes as well (then you do not have to reverse the vector), but keep 
    81    in mind that if you do that, the model's front has to point in +z direction, and left 
     80   can use this for meshes as well (then you do not have to reverse the vector), but keep
     81   in mind that if you do that, the model's front has to point in +z direction, and left
    8282   and right should be -x or +x respectively or the mesh wont rotate correctly.
    8383*/
    … …  
    8585{
    8686  Vector z = dir;
    87   z.normalize(); 
     87  z.normalize();
    8888  Vector x = up.cross(z);
    89   x.normalize(); 
     89  x.normalize();
    9090  Vector y = z.cross(x);
    91  
     91
    9292  float m[4][4];
    9393  m[0][0] = x.x;
    … …  
    107107  m[3][2] = 0;
    108108  m[3][3] = 1;
    109  
     109
    110110  *this = Quaternion (m);
    111111}
    … …  
    120120{
    121121  float cr, cp, cy, sr, sp, sy, cpcy, spsy;
    122  
     122
    123123  // calculate trig identities
    124124  cr = cos(roll/2);
    125125  cp = cos(pitch/2);
    126126  cy = cos(yaw/2);
    127  
     127
    128128  sr = sin(roll/2);
    129129  sp = sin(pitch/2);
    130130  sy = sin(yaw/2);
    131  
     131
    132132  cpcy = cp * cy;
    133133  spsy = sp * sy;
    134  
     134
    135135  w = cr * cpcy + sr * spsy;
    136136  v.x = sr * cpcy - cr * spsy;
    … …  
    147147{
    148148  float A, B, C, D, E, F, G, H;
    149  
     149
    150150  A = (w   + v.x)*(q.w   + q.v.x);
    151151  B = (v.z - v.y)*(q.v.y - q.v.z);
    152   C = (w   - v.x)*(q.v.y + q.v.z); 
     152  C = (w   - v.x)*(q.v.y + q.v.z);
    153153  D = (v.y + v.z)*(q.w   - q.v.x);
    154154  E = (v.x + v.z)*(q.v.x + q.v.y);
    … …  
    156156  G = (w   + v.y)*(q.w   - q.v.z);
    157157  H = (w   - v.y)*(q.w   + q.v.z);
    158  
     158
    159159  Quaternion r;
    160160  r.v.x = A - (E + F + G + H)/2;
    … …  
    234234   \brief calculate the inverse value of the Quaternion
    235235   \return the inverse Quaternion
    236    
    237         Note that this is equal to conjugate() if the Quaternion's norm is 1
     236
     237        Note that this is equal to conjugate() if the Quaternion's norm is 1
    238238*/
    239239Quaternion Quaternion::inverse() const
    … …  
    253253void Quaternion::matrix (float m[4][4]) const
    254254{
    255   float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
    256  
     255  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
     256
    257257  // calculate coefficients
    258258  x2 = v.x + v.x;
    259   y2 = v.y + v.y; 
     259  y2 = v.y + v.y;
    260260  z2 = v.z + v.z;
    261261  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
    262262  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
    263263  wx = w * x2; wy = w * y2; wz = w * z2;
    264  
     264
    265265  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
    266266  m[2][0] = xz + wy; m[3][0] = 0.0;
    267        
     267
    268268  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
    269269  m[2][1] = yz - wx; m[3][1] = 0.0;
    270  
     270
    271271  m[0][2] = xz - wy; m[1][2] = yz + wx;
    272272  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
    273  
     273
    274274  m[0][3] = 0; m[1][3] = 0;
    275275  m[2][3] = 0; m[3][3] = 1;
    … …  
    292292  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
    293293
    294   if( cosom < 0.0 ) 
    295     { 
    296       cosom = -cosom; 
     294  if( cosom < 0.0 )
     295    {
     296      cosom = -cosom;
    297297      tol[0] = -to.v.x;
    298298      tol[1] = -to.v.y;
    … …  
    307307      tol[3] = to.w;
    308308    }
    309  
     309
    310310  //if( (1.0 - cosom) > DELTA )
    311311  //{
    … …  
    332332  */
    333333  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
    334                            scale0 * from.v.y + scale1 * tol[1],
    335                            scale0 * from.v.z + scale1 * tol[2]),
    336                     scale0 * from.w + scale1 * tol[3]);
     334                           scale0 * from.v.y + scale1 * tol[1],
     335                           scale0 * from.v.z + scale1 * tol[2]),
     336                    scale0 * from.w + scale1 * tol[3]);
    337337}
    338338
    … …  
    344344Quaternion::Quaternion (float m[4][4])
    345345{
    346        
     346
    347347  float  tr, s, q[4];
    348348  int    i, j, k;
    … …  
    352352  tr = m[0][0] + m[1][1] + m[2][2];
    353353
    354         // check the diagonal
    355   if (tr > 0.0) 
     354        // check the diagonal
     355  if (tr > 0.0)
    356356  {
    357357    s = sqrt (tr + 1.0);
    … …  
    361361    v.y = (m[2][0] - m[0][2]) * s;
    362362    v.z = (m[0][1] - m[1][0]) * s;
    363         }
    364         else
    365         {
    366                 // diagonal is negative
    367         i = 0;
    368         if (m[1][1] > m[0][0]) i = 1;
     363        }
     364        else
     365        {
     366                // diagonal is negative
     367        i = 0;
     368        if (m[1][1] > m[0][0]) i = 1;
    369369    if (m[2][2] > m[i][i]) i = 2;
    370370    j = nxt[i];
    … …  
    372372
    373373    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
    374      
     374
    375375    q[i] = s * 0.5;
    376            
     376
    377377    if (s != 0.0) s = 0.5 / s;
    378        
    379           q[3] = (m[j][k] - m[k][j]) * s;
     378
     379          q[3] = (m[j][k] - m[k][j]) * s;
    380380    q[j] = (m[i][j] + m[j][i]) * s;
    381381    q[k] = (m[i][k] + m[k][i]) * s;
    382382
    383         v.x = q[0];
    384         v.y = q[1];
    385         v.z = q[2];
    386         w = q[3];
     383        v.x = q[0];
     384        v.y = q[1];
     385        v.z = q[2];
     386        w = q[3];
    387387  }
    388388}
    … …  
    486486   \brief fills the specified buffer with a 4x4 glmatrix
    487487   \param buffer: Pointer to an array of 16 floats
    488    
     488
    489489   Use this to get the rotation in a gl-compatible format
    490490*/
    491491void Rotation::glmatrix (float* buffer)
    492492{
    493         buffer[0] = m[0];
    494         buffer[1] = m[3];
    495         buffer[2] = m[6];
    496         buffer[3] = m[0];
    497         buffer[4] = m[1];
    498         buffer[5] = m[4];
    499         buffer[6] = m[7];
    500         buffer[7] = m[0];
    501         buffer[8] = m[2];
    502         buffer[9] = m[5];
    503         buffer[10] = m[8];
    504         buffer[11] = m[0];
    505         buffer[12] = m[0];
    506         buffer[13] = m[0];
    507         buffer[14] = m[0];
    508         buffer[15] = m[1];
     493        buffer[0] = m[0];
     494        buffer[1] = m[3];
     495        buffer[2] = m[6];
     496        buffer[3] = m[0];
     497        buffer[4] = m[1];
     498        buffer[5] = m[4];
     499        buffer[6] = m[7];
     500        buffer[7] = m[0];
     501        buffer[8] = m[2];
     502        buffer[9] = m[5];
     503        buffer[10] = m[8];
     504        buffer[11] = m[0];
     505        buffer[12] = m[0];
     506        buffer[13] = m[0];
     507        buffer[14] = m[0];
     508        buffer[15] = m[1];
    509509}
    510510
    … …  
    513513   \param r: another Rotation
    514514   \return the matrix product of the Rotations
    515    
     515
    516516   Use this to rotate one rotation by another
    517517*/
    518518Rotation Rotation::operator* (const Rotation& r)
    519519{
    520         Rotation p;
    521        
    522         p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
    523         p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
    524         p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
    525        
    526         p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
    527         p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
    528         p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
    529 
    530         p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
    531         p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
    532         p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
    533        
    534         return p;
     520        Rotation p;
     521
     522        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
     523        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
     524        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
     525
     526        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
     527        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
     528        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
     529
     530        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
     531        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
     532        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
     533
     534        return p;
    535535}
    536536
    … …  
    545545{
    546546  Vector t;
    547  
     547
    548548  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
    549549  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
    … …  
    581581
    582582/**
     583   \brief calculate the distance between a line and a point
     584   \param v: the point
     585   \return the distance between the Line and the point
     586 */
     587float Line::distancePoint (const sVec3D& v) const
     588{
     589  Vector s(v[0], v[1], v[2]);
     590  Vector d = s - r;
     591  Vector u = a * d.dot( a);
     592  return (d - u).len();
     593}
     594
     595/**
    583596   \brief calculate the two points of minimal distance of two lines
    584597   \param l: the other line
    … …  
    597610/**
    598611  \brief calculate the length of a line
    599   \return the lenght of the line 
     612  \return the lenght of the line
    600613*/
    601614float Line::len() const

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

    r4562 r4578  
    1 /*! 
     1/*!
    22    \file vector.h
    33    \brief A basic 3D math framework
    4    
     4
    55    Contains classes to handle vectors, lines, rotations and planes
    6 */ 
     6*/
    77
    88#ifndef _VECTOR_H
    … …  
    1717//! 3D Vector
    1818/**
    19         Class to handle 3D Vectors
     19        Class to handle 3D Vectors
    2020*/
    2121class Vector {
    … …  
    6363  /** \brief normalizes the vector */
    6464  inline void normalize() {
    65                       float l = len(); 
    66                       if( unlikely(l == 0.0))
    67                         {
    68                           // Prevent divide by zero
    69                           return;
    70                         }
    71                       x = x / l;
    72                       y = y / l;
    73                       z = z / l;
     65                      float l = len();
     66                      if( unlikely(l == 0.0))
     67                        {
     68                          // Prevent divide by zero
     69                          return;
     70                        }
     71                      x = x / l;
     72                      y = y / l;
     73                      z = z / l;
    7474                    }
    7575  Vector getNormalized() const;
    … …  
    123123  inline const Quaternion& operator*= (const float& f) {*this = *this * f; return *this;}
    124124  Quaternion operator* (const Quaternion& q) const;
    125   /** \param q: the Quaternion to multiply by \returns the quaternion multiplied by q (this *= q) */ 
     125  /** \param q: the Quaternion to multiply by \returns the quaternion multiplied by q (this *= q) */
    126126  inline const Quaternion operator*= (const Quaternion& q) {*this = *this * q; return *this; };
    127127  /** \param q the Quaternion to add to this \returns the quaternion added with q (this + q) */
    … …  
    141141  float norm () const;
    142142  void matrix (float m[4][4]) const;
    143  
     143
    144144  void debug();
    145145
    … …  
    161161class Rotation {
    162162  public:
    163  
     163
    164164  float m[9]; //!< 3x3 Rotation Matrix
    165  
     165
    166166  Rotation ( const Vector& v);
    167167  Rotation ( const Vector& axis, float angle);
    … …  
    169169  Rotation ();
    170170  ~Rotation () {}
    171  
     171
    172172  Rotation operator* (const Rotation& r);
    173  
     173
    174174  void glmatrix (float* buffer);
    175175};
    … …  
    187187{
    188188  public:
    189  
     189
    190190  Vector r;   //!< Offset
    191191  Vector a;   //!< Direction
    192  
     192
    193193  Line ( Vector r, Vector a) : r(r), a(a) {}  //!< assignment constructor
    194194  Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
    195195  ~Line () {}
    196  
     196
    197197  float distance (const Line& l) const;
    198198  float distancePoint (const Vector& v) const;
     199  float distancePoint (const sVec3D& v) const;
    199200  Vector* footpoints (const Line& l) const;
    200201  float len () const;
    201  
     202
    202203  void rotate(const Rotation& rot);
    203204};
    … …  
    206207/**
    207208  Class to handle planes in 3-dimensional space
    208  
     209
    209210  Critical for polygon-based collision detection
    210211*/
    … …  
    212213{
    213214  public:
    214  
     215
    215216  Vector n;   //!< Normal vector
    216217  float k;    //!< Offset constant
    217  
     218
    218219  Plane (Vector a, Vector b, Vector c);
    219220  Plane (Vector norm, Vector p);
    … …  
    221222  Plane () : n(Vector(1,1,1)), k(0) {}
    222223  ~Plane () {}
    223  
     224
    224225  Vector intersectLine (const Line& l) const;
    225226  float distancePoint (const Vector& p) const;
Note: See TracChangeset for help on using the changeset viewer.