Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/convention/src/lib/math/quaternion.h @ 3756

Last change on this file since 3756 was 3607, checked in by patrick, 20 years ago

orxonox/trunk: stdincl was still included everywhere. removed it out of the stdincl.h file to enable the possibility of compile-speedup. added some testclasses for vector/quaternion.

File size: 9.1 KB
Line 
1/*!
2    \file vector.h
3    \brief A basic 3D math framework
4   
5    Contains classes to handle vectors, lines, rotations and planes
6*/ 
7
8#ifndef _QUATERNION_H
9#define _QUATERNION_H
10
11#include <math.h>
12//! PI the circle-constant
13
14//! Quaternion
15/**
16        Class to handle 3-dimensional rotation efficiently
17*/
18class Quaternion
19{
20 public:
21  Vector v;     //!< Imaginary Vector
22  float w;        //!< Real part of the number
23 
24  Quaternion ();
25  Quaternion (float m[4][4]);
26  Quaternion (float angle, const Vector& axis);
27  Quaternion (const Vector& dir, const Vector& up);
28  Quaternion (float roll, float pitch, float yaw);
29 
30  Quaternion operator/ (const float& f) const;
31  Quaternion operator* (const float& f) const;
32  Quaternion operator* (const Quaternion& q) const;
33  Quaternion operator+ (const Quaternion& q) const;
34  Quaternion operator- (const Quaternion& q) const;
35  Quaternion conjugate () const;
36  Quaternion inverse () const;
37  Vector apply (Vector& f) const;
38  float norm () const;
39  void matrix (float m[4][4]) const;
40  void quatSlerp(const Quaternion* from, const Quaternion* to, const float t, Quaternion* res);
41 
42  void debug();
43 private:
44  float DELTA;      //!< resolution of calculation
45
46};
47
48
49/**
50        \brief creates a multiplicational identity Quaternion
51*/
52Quaternion::Quaternion ()
53{
54        w = 1;
55        v = Vector(0,0,0);
56}
57
58/**
59        \brief turns a rotation along an axis into a Quaternion
60        \param angle: the amount of radians to rotate
61        \param axis: the axis to rotate around
62*/
63Quaternion::Quaternion (float angle, const Vector& axis)
64{
65        w = cos(angle/2);
66        v = axis * sin(angle/2);
67}
68
69/**
70   \brief calculates a lookAt rotation
71   \param dir: the direction you want to look
72   \param up: specify what direction up should be
73   
74   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
75   the same way as dir. If you want to use this with cameras, you'll have to reverse the
76   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
77   can use this for meshes as well (then you do not have to reverse the vector), but keep
78   in mind that if you do that, the model's front has to point in +z direction, and left
79   and right should be -x or +x respectively or the mesh wont rotate correctly.
80*/
81Quaternion::Quaternion (const Vector& dir, const Vector& up)
82{
83  Vector z = dir;
84  z.normalize(); 
85  Vector x = up.cross(z);
86  x.normalize(); 
87  Vector y = z.cross(x);
88 
89  float m[4][4];
90  m[0][0] = x.x;
91  m[0][1] = x.y;
92  m[0][2] = x.z;
93  m[0][3] = 0;
94  m[1][0] = y.x;
95  m[1][1] = y.y;
96  m[1][2] = y.z;
97  m[1][3] = 0;
98  m[2][0] = z.x;
99  m[2][1] = z.y;
100  m[2][2] = z.z;
101  m[2][3] = 0;
102  m[3][0] = 0;
103  m[3][1] = 0;
104  m[3][2] = 0;
105  m[3][3] = 1;
106 
107  *this = Quaternion (m);
108}
109
110/**
111        \brief calculates a rotation from euler angles
112        \param roll: the roll in radians
113        \param pitch: the pitch in radians
114        \param yaw: the yaw in radians
115       
116        I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know
117        in what order they are applied, I just copy-pasted the code.
118*/
119Quaternion::Quaternion (float roll, float pitch, float yaw)
120{
121        float cr, cp, cy, sr, sp, sy, cpcy, spsy;
122       
123        // calculate trig identities
124        cr = cos(roll/2);
125        cp = cos(pitch/2);
126        cy = cos(yaw/2);
127       
128        sr = sin(roll/2);
129        sp = sin(pitch/2);
130        sy = sin(yaw/2);
131       
132        cpcy = cp * cy;
133        spsy = sp * sy;
134       
135        w = cr * cpcy + sr * spsy;
136        v.x = sr * cpcy - cr * spsy;
137        v.y = cr * sp * cy + sr * cp * sy;
138        v.z = cr * cp * sy - sr * sp * cy;
139}
140
141/**
142        \brief rotates one Quaternion by another
143        \param q: another Quaternion to rotate this by
144        \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))
145*/
146Quaternion Quaternion::operator*(const Quaternion& q) const
147{
148        float A, B, C, D, E, F, G, H;
149        Quaternion r;
150       
151        A = (w   + v.x)*(q.w   + q.v.x);
152        B = (v.z - v.y)*(q.v.y - q.v.z);
153        C = (w   - v.x)*(q.v.y + q.v.z); 
154        D = (v.y + v.z)*(q.w   - q.v.x);
155        E = (v.x + v.z)*(q.v.x + q.v.y);
156        F = (v.x - v.z)*(q.v.x - q.v.y);
157        G = (w   + v.y)*(q.w   - q.v.z);
158        H = (w   - v.y)*(q.w   + q.v.z);
159       
160        r.w = B +  (-E - F + G + H)/2;
161        r.v.x = A - (E + F + G + H)/2; 
162        r.v.y = C + (E - F + G - H)/2; 
163        r.v.z = D + (E - F - G + H)/2;
164       
165        return r;
166}
167
168/**
169        \brief add two Quaternions
170        \param q: another Quaternion
171        \return the sum of both Quaternions
172*/
173Quaternion Quaternion::operator+(const Quaternion& q) const
174{
175        Quaternion r(*this);
176        r.w = r.w + q.w;
177        r.v = r.v + q.v;
178        return r;
179}
180
181/**
182        \brief subtract two Quaternions
183        \param q: another Quaternion
184        \return the difference of both Quaternions
185*/
186Quaternion Quaternion::operator- (const Quaternion& q) const
187{
188        Quaternion r(*this);
189        r.w = r.w - q.w;
190        r.v = r.v - q.v;
191        return r;
192}
193
194/**
195        \brief rotate a Vector by a Quaternion
196        \param v: the Vector
197        \return a new Vector representing v rotated by the Quaternion
198*/
199Vector Quaternion::apply (Vector& v) const
200{
201        Quaternion q;
202        q.v = v;
203        q.w = 0;
204        q = *this * q * conjugate();
205        return q.v;
206}
207
208/**
209        \brief multiply a Quaternion with a real value
210        \param f: a real value
211        \return a new Quaternion containing the product
212*/
213Quaternion Quaternion::operator*(const float& f) const
214{
215        Quaternion r(*this);
216        r.w = r.w*f;
217        r.v = r.v*f;
218        return r;
219}
220
221/**
222        \brief divide a Quaternion by a real value
223        \param f: a real value
224        \return a new Quaternion containing the quotient
225*/
226Quaternion Quaternion::operator/(const float& f) const
227{
228        if( f == 0) return Quaternion();
229        Quaternion r(*this);
230        r.w = r.w/f;
231        r.v = r.v/f;
232        return r;
233}
234
235/**
236        \brief calculate the conjugate value of the Quaternion
237        \return the conjugate Quaternion
238*/
239Quaternion Quaternion::conjugate() const
240{
241        Quaternion r(*this);
242        r.v = Vector() - r.v;
243        return r;
244}
245
246/**
247        \brief calculate the norm of the Quaternion
248        \return the norm of The Quaternion
249*/
250float Quaternion::norm() const
251{
252        return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
253}
254
255/**
256        \brief calculate the inverse value of the Quaternion
257        \return the inverse Quaternion
258       
259        Note that this is equal to conjugate() if the Quaternion's norm is 1
260*/
261Quaternion Quaternion::inverse() const
262{
263        float n = norm();
264        if (n != 0)
265        {
266                return conjugate() / norm();
267        }
268        else return Quaternion();
269}
270
271/**
272        \brief convert the Quaternion to a 4x4 rotational glMatrix
273        \param m: a buffer to store the Matrix in
274*/
275void Quaternion::matrix (float m[4][4]) const
276{
277  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
278 
279  // calculate coefficients
280  x2 = v.x + v.x;
281  y2 = v.y + v.y; 
282  z2 = v.z + v.z;
283  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
284  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
285  wx = w * x2; wy = w * y2; wz = w * z2;
286 
287  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
288  m[2][0] = xz + wy; m[3][0] = 0.0;
289       
290  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
291  m[2][1] = yz - wx; m[3][1] = 0.0;
292 
293  m[0][2] = xz - wy; m[1][2] = yz + wx;
294  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
295 
296  m[0][3] = 0; m[1][3] = 0;
297  m[2][3] = 0; m[3][3] = 1;
298}
299
300/**
301   \brief performs a smooth move.
302   \param from from where
303   \param to to where
304   \param t the time this transformation should take
305   \param res The approximation-density
306*/
307void Quaternion::quatSlerp(const Quaternion* from, const Quaternion* to, float t, Quaternion* res)
308{
309  float tol[4];
310  double omega, cosom, sinom, scale0, scale1;
311  DELTA = 0.2;
312
313  cosom = from->v.x * to->v.x + from->v.y * to->v.y + from->v.z * to->v.z + from->w * to->w;
314
315  if( cosom < 0.0 ) 
316    { 
317      cosom = -cosom; 
318      tol[0] = -to->v.x;
319      tol[1] = -to->v.y;
320      tol[2] = -to->v.z;
321      tol[3] = -to->w;
322    }
323  else
324    {
325      tol[0] = to->v.x;
326      tol[1] = to->v.y;
327      tol[2] = to->v.z;
328      tol[3] = to->w;
329    }
330 
331  //if( (1.0 - cosom) > DELTA )
332  //{
333      omega = acos(cosom);
334      sinom = sin(omega);
335      scale0 = sin((1.0 - t) * omega) / sinom;
336      scale1 = sin(t * omega) / sinom;
337      //}
338      /*
339  else
340    {
341      scale0 = 1.0 - t;
342      scale1 = t;
343    }
344      */
345  res->v.x = scale0 * from->v.x + scale1 * tol[0];
346  res->v.y = scale0 * from->v.y + scale1 * tol[1];
347  res->v.z = scale0 * from->v.z + scale1 * tol[2];
348  res->w = scale0 * from->w + scale1 * tol[3];
349}
350
351
352/**
353   \brief convert a rotational 4x4 glMatrix into a Quaternion
354   \param m: a 4x4 matrix in glMatrix order
355*/
356Quaternion::Quaternion (float m[4][4])
357{
358       
359  float  tr, s, q[4];
360  int    i, j, k;
361
362  int nxt[3] = {1, 2, 0};
363
364  tr = m[0][0] + m[1][1] + m[2][2];
365
366        // check the diagonal
367  if (tr > 0.0) 
368  {
369    s = sqrt (tr + 1.0);
370    w = s / 2.0;
371    s = 0.5 / s;
372    v.x = (m[1][2] - m[2][1]) * s;
373    v.y = (m[2][0] - m[0][2]) * s;
374    v.z = (m[0][1] - m[1][0]) * s;
375        } 
376        else 
377        {
378                // diagonal is negative
379        i = 0;
380        if (m[1][1] > m[0][0]) i = 1;
381    if (m[2][2] > m[i][i]) i = 2;
382    j = nxt[i];
383    k = nxt[j];
384
385    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
386     
387    q[i] = s * 0.5;
388           
389    if (s != 0.0) s = 0.5 / s;
390       
391          q[3] = (m[j][k] - m[k][j]) * s;
392    q[j] = (m[i][j] + m[j][i]) * s;
393    q[k] = (m[i][k] + m[k][i]) * s;
394
395        v.x = q[0];
396        v.y = q[1];
397        v.z = q[2];
398        w = q[3];
399  }
400}
401
402/**
403   \brief outputs some nice formated debug information about this quaternion
404*/
405void Quaternion::debug(void)
406{
407  PRINT(0)("Quaternion Debug Information\n");
408  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
409}
410
411
412
413#endif /* _QUATERNION_H */
Note: See TracBrowser for help on using the repository browser.