Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

orxonox/trunk: fixed a nasty PNode bug …
@patrick: you were right about it being still debugged :)

File size: 5.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 *  performs a smooth move.
136 * @param from  where
137 * @param to where
138 * @param t the time this transformation should take value [0..1]
139 * @returns the Result of the smooth move
140 */
141Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
142{
143  float tol[4];
144  double omega, cosom, sinom, scale0, scale1;
145  //  float DELTA = 0.2;
146
147  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
148
149  if( cosom < 0.0 )
150    {
151      cosom = -cosom;
152      tol[0] = -to.v.x;
153      tol[1] = -to.v.y;
154      tol[2] = -to.v.z;
155      tol[3] = -to.w;
156    }
157  else
158    {
159      tol[0] = to.v.x;
160      tol[1] = to.v.y;
161      tol[2] = to.v.z;
162      tol[3] = to.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  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
170                    scale0 * from.v.y + scale1 * tol[1],
171                    scale0 * from.v.z + scale1 * tol[2]),
172                    scale0 * from.w + scale1 * tol[3]);
173}
174
175
176/**
177 *  convert a rotational 4x4 glMatrix into a Quaternion
178 * @param m: a 4x4 matrix in glMatrix order
179 */
180Quaternion::Quaternion (float m[4][4])
181{
182
183  float  tr, s, q[4];
184  int    i, j, k;
185
186  int nxt[3] = {1, 2, 0};
187
188  tr = m[0][0] + m[1][1] + m[2][2];
189
190        // check the diagonal
191  if (tr > 0.0)
192  {
193    s = sqrt (tr + 1.0);
194    w = s / 2.0;
195    s = 0.5 / s;
196    v.x = (m[1][2] - m[2][1]) * s;
197    v.y = (m[2][0] - m[0][2]) * s;
198    v.z = (m[0][1] - m[1][0]) * s;
199        }
200        else
201        {
202                // diagonal is negative
203        i = 0;
204        if (m[1][1] > m[0][0]) i = 1;
205    if (m[2][2] > m[i][i]) i = 2;
206    j = nxt[i];
207    k = nxt[j];
208
209    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
210
211    q[i] = s * 0.5;
212
213    if (s != 0.0) s = 0.5 / s;
214
215          q[3] = (m[j][k] - m[k][j]) * s;
216    q[j] = (m[i][j] + m[j][i]) * s;
217    q[k] = (m[i][k] + m[k][i]) * s;
218
219        v.x = q[0];
220        v.y = q[1];
221        v.z = q[2];
222        w = q[3];
223  }
224}
225
226/**
227 *  outputs some nice formated debug information about this quaternion
228*/
229void Quaternion::debug() const
230{
231  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
232}
233
234void Quaternion::debug2() const
235{
236  Vector axis = this->getSpacialAxis();
237  PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
238}
Note: See TracBrowser for help on using the repository browser.