Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/trunk/src/lib/math/quaternion.cc @ 8770

Last change on this file since 8770 was 8731, checked in by bensch, 18 years ago

new player

File size: 8.9 KB
Line 
1/*
2   orxonox - the future of 3D-vertical-scrollers
3
4   Copyright (C) 2004 orx
5
6   This program is free software; you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation; either version 2, or (at your option)
9   any later version.
10
11   ### File Specific:
12   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)
19*/
20
21#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
22
23#include "quaternion.h"
24#ifdef DEBUG
25  #include "debug.h"
26#else
27  #include <stdio.h>
28  #define PRINT(x) printf
29#endif
30
31using namespace std;
32
33/////////////////
34/* QUATERNIONS */
35/////////////////
36/**
37 * @brief calculates a lookAt rotation
38 * @param dir: the direction you want to look
39 * @param up: specify what direction up should be
40 *
41 * Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
42 * the same way as dir. If you want to use this with cameras, you'll have to reverse the
43 * dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
44 * can use this for meshes as well (then you do not have to reverse the vector), but keep
45 * in mind that if you do that, the model's front has to point in +z direction, and left
46 * and right should be -x or +x respectively or the mesh wont rotate correctly.
47 *
48 * @TODO !!! OPTIMIZE THIS !!!
49 */
50Quaternion::Quaternion (const Vector& dir, const Vector& up)
51{
52  Vector z = dir.getNormalized();
53  Vector x = up.cross(z).getNormalized();
54  Vector y = z.cross(x);
55
56  float m[4][4];
57  m[0][0] = x.x;
58  m[0][1] = x.y;
59  m[0][2] = x.z;
60  m[0][3] = 0;
61  m[1][0] = y.x;
62  m[1][1] = y.y;
63  m[1][2] = y.z;
64  m[1][3] = 0;
65  m[2][0] = z.x;
66  m[2][1] = z.y;
67  m[2][2] = z.z;
68  m[2][3] = 0;
69  m[3][0] = 0;
70  m[3][1] = 0;
71  m[3][2] = 0;
72  m[3][3] = 1;
73
74  *this = Quaternion (m);
75}
76
77/**
78 * @brief calculates a rotation from euler angles
79 * @param roll: the roll in radians
80 * @param pitch: the pitch in radians
81 * @param yaw: the yaw in radians
82 */
83Quaternion::Quaternion (float roll, float pitch, float yaw)
84{
85  float cr, cp, cy, sr, sp, sy, cpcy, spsy;
86
87  // calculate trig identities
88  cr = cos(roll/2);
89  cp = cos(pitch/2);
90  cy = cos(yaw/2);
91
92  sr = sin(roll/2);
93  sp = sin(pitch/2);
94  sy = sin(yaw/2);
95
96  cpcy = cp * cy;
97  spsy = sp * sy;
98
99  w = cr * cpcy + sr * spsy;
100  v.x = sr * cpcy - cr * spsy;
101  v.y = cr * sp * cy + sr * cp * sy;
102  v.z = cr * cp * sy - sr * sp * cy;
103}
104
105/**
106 * @brief convert the Quaternion to a 4x4 rotational glMatrix
107 * @param m: a buffer to store the Matrix in
108 */
109void Quaternion::matrix (float m[4][4]) const
110{
111  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
112
113  // calculate coefficients
114  x2 = v.x + v.x;
115  y2 = v.y + v.y;
116  z2 = v.z + v.z;
117  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
118  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
119  wx = w * x2; wy = w * y2; wz = w * z2;
120
121  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
122  m[2][0] = xz + wy; m[3][0] = 0.0;
123
124  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
125  m[2][1] = yz - wx; m[3][1] = 0.0;
126
127  m[0][2] = xz - wy; m[1][2] = yz + wx;
128  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
129
130  m[0][3] = 0; m[1][3] = 0;
131  m[2][3] = 0; m[3][3] = 1;
132}
133
134
135
136/**
137 * @brief Slerps this QUaternion performs a smooth move.
138 * @param toQuat to this Quaternion
139 * @param t \% inth the the direction[0..1]
140 */
141void Quaternion::slerpTo(const Quaternion& toQuat, float t)
142{
143  float tol[4];
144  double omega, cosom, sinom, scale0, scale1;
145  //  float DELTA = 0.2;
146
147  cosom = this->v.x * toQuat.v.x + this->v.y * toQuat.v.y + this->v.z * toQuat.v.z + this->w * toQuat.w;
148
149  if( cosom < 0.0 )
150  {
151    cosom = -cosom;
152    tol[0] = -toQuat.v.x;
153    tol[1] = -toQuat.v.y;
154    tol[2] = -toQuat.v.z;
155    tol[3] = -toQuat.w;
156  }
157  else
158  {
159    tol[0] = toQuat.v.x;
160    tol[1] = toQuat.v.y;
161    tol[2] = toQuat.v.z;
162    tol[3] = toQuat.w;
163  }
164
165  omega = acos(cosom);
166  sinom = sin(omega);
167  scale0 = sin((1.0 - t) * omega) / sinom;
168  scale1 = sin(t * omega) / sinom;
169  this->v = Vector(scale0 * this->v.x + scale1 * tol[0],
170                   scale0 * this->v.y + scale1 * tol[1],
171                   scale0 * this->v.z + scale1 * tol[2]);
172  this->w = scale0 * this->w + scale1 * tol[3];
173}
174
175
176/**
177 * @brief performs a smooth move.
178 * @param from  where
179 * @param to where
180 * @param t the time this transformation should take value [0..1]
181 * @returns the Result of the smooth move
182 */
183Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
184{
185  float tol[4];
186  double omega, cosom, sinom, scale0, scale1;
187  //  float DELTA = 0.2;
188
189  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
190
191  if( cosom < 0.0 )
192  {
193    cosom = -cosom;
194    tol[0] = -to.v.x;
195    tol[1] = -to.v.y;
196    tol[2] = -to.v.z;
197    tol[3] = -to.w;
198  }
199  else
200  {
201    tol[0] = to.v.x;
202    tol[1] = to.v.y;
203    tol[2] = to.v.z;
204    tol[3] = to.w;
205  }
206
207  omega = acos(cosom);
208  sinom = sin(omega);
209  scale0 = sin((1.0 - t) * omega) / sinom;
210  scale1 = sin(t * omega) / sinom;
211  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
212                           scale0 * from.v.y + scale1 * tol[1],
213                           scale0 * from.v.z + scale1 * tol[2]),
214                    scale0 * from.w + scale1 * tol[3]);
215}
216
217/**
218 * @returns the Heading
219 */
220float Quaternion::getHeading() const
221{
222  float pole = this->v.x*this->v.y + this->v.z*this->w;
223  if (fabsf(pole) != 0.5)
224    return atan2(2.0* (v.y*w - v.x*v.z), 1 - 2.0*(v.y*v.y - v.z*v.z));
225  else if (pole == .5) // North Pole
226    return 2.0 * atan2(v.x, w);
227  else // South Pole
228    return -2.0 * atan2(v.x, w);
229}
230
231/**
232 * @returns the Heading-Quaternion
233 */
234Quaternion Quaternion::getHeadingQuat() const
235{
236  return Quaternion(this->getHeading(), Vector(0,1,0));
237}
238
239/**
240 * @returns the Attitude
241 */
242float Quaternion::getAttitude() const
243{
244  return asin(2.0 * (v.x*v.y + v.z*w));
245}
246
247/**
248 * @returns the Attitude-Quaternion
249 */
250Quaternion Quaternion::getAttitudeQuat() const
251{
252  return Quaternion(this->getAttitude(), Vector(0,0,1));
253}
254
255
256/**
257 * @returns the Bank
258 */
259float Quaternion::getBank() const
260{
261  if (fabsf(this->v.x*this->v.y + this->v.z*this->w) != 0.5)
262    return atan2(2.0*(v.x*w-v.y*v.z) , 1 - 2.0*(v.x*v.x - v.z*v.z));
263  else
264    return 0.0f;
265}
266
267/**
268 * @returns the Bank-Quaternion
269 */
270Quaternion Quaternion::getBankQuat() const
271{
272  return Quaternion(this->getBank(), Vector(1,0,0));
273}
274
275
276
277/**
278 * @brief convert a rotational 4x4 glMatrix into a Quaternion
279 * @param m: a 4x4 matrix in glMatrix order
280 */
281Quaternion::Quaternion (float m[4][4])
282{
283
284  float  tr, s, q[4];
285  int    i, j, k;
286
287  int nxt[3] = {1, 2, 0};
288
289  tr = m[0][0] + m[1][1] + m[2][2];
290
291  // check the diagonal
292  if (tr > 0.0)
293  {
294    s = sqrt (tr + 1.0);
295    w = s / 2.0;
296    s = 0.5 / s;
297    v.x = (m[1][2] - m[2][1]) * s;
298    v.y = (m[2][0] - m[0][2]) * s;
299    v.z = (m[0][1] - m[1][0]) * s;
300  }
301  else
302  {
303    // diagonal is negative
304    i = 0;
305    if (m[1][1] > m[0][0]) i = 1;
306    if (m[2][2] > m[i][i]) i = 2;
307    j = nxt[i];
308    k = nxt[j];
309
310    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
311
312    q[i] = s * 0.5;
313
314    if (s != 0.0) s = 0.5 / s;
315
316    q[3] = (m[j][k] - m[k][j]) * s;
317    q[j] = (m[i][j] + m[j][i]) * s;
318    q[k] = (m[i][k] + m[k][i]) * s;
319
320    v.x = q[0];
321    v.y = q[1];
322    v.z = q[2];
323    w = q[3];
324  }
325}
326
327
328/**
329 * Creates a quaternion from a 3x3 rotation matrix.
330 * @param mat The 3x3 source rotation matrix.
331 * @return The equivalent 4 float quaternion.
332 */
333Quaternion::Quaternion(float mat[3][3])
334{
335  int   NXT[] = {1, 2, 0};
336  float q[4];
337
338  // check the diagonal
339  float tr = mat[0][0] + mat[1][1] + mat[2][2];
340  if( tr > 0.0f) {
341    float s = (float)sqrtf(tr + 1.0f);
342    this->w = s * 0.5f;
343    s = 0.5f / s;
344    this->v.x = (mat[1][2] - mat[2][1]) * s;
345    this->v.y = (mat[2][0] - mat[0][2]) * s;
346    this->v.z = (mat[0][1] - mat[1][0]) * s;
347  }
348  else
349  {
350    // diagonal is negative
351    // get biggest diagonal element
352    int i = 0;
353    if (mat[1][1] > mat[0][0]) i = 1;
354    if (mat[2][2] > mat[i][i]) i = 2;
355    //setup index sequence
356    int j = NXT[i];
357    int k = NXT[j];
358
359    float s = (float)sqrtf((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0f);
360
361    q[i] = s * 0.5f;
362
363    if (s != 0.0f) s = 0.5f / s;
364
365    q[j] = (mat[i][j] + mat[j][i]) * s;
366    q[k] = (mat[i][k] + mat[k][i]) * s;
367    q[3] = (mat[j][k] - mat[k][j]) * s;
368
369    this->v.x = q[0];
370    this->v.y = q[1];
371    this->v.z = q[2];
372    this->w   = q[3];
373  }
374}
375
376/**
377 * @brief outputs some nice formated debug information about this quaternion
378*/
379void Quaternion::debug() const
380{
381  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
382}
383
384/**
385 * @brief another better Quaternion Debug Function.
386 */
387void Quaternion::debug2() const
388{
389  Vector axis = this->getSpacialAxis();
390  PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
391}
392
393
394
395
396
397
Note: See TracBrowser for help on using the repository browser.