Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 9122 was 9110, checked in by bensch, 18 years ago

orxonox/trunk: merged the Presentation back

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