Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/branches/resources/src/lib/math/quaternion.cc @ 7341

Last change on this file since 7341 was 7191, checked in by bensch, 19 years ago

orxonox/trunk: some better and faster slerp functions

File size: 6.7 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 *  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 *  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 *  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 * Slerps this QUaternion performs a smooth move.
137 * @param toQuat to this Quaternion
138 * @param t \% inth the the direction[0..1]
139 */
140void Quaternion::slerpTo(const Quaternion& toQuat, float t)
141{
142  float tol[4];
143  double omega, cosom, sinom, scale0, scale1;
144  //  float DELTA = 0.2;
145
146  cosom = this->v.x * toQuat.v.x + this->v.y * toQuat.v.y + this->v.z * toQuat.v.z + this->w * toQuat.w;
147
148  if( cosom < 0.0 )
149  {
150    cosom = -cosom;
151    tol[0] = -toQuat.v.x;
152    tol[1] = -toQuat.v.y;
153    tol[2] = -toQuat.v.z;
154    tol[3] = -toQuat.w;
155  }
156  else
157  {
158    tol[0] = toQuat.v.x;
159    tol[1] = toQuat.v.y;
160    tol[2] = toQuat.v.z;
161    tol[3] = toQuat.w;
162  }
163
164  omega = acos(cosom);
165  sinom = sin(omega);
166  scale0 = sin((1.0 - t) * omega) / sinom;
167  scale1 = sin(t * omega) / sinom;
168  this->v = Vector(scale0 * this->v.x + scale1 * tol[0],
169                    scale0 * this->v.y + scale1 * tol[1],
170                    scale0 * this->v.z + scale1 * tol[2]);
171  this->w = scale0 * this->w + scale1 * tol[3];
172}
173
174
175/**
176 *  performs a smooth move.
177 * @param from  where
178 * @param to where
179 * @param t the time this transformation should take value [0..1]
180 * @returns the Result of the smooth move
181 */
182Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
183{
184  float tol[4];
185  double omega, cosom, sinom, scale0, scale1;
186  //  float DELTA = 0.2;
187
188  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
189
190  if( cosom < 0.0 )
191    {
192      cosom = -cosom;
193      tol[0] = -to.v.x;
194      tol[1] = -to.v.y;
195      tol[2] = -to.v.z;
196      tol[3] = -to.w;
197    }
198  else
199    {
200      tol[0] = to.v.x;
201      tol[1] = to.v.y;
202      tol[2] = to.v.z;
203      tol[3] = to.w;
204    }
205
206  omega = acos(cosom);
207  sinom = sin(omega);
208  scale0 = sin((1.0 - t) * omega) / sinom;
209  scale1 = sin(t * omega) / sinom;
210  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
211                    scale0 * from.v.y + scale1 * tol[1],
212                    scale0 * from.v.z + scale1 * tol[2]),
213                    scale0 * from.w + scale1 * tol[3]);
214}
215
216
217/**
218 *  convert a rotational 4x4 glMatrix into a Quaternion
219 * @param m: a 4x4 matrix in glMatrix order
220 */
221Quaternion::Quaternion (float m[4][4])
222{
223
224  float  tr, s, q[4];
225  int    i, j, k;
226
227  int nxt[3] = {1, 2, 0};
228
229  tr = m[0][0] + m[1][1] + m[2][2];
230
231        // check the diagonal
232  if (tr > 0.0)
233  {
234    s = sqrt (tr + 1.0);
235    w = s / 2.0;
236    s = 0.5 / s;
237    v.x = (m[1][2] - m[2][1]) * s;
238    v.y = (m[2][0] - m[0][2]) * s;
239    v.z = (m[0][1] - m[1][0]) * s;
240        }
241        else
242        {
243                // diagonal is negative
244        i = 0;
245        if (m[1][1] > m[0][0]) i = 1;
246    if (m[2][2] > m[i][i]) i = 2;
247    j = nxt[i];
248    k = nxt[j];
249
250    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
251
252    q[i] = s * 0.5;
253
254    if (s != 0.0) s = 0.5 / s;
255
256          q[3] = (m[j][k] - m[k][j]) * s;
257    q[j] = (m[i][j] + m[j][i]) * s;
258    q[k] = (m[i][k] + m[k][i]) * s;
259
260        v.x = q[0];
261        v.y = q[1];
262        v.z = q[2];
263        w = q[3];
264  }
265}
266
267/**
268 *  outputs some nice formated debug information about this quaternion
269*/
270void Quaternion::debug() const
271{
272  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
273}
274
275void Quaternion::debug2() const
276{
277  Vector axis = this->getSpacialAxis();
278  PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
279}
Note: See TracBrowser for help on using the repository browser.