diff options
Diffstat (limited to 'libraries/irrlicht-1.8/include/quaternion.h')
-rw-r--r-- | libraries/irrlicht-1.8/include/quaternion.h | 1392 |
1 files changed, 696 insertions, 696 deletions
diff --git a/libraries/irrlicht-1.8/include/quaternion.h b/libraries/irrlicht-1.8/include/quaternion.h index 2c6f0cf..7fbe50e 100644 --- a/libraries/irrlicht-1.8/include/quaternion.h +++ b/libraries/irrlicht-1.8/include/quaternion.h | |||
@@ -1,696 +1,696 @@ | |||
1 | // Copyright (C) 2002-2012 Nikolaus Gebhardt | 1 | // Copyright (C) 2002-2012 Nikolaus Gebhardt |
2 | // This file is part of the "Irrlicht Engine". | 2 | // This file is part of the "Irrlicht Engine". |
3 | // For conditions of distribution and use, see copyright notice in irrlicht.h | 3 | // For conditions of distribution and use, see copyright notice in irrlicht.h |
4 | 4 | ||
5 | #ifndef __IRR_QUATERNION_H_INCLUDED__ | 5 | #ifndef __IRR_QUATERNION_H_INCLUDED__ |
6 | #define __IRR_QUATERNION_H_INCLUDED__ | 6 | #define __IRR_QUATERNION_H_INCLUDED__ |
7 | 7 | ||
8 | #include "irrTypes.h" | 8 | #include "irrTypes.h" |
9 | #include "irrMath.h" | 9 | #include "irrMath.h" |
10 | #include "matrix4.h" | 10 | #include "matrix4.h" |
11 | #include "vector3d.h" | 11 | #include "vector3d.h" |
12 | 12 | ||
13 | // Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed. | 13 | // Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed. |
14 | // This define disables all involved functions completely to allow finding all places | 14 | // This define disables all involved functions completely to allow finding all places |
15 | // where the wrong conversions had been in use. | 15 | // where the wrong conversions had been in use. |
16 | #define IRR_TEST_BROKEN_QUATERNION_USE 0 | 16 | #define IRR_TEST_BROKEN_QUATERNION_USE 0 |
17 | 17 | ||
18 | namespace irr | 18 | namespace irr |
19 | { | 19 | { |
20 | namespace core | 20 | namespace core |
21 | { | 21 | { |
22 | 22 | ||
23 | //! Quaternion class for representing rotations. | 23 | //! Quaternion class for representing rotations. |
24 | /** It provides cheap combinations and avoids gimbal locks. | 24 | /** It provides cheap combinations and avoids gimbal locks. |
25 | Also useful for interpolations. */ | 25 | Also useful for interpolations. */ |
26 | class quaternion | 26 | class quaternion |
27 | { | 27 | { |
28 | public: | 28 | public: |
29 | 29 | ||
30 | //! Default Constructor | 30 | //! Default Constructor |
31 | quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {} | 31 | quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {} |
32 | 32 | ||
33 | //! Constructor | 33 | //! Constructor |
34 | quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { } | 34 | quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { } |
35 | 35 | ||
36 | //! Constructor which converts euler angles (radians) to a quaternion | 36 | //! Constructor which converts euler angles (radians) to a quaternion |
37 | quaternion(f32 x, f32 y, f32 z); | 37 | quaternion(f32 x, f32 y, f32 z); |
38 | 38 | ||
39 | //! Constructor which converts euler angles (radians) to a quaternion | 39 | //! Constructor which converts euler angles (radians) to a quaternion |
40 | quaternion(const vector3df& vec); | 40 | quaternion(const vector3df& vec); |
41 | 41 | ||
42 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 42 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
43 | //! Constructor which converts a matrix to a quaternion | 43 | //! Constructor which converts a matrix to a quaternion |
44 | quaternion(const matrix4& mat); | 44 | quaternion(const matrix4& mat); |
45 | #endif | 45 | #endif |
46 | 46 | ||
47 | //! Equalilty operator | 47 | //! Equalilty operator |
48 | bool operator==(const quaternion& other) const; | 48 | bool operator==(const quaternion& other) const; |
49 | 49 | ||
50 | //! inequality operator | 50 | //! inequality operator |
51 | bool operator!=(const quaternion& other) const; | 51 | bool operator!=(const quaternion& other) const; |
52 | 52 | ||
53 | //! Assignment operator | 53 | //! Assignment operator |
54 | inline quaternion& operator=(const quaternion& other); | 54 | inline quaternion& operator=(const quaternion& other); |
55 | 55 | ||
56 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 56 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
57 | //! Matrix assignment operator | 57 | //! Matrix assignment operator |
58 | inline quaternion& operator=(const matrix4& other); | 58 | inline quaternion& operator=(const matrix4& other); |
59 | #endif | 59 | #endif |
60 | 60 | ||
61 | //! Add operator | 61 | //! Add operator |
62 | quaternion operator+(const quaternion& other) const; | 62 | quaternion operator+(const quaternion& other) const; |
63 | 63 | ||
64 | //! Multiplication operator | 64 | //! Multiplication operator |
65 | quaternion operator*(const quaternion& other) const; | 65 | quaternion operator*(const quaternion& other) const; |
66 | 66 | ||
67 | //! Multiplication operator with scalar | 67 | //! Multiplication operator with scalar |
68 | quaternion operator*(f32 s) const; | 68 | quaternion operator*(f32 s) const; |
69 | 69 | ||
70 | //! Multiplication operator with scalar | 70 | //! Multiplication operator with scalar |
71 | quaternion& operator*=(f32 s); | 71 | quaternion& operator*=(f32 s); |
72 | 72 | ||
73 | //! Multiplication operator | 73 | //! Multiplication operator |
74 | vector3df operator*(const vector3df& v) const; | 74 | vector3df operator*(const vector3df& v) const; |
75 | 75 | ||
76 | //! Multiplication operator | 76 | //! Multiplication operator |
77 | quaternion& operator*=(const quaternion& other); | 77 | quaternion& operator*=(const quaternion& other); |
78 | 78 | ||
79 | //! Calculates the dot product | 79 | //! Calculates the dot product |
80 | inline f32 dotProduct(const quaternion& other) const; | 80 | inline f32 dotProduct(const quaternion& other) const; |
81 | 81 | ||
82 | //! Sets new quaternion | 82 | //! Sets new quaternion |
83 | inline quaternion& set(f32 x, f32 y, f32 z, f32 w); | 83 | inline quaternion& set(f32 x, f32 y, f32 z, f32 w); |
84 | 84 | ||
85 | //! Sets new quaternion based on euler angles (radians) | 85 | //! Sets new quaternion based on euler angles (radians) |
86 | inline quaternion& set(f32 x, f32 y, f32 z); | 86 | inline quaternion& set(f32 x, f32 y, f32 z); |
87 | 87 | ||
88 | //! Sets new quaternion based on euler angles (radians) | 88 | //! Sets new quaternion based on euler angles (radians) |
89 | inline quaternion& set(const core::vector3df& vec); | 89 | inline quaternion& set(const core::vector3df& vec); |
90 | 90 | ||
91 | //! Sets new quaternion from other quaternion | 91 | //! Sets new quaternion from other quaternion |
92 | inline quaternion& set(const core::quaternion& quat); | 92 | inline quaternion& set(const core::quaternion& quat); |
93 | 93 | ||
94 | //! returns if this quaternion equals the other one, taking floating point rounding errors into account | 94 | //! returns if this quaternion equals the other one, taking floating point rounding errors into account |
95 | inline bool equals(const quaternion& other, | 95 | inline bool equals(const quaternion& other, |
96 | const f32 tolerance = ROUNDING_ERROR_f32 ) const; | 96 | const f32 tolerance = ROUNDING_ERROR_f32 ) const; |
97 | 97 | ||
98 | //! Normalizes the quaternion | 98 | //! Normalizes the quaternion |
99 | inline quaternion& normalize(); | 99 | inline quaternion& normalize(); |
100 | 100 | ||
101 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 101 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
102 | //! Creates a matrix from this quaternion | 102 | //! Creates a matrix from this quaternion |
103 | matrix4 getMatrix() const; | 103 | matrix4 getMatrix() const; |
104 | #endif | 104 | #endif |
105 | 105 | ||
106 | //! Creates a matrix from this quaternion | 106 | //! Creates a matrix from this quaternion |
107 | void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const; | 107 | void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const; |
108 | 108 | ||
109 | /*! | 109 | /*! |
110 | Creates a matrix from this quaternion | 110 | Creates a matrix from this quaternion |
111 | Rotate about a center point | 111 | Rotate about a center point |
112 | shortcut for | 112 | shortcut for |
113 | core::quaternion q; | 113 | core::quaternion q; |
114 | q.rotationFromTo ( vin[i].Normal, forward ); | 114 | q.rotationFromTo ( vin[i].Normal, forward ); |
115 | q.getMatrixCenter ( lookat, center, newPos ); | 115 | q.getMatrixCenter ( lookat, center, newPos ); |
116 | 116 | ||
117 | core::matrix4 m2; | 117 | core::matrix4 m2; |
118 | m2.setInverseTranslation ( center ); | 118 | m2.setInverseTranslation ( center ); |
119 | lookat *= m2; | 119 | lookat *= m2; |
120 | 120 | ||
121 | core::matrix4 m3; | 121 | core::matrix4 m3; |
122 | m2.setTranslation ( newPos ); | 122 | m2.setTranslation ( newPos ); |
123 | lookat *= m3; | 123 | lookat *= m3; |
124 | 124 | ||
125 | */ | 125 | */ |
126 | void getMatrixCenter( matrix4 &dest, const core::vector3df ¢er, const core::vector3df &translation ) const; | 126 | void getMatrixCenter( matrix4 &dest, const core::vector3df ¢er, const core::vector3df &translation ) const; |
127 | 127 | ||
128 | //! Creates a matrix from this quaternion | 128 | //! Creates a matrix from this quaternion |
129 | inline void getMatrix_transposed( matrix4 &dest ) const; | 129 | inline void getMatrix_transposed( matrix4 &dest ) const; |
130 | 130 | ||
131 | //! Inverts this quaternion | 131 | //! Inverts this quaternion |
132 | quaternion& makeInverse(); | 132 | quaternion& makeInverse(); |
133 | 133 | ||
134 | //! Set this quaternion to the linear interpolation between two quaternions | 134 | //! Set this quaternion to the linear interpolation between two quaternions |
135 | /** \param q1 First quaternion to be interpolated. | 135 | /** \param q1 First quaternion to be interpolated. |
136 | \param q2 Second quaternion to be interpolated. | 136 | \param q2 Second quaternion to be interpolated. |
137 | \param time Progress of interpolation. For time=0 the result is | 137 | \param time Progress of interpolation. For time=0 the result is |
138 | q1, for time=1 the result is q2. Otherwise interpolation | 138 | q1, for time=1 the result is q2. Otherwise interpolation |
139 | between q1 and q2. | 139 | between q1 and q2. |
140 | */ | 140 | */ |
141 | quaternion& lerp(quaternion q1, quaternion q2, f32 time); | 141 | quaternion& lerp(quaternion q1, quaternion q2, f32 time); |
142 | 142 | ||
143 | //! Set this quaternion to the result of the spherical interpolation between two quaternions | 143 | //! Set this quaternion to the result of the spherical interpolation between two quaternions |
144 | /** \param q1 First quaternion to be interpolated. | 144 | /** \param q1 First quaternion to be interpolated. |
145 | \param q2 Second quaternion to be interpolated. | 145 | \param q2 Second quaternion to be interpolated. |
146 | \param time Progress of interpolation. For time=0 the result is | 146 | \param time Progress of interpolation. For time=0 the result is |
147 | q1, for time=1 the result is q2. Otherwise interpolation | 147 | q1, for time=1 the result is q2. Otherwise interpolation |
148 | between q1 and q2. | 148 | between q1 and q2. |
149 | \param threshold To avoid inaccuracies at the end (time=1) the | 149 | \param threshold To avoid inaccuracies at the end (time=1) the |
150 | interpolation switches to linear interpolation at some point. | 150 | interpolation switches to linear interpolation at some point. |
151 | This value defines how much of the remaining interpolation will | 151 | This value defines how much of the remaining interpolation will |
152 | be calculated with lerp. Everything from 1-threshold up will be | 152 | be calculated with lerp. Everything from 1-threshold up will be |
153 | linear interpolation. | 153 | linear interpolation. |
154 | */ | 154 | */ |
155 | quaternion& slerp(quaternion q1, quaternion q2, | 155 | quaternion& slerp(quaternion q1, quaternion q2, |
156 | f32 time, f32 threshold=.05f); | 156 | f32 time, f32 threshold=.05f); |
157 | 157 | ||
158 | //! Create quaternion from rotation angle and rotation axis. | 158 | //! Create quaternion from rotation angle and rotation axis. |
159 | /** Axis must be unit length. | 159 | /** Axis must be unit length. |
160 | The quaternion representing the rotation is | 160 | The quaternion representing the rotation is |
161 | q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k). | 161 | q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k). |
162 | \param angle Rotation Angle in radians. | 162 | \param angle Rotation Angle in radians. |
163 | \param axis Rotation axis. */ | 163 | \param axis Rotation axis. */ |
164 | quaternion& fromAngleAxis (f32 angle, const vector3df& axis); | 164 | quaternion& fromAngleAxis (f32 angle, const vector3df& axis); |
165 | 165 | ||
166 | //! Fills an angle (radians) around an axis (unit vector) | 166 | //! Fills an angle (radians) around an axis (unit vector) |
167 | void toAngleAxis (f32 &angle, core::vector3df& axis) const; | 167 | void toAngleAxis (f32 &angle, core::vector3df& axis) const; |
168 | 168 | ||
169 | //! Output this quaternion to an euler angle (radians) | 169 | //! Output this quaternion to an euler angle (radians) |
170 | void toEuler(vector3df& euler) const; | 170 | void toEuler(vector3df& euler) const; |
171 | 171 | ||
172 | //! Set quaternion to identity | 172 | //! Set quaternion to identity |
173 | quaternion& makeIdentity(); | 173 | quaternion& makeIdentity(); |
174 | 174 | ||
175 | //! Set quaternion to represent a rotation from one vector to another. | 175 | //! Set quaternion to represent a rotation from one vector to another. |
176 | quaternion& rotationFromTo(const vector3df& from, const vector3df& to); | 176 | quaternion& rotationFromTo(const vector3df& from, const vector3df& to); |
177 | 177 | ||
178 | //! Quaternion elements. | 178 | //! Quaternion elements. |
179 | f32 X; // vectorial (imaginary) part | 179 | f32 X; // vectorial (imaginary) part |
180 | f32 Y; | 180 | f32 Y; |
181 | f32 Z; | 181 | f32 Z; |
182 | f32 W; // real part | 182 | f32 W; // real part |
183 | }; | 183 | }; |
184 | 184 | ||
185 | 185 | ||
186 | // Constructor which converts euler angles to a quaternion | 186 | // Constructor which converts euler angles to a quaternion |
187 | inline quaternion::quaternion(f32 x, f32 y, f32 z) | 187 | inline quaternion::quaternion(f32 x, f32 y, f32 z) |
188 | { | 188 | { |
189 | set(x,y,z); | 189 | set(x,y,z); |
190 | } | 190 | } |
191 | 191 | ||
192 | 192 | ||
193 | // Constructor which converts euler angles to a quaternion | 193 | // Constructor which converts euler angles to a quaternion |
194 | inline quaternion::quaternion(const vector3df& vec) | 194 | inline quaternion::quaternion(const vector3df& vec) |
195 | { | 195 | { |
196 | set(vec.X,vec.Y,vec.Z); | 196 | set(vec.X,vec.Y,vec.Z); |
197 | } | 197 | } |
198 | 198 | ||
199 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 199 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
200 | // Constructor which converts a matrix to a quaternion | 200 | // Constructor which converts a matrix to a quaternion |
201 | inline quaternion::quaternion(const matrix4& mat) | 201 | inline quaternion::quaternion(const matrix4& mat) |
202 | { | 202 | { |
203 | (*this) = mat; | 203 | (*this) = mat; |
204 | } | 204 | } |
205 | #endif | 205 | #endif |
206 | 206 | ||
207 | // equal operator | 207 | // equal operator |
208 | inline bool quaternion::operator==(const quaternion& other) const | 208 | inline bool quaternion::operator==(const quaternion& other) const |
209 | { | 209 | { |
210 | return ((X == other.X) && | 210 | return ((X == other.X) && |
211 | (Y == other.Y) && | 211 | (Y == other.Y) && |
212 | (Z == other.Z) && | 212 | (Z == other.Z) && |
213 | (W == other.W)); | 213 | (W == other.W)); |
214 | } | 214 | } |
215 | 215 | ||
216 | // inequality operator | 216 | // inequality operator |
217 | inline bool quaternion::operator!=(const quaternion& other) const | 217 | inline bool quaternion::operator!=(const quaternion& other) const |
218 | { | 218 | { |
219 | return !(*this == other); | 219 | return !(*this == other); |
220 | } | 220 | } |
221 | 221 | ||
222 | // assignment operator | 222 | // assignment operator |
223 | inline quaternion& quaternion::operator=(const quaternion& other) | 223 | inline quaternion& quaternion::operator=(const quaternion& other) |
224 | { | 224 | { |
225 | X = other.X; | 225 | X = other.X; |
226 | Y = other.Y; | 226 | Y = other.Y; |
227 | Z = other.Z; | 227 | Z = other.Z; |
228 | W = other.W; | 228 | W = other.W; |
229 | return *this; | 229 | return *this; |
230 | } | 230 | } |
231 | 231 | ||
232 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 232 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
233 | // matrix assignment operator | 233 | // matrix assignment operator |
234 | inline quaternion& quaternion::operator=(const matrix4& m) | 234 | inline quaternion& quaternion::operator=(const matrix4& m) |
235 | { | 235 | { |
236 | const f32 diag = m[0] + m[5] + m[10] + 1; | 236 | const f32 diag = m[0] + m[5] + m[10] + 1; |
237 | 237 | ||
238 | if( diag > 0.0f ) | 238 | if( diag > 0.0f ) |
239 | { | 239 | { |
240 | const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal | 240 | const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal |
241 | 241 | ||
242 | // TODO: speed this up | 242 | // TODO: speed this up |
243 | X = (m[6] - m[9]) / scale; | 243 | X = (m[6] - m[9]) / scale; |
244 | Y = (m[8] - m[2]) / scale; | 244 | Y = (m[8] - m[2]) / scale; |
245 | Z = (m[1] - m[4]) / scale; | 245 | Z = (m[1] - m[4]) / scale; |
246 | W = 0.25f * scale; | 246 | W = 0.25f * scale; |
247 | } | 247 | } |
248 | else | 248 | else |
249 | { | 249 | { |
250 | if (m[0]>m[5] && m[0]>m[10]) | 250 | if (m[0]>m[5] && m[0]>m[10]) |
251 | { | 251 | { |
252 | // 1st element of diag is greatest value | 252 | // 1st element of diag is greatest value |
253 | // find scale according to 1st element, and double it | 253 | // find scale according to 1st element, and double it |
254 | const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f; | 254 | const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f; |
255 | 255 | ||
256 | // TODO: speed this up | 256 | // TODO: speed this up |
257 | X = 0.25f * scale; | 257 | X = 0.25f * scale; |
258 | Y = (m[4] + m[1]) / scale; | 258 | Y = (m[4] + m[1]) / scale; |
259 | Z = (m[2] + m[8]) / scale; | 259 | Z = (m[2] + m[8]) / scale; |
260 | W = (m[6] - m[9]) / scale; | 260 | W = (m[6] - m[9]) / scale; |
261 | } | 261 | } |
262 | else if (m[5]>m[10]) | 262 | else if (m[5]>m[10]) |
263 | { | 263 | { |
264 | // 2nd element of diag is greatest value | 264 | // 2nd element of diag is greatest value |
265 | // find scale according to 2nd element, and double it | 265 | // find scale according to 2nd element, and double it |
266 | const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f; | 266 | const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f; |
267 | 267 | ||
268 | // TODO: speed this up | 268 | // TODO: speed this up |
269 | X = (m[4] + m[1]) / scale; | 269 | X = (m[4] + m[1]) / scale; |
270 | Y = 0.25f * scale; | 270 | Y = 0.25f * scale; |
271 | Z = (m[9] + m[6]) / scale; | 271 | Z = (m[9] + m[6]) / scale; |
272 | W = (m[8] - m[2]) / scale; | 272 | W = (m[8] - m[2]) / scale; |
273 | } | 273 | } |
274 | else | 274 | else |
275 | { | 275 | { |
276 | // 3rd element of diag is greatest value | 276 | // 3rd element of diag is greatest value |
277 | // find scale according to 3rd element, and double it | 277 | // find scale according to 3rd element, and double it |
278 | const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f; | 278 | const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f; |
279 | 279 | ||
280 | // TODO: speed this up | 280 | // TODO: speed this up |
281 | X = (m[8] + m[2]) / scale; | 281 | X = (m[8] + m[2]) / scale; |
282 | Y = (m[9] + m[6]) / scale; | 282 | Y = (m[9] + m[6]) / scale; |
283 | Z = 0.25f * scale; | 283 | Z = 0.25f * scale; |
284 | W = (m[1] - m[4]) / scale; | 284 | W = (m[1] - m[4]) / scale; |
285 | } | 285 | } |
286 | } | 286 | } |
287 | 287 | ||
288 | return normalize(); | 288 | return normalize(); |
289 | } | 289 | } |
290 | #endif | 290 | #endif |
291 | 291 | ||
292 | 292 | ||
293 | // multiplication operator | 293 | // multiplication operator |
294 | inline quaternion quaternion::operator*(const quaternion& other) const | 294 | inline quaternion quaternion::operator*(const quaternion& other) const |
295 | { | 295 | { |
296 | quaternion tmp; | 296 | quaternion tmp; |
297 | 297 | ||
298 | tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z); | 298 | tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z); |
299 | tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y); | 299 | tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y); |
300 | tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z); | 300 | tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z); |
301 | tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X); | 301 | tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X); |
302 | 302 | ||
303 | return tmp; | 303 | return tmp; |
304 | } | 304 | } |
305 | 305 | ||
306 | 306 | ||
307 | // multiplication operator | 307 | // multiplication operator |
308 | inline quaternion quaternion::operator*(f32 s) const | 308 | inline quaternion quaternion::operator*(f32 s) const |
309 | { | 309 | { |
310 | return quaternion(s*X, s*Y, s*Z, s*W); | 310 | return quaternion(s*X, s*Y, s*Z, s*W); |
311 | } | 311 | } |
312 | 312 | ||
313 | 313 | ||
314 | // multiplication operator | 314 | // multiplication operator |
315 | inline quaternion& quaternion::operator*=(f32 s) | 315 | inline quaternion& quaternion::operator*=(f32 s) |
316 | { | 316 | { |
317 | X*=s; | 317 | X*=s; |
318 | Y*=s; | 318 | Y*=s; |
319 | Z*=s; | 319 | Z*=s; |
320 | W*=s; | 320 | W*=s; |
321 | return *this; | 321 | return *this; |
322 | } | 322 | } |
323 | 323 | ||
324 | // multiplication operator | 324 | // multiplication operator |
325 | inline quaternion& quaternion::operator*=(const quaternion& other) | 325 | inline quaternion& quaternion::operator*=(const quaternion& other) |
326 | { | 326 | { |
327 | return (*this = other * (*this)); | 327 | return (*this = other * (*this)); |
328 | } | 328 | } |
329 | 329 | ||
330 | // add operator | 330 | // add operator |
331 | inline quaternion quaternion::operator+(const quaternion& b) const | 331 | inline quaternion quaternion::operator+(const quaternion& b) const |
332 | { | 332 | { |
333 | return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W); | 333 | return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W); |
334 | } | 334 | } |
335 | 335 | ||
336 | #if !IRR_TEST_BROKEN_QUATERNION_USE | 336 | #if !IRR_TEST_BROKEN_QUATERNION_USE |
337 | // Creates a matrix from this quaternion | 337 | // Creates a matrix from this quaternion |
338 | inline matrix4 quaternion::getMatrix() const | 338 | inline matrix4 quaternion::getMatrix() const |
339 | { | 339 | { |
340 | core::matrix4 m; | 340 | core::matrix4 m; |
341 | getMatrix(m); | 341 | getMatrix(m); |
342 | return m; | 342 | return m; |
343 | } | 343 | } |
344 | #endif | 344 | #endif |
345 | 345 | ||
346 | /*! | 346 | /*! |
347 | Creates a matrix from this quaternion | 347 | Creates a matrix from this quaternion |
348 | */ | 348 | */ |
349 | inline void quaternion::getMatrix(matrix4 &dest, | 349 | inline void quaternion::getMatrix(matrix4 &dest, |
350 | const core::vector3df ¢er) const | 350 | const core::vector3df ¢er) const |
351 | { | 351 | { |
352 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; | 352 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; |
353 | dest[1] = 2.0f*X*Y + 2.0f*Z*W; | 353 | dest[1] = 2.0f*X*Y + 2.0f*Z*W; |
354 | dest[2] = 2.0f*X*Z - 2.0f*Y*W; | 354 | dest[2] = 2.0f*X*Z - 2.0f*Y*W; |
355 | dest[3] = 0.0f; | 355 | dest[3] = 0.0f; |
356 | 356 | ||
357 | dest[4] = 2.0f*X*Y - 2.0f*Z*W; | 357 | dest[4] = 2.0f*X*Y - 2.0f*Z*W; |
358 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; | 358 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; |
359 | dest[6] = 2.0f*Z*Y + 2.0f*X*W; | 359 | dest[6] = 2.0f*Z*Y + 2.0f*X*W; |
360 | dest[7] = 0.0f; | 360 | dest[7] = 0.0f; |
361 | 361 | ||
362 | dest[8] = 2.0f*X*Z + 2.0f*Y*W; | 362 | dest[8] = 2.0f*X*Z + 2.0f*Y*W; |
363 | dest[9] = 2.0f*Z*Y - 2.0f*X*W; | 363 | dest[9] = 2.0f*Z*Y - 2.0f*X*W; |
364 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; | 364 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; |
365 | dest[11] = 0.0f; | 365 | dest[11] = 0.0f; |
366 | 366 | ||
367 | dest[12] = center.X; | 367 | dest[12] = center.X; |
368 | dest[13] = center.Y; | 368 | dest[13] = center.Y; |
369 | dest[14] = center.Z; | 369 | dest[14] = center.Z; |
370 | dest[15] = 1.f; | 370 | dest[15] = 1.f; |
371 | 371 | ||
372 | dest.setDefinitelyIdentityMatrix ( false ); | 372 | dest.setDefinitelyIdentityMatrix ( false ); |
373 | } | 373 | } |
374 | 374 | ||
375 | 375 | ||
376 | /*! | 376 | /*! |
377 | Creates a matrix from this quaternion | 377 | Creates a matrix from this quaternion |
378 | Rotate about a center point | 378 | Rotate about a center point |
379 | shortcut for | 379 | shortcut for |
380 | core::quaternion q; | 380 | core::quaternion q; |
381 | q.rotationFromTo(vin[i].Normal, forward); | 381 | q.rotationFromTo(vin[i].Normal, forward); |
382 | q.getMatrix(lookat, center); | 382 | q.getMatrix(lookat, center); |
383 | 383 | ||
384 | core::matrix4 m2; | 384 | core::matrix4 m2; |
385 | m2.setInverseTranslation(center); | 385 | m2.setInverseTranslation(center); |
386 | lookat *= m2; | 386 | lookat *= m2; |
387 | */ | 387 | */ |
388 | inline void quaternion::getMatrixCenter(matrix4 &dest, | 388 | inline void quaternion::getMatrixCenter(matrix4 &dest, |
389 | const core::vector3df ¢er, | 389 | const core::vector3df ¢er, |
390 | const core::vector3df &translation) const | 390 | const core::vector3df &translation) const |
391 | { | 391 | { |
392 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; | 392 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; |
393 | dest[1] = 2.0f*X*Y + 2.0f*Z*W; | 393 | dest[1] = 2.0f*X*Y + 2.0f*Z*W; |
394 | dest[2] = 2.0f*X*Z - 2.0f*Y*W; | 394 | dest[2] = 2.0f*X*Z - 2.0f*Y*W; |
395 | dest[3] = 0.0f; | 395 | dest[3] = 0.0f; |
396 | 396 | ||
397 | dest[4] = 2.0f*X*Y - 2.0f*Z*W; | 397 | dest[4] = 2.0f*X*Y - 2.0f*Z*W; |
398 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; | 398 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; |
399 | dest[6] = 2.0f*Z*Y + 2.0f*X*W; | 399 | dest[6] = 2.0f*Z*Y + 2.0f*X*W; |
400 | dest[7] = 0.0f; | 400 | dest[7] = 0.0f; |
401 | 401 | ||
402 | dest[8] = 2.0f*X*Z + 2.0f*Y*W; | 402 | dest[8] = 2.0f*X*Z + 2.0f*Y*W; |
403 | dest[9] = 2.0f*Z*Y - 2.0f*X*W; | 403 | dest[9] = 2.0f*Z*Y - 2.0f*X*W; |
404 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; | 404 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; |
405 | dest[11] = 0.0f; | 405 | dest[11] = 0.0f; |
406 | 406 | ||
407 | dest.setRotationCenter ( center, translation ); | 407 | dest.setRotationCenter ( center, translation ); |
408 | } | 408 | } |
409 | 409 | ||
410 | // Creates a matrix from this quaternion | 410 | // Creates a matrix from this quaternion |
411 | inline void quaternion::getMatrix_transposed(matrix4 &dest) const | 411 | inline void quaternion::getMatrix_transposed(matrix4 &dest) const |
412 | { | 412 | { |
413 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; | 413 | dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z; |
414 | dest[4] = 2.0f*X*Y + 2.0f*Z*W; | 414 | dest[4] = 2.0f*X*Y + 2.0f*Z*W; |
415 | dest[8] = 2.0f*X*Z - 2.0f*Y*W; | 415 | dest[8] = 2.0f*X*Z - 2.0f*Y*W; |
416 | dest[12] = 0.0f; | 416 | dest[12] = 0.0f; |
417 | 417 | ||
418 | dest[1] = 2.0f*X*Y - 2.0f*Z*W; | 418 | dest[1] = 2.0f*X*Y - 2.0f*Z*W; |
419 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; | 419 | dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z; |
420 | dest[9] = 2.0f*Z*Y + 2.0f*X*W; | 420 | dest[9] = 2.0f*Z*Y + 2.0f*X*W; |
421 | dest[13] = 0.0f; | 421 | dest[13] = 0.0f; |
422 | 422 | ||
423 | dest[2] = 2.0f*X*Z + 2.0f*Y*W; | 423 | dest[2] = 2.0f*X*Z + 2.0f*Y*W; |
424 | dest[6] = 2.0f*Z*Y - 2.0f*X*W; | 424 | dest[6] = 2.0f*Z*Y - 2.0f*X*W; |
425 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; | 425 | dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y; |
426 | dest[14] = 0.0f; | 426 | dest[14] = 0.0f; |
427 | 427 | ||
428 | dest[3] = 0.f; | 428 | dest[3] = 0.f; |
429 | dest[7] = 0.f; | 429 | dest[7] = 0.f; |
430 | dest[11] = 0.f; | 430 | dest[11] = 0.f; |
431 | dest[15] = 1.f; | 431 | dest[15] = 1.f; |
432 | 432 | ||
433 | dest.setDefinitelyIdentityMatrix(false); | 433 | dest.setDefinitelyIdentityMatrix(false); |
434 | } | 434 | } |
435 | 435 | ||
436 | 436 | ||
437 | // Inverts this quaternion | 437 | // Inverts this quaternion |
438 | inline quaternion& quaternion::makeInverse() | 438 | inline quaternion& quaternion::makeInverse() |
439 | { | 439 | { |
440 | X = -X; Y = -Y; Z = -Z; | 440 | X = -X; Y = -Y; Z = -Z; |
441 | return *this; | 441 | return *this; |
442 | } | 442 | } |
443 | 443 | ||
444 | 444 | ||
445 | // sets new quaternion | 445 | // sets new quaternion |
446 | inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w) | 446 | inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w) |
447 | { | 447 | { |
448 | X = x; | 448 | X = x; |
449 | Y = y; | 449 | Y = y; |
450 | Z = z; | 450 | Z = z; |
451 | W = w; | 451 | W = w; |
452 | return *this; | 452 | return *this; |
453 | } | 453 | } |
454 | 454 | ||
455 | 455 | ||
456 | // sets new quaternion based on euler angles | 456 | // sets new quaternion based on euler angles |
457 | inline quaternion& quaternion::set(f32 x, f32 y, f32 z) | 457 | inline quaternion& quaternion::set(f32 x, f32 y, f32 z) |
458 | { | 458 | { |
459 | f64 angle; | 459 | f64 angle; |
460 | 460 | ||
461 | angle = x * 0.5; | 461 | angle = x * 0.5; |
462 | const f64 sr = sin(angle); | 462 | const f64 sr = sin(angle); |
463 | const f64 cr = cos(angle); | 463 | const f64 cr = cos(angle); |
464 | 464 | ||
465 | angle = y * 0.5; | 465 | angle = y * 0.5; |
466 | const f64 sp = sin(angle); | 466 | const f64 sp = sin(angle); |
467 | const f64 cp = cos(angle); | 467 | const f64 cp = cos(angle); |
468 | 468 | ||
469 | angle = z * 0.5; | 469 | angle = z * 0.5; |
470 | const f64 sy = sin(angle); | 470 | const f64 sy = sin(angle); |
471 | const f64 cy = cos(angle); | 471 | const f64 cy = cos(angle); |
472 | 472 | ||
473 | const f64 cpcy = cp * cy; | 473 | const f64 cpcy = cp * cy; |
474 | const f64 spcy = sp * cy; | 474 | const f64 spcy = sp * cy; |
475 | const f64 cpsy = cp * sy; | 475 | const f64 cpsy = cp * sy; |
476 | const f64 spsy = sp * sy; | 476 | const f64 spsy = sp * sy; |
477 | 477 | ||
478 | X = (f32)(sr * cpcy - cr * spsy); | 478 | X = (f32)(sr * cpcy - cr * spsy); |
479 | Y = (f32)(cr * spcy + sr * cpsy); | 479 | Y = (f32)(cr * spcy + sr * cpsy); |
480 | Z = (f32)(cr * cpsy - sr * spcy); | 480 | Z = (f32)(cr * cpsy - sr * spcy); |
481 | W = (f32)(cr * cpcy + sr * spsy); | 481 | W = (f32)(cr * cpcy + sr * spsy); |
482 | 482 | ||
483 | return normalize(); | 483 | return normalize(); |
484 | } | 484 | } |
485 | 485 | ||
486 | // sets new quaternion based on euler angles | 486 | // sets new quaternion based on euler angles |
487 | inline quaternion& quaternion::set(const core::vector3df& vec) | 487 | inline quaternion& quaternion::set(const core::vector3df& vec) |
488 | { | 488 | { |
489 | return set(vec.X, vec.Y, vec.Z); | 489 | return set(vec.X, vec.Y, vec.Z); |
490 | } | 490 | } |
491 | 491 | ||
492 | // sets new quaternion based on other quaternion | 492 | // sets new quaternion based on other quaternion |
493 | inline quaternion& quaternion::set(const core::quaternion& quat) | 493 | inline quaternion& quaternion::set(const core::quaternion& quat) |
494 | { | 494 | { |
495 | return (*this=quat); | 495 | return (*this=quat); |
496 | } | 496 | } |
497 | 497 | ||
498 | 498 | ||
499 | //! returns if this quaternion equals the other one, taking floating point rounding errors into account | 499 | //! returns if this quaternion equals the other one, taking floating point rounding errors into account |
500 | inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const | 500 | inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const |
501 | { | 501 | { |
502 | return core::equals(X, other.X, tolerance) && | 502 | return core::equals(X, other.X, tolerance) && |
503 | core::equals(Y, other.Y, tolerance) && | 503 | core::equals(Y, other.Y, tolerance) && |
504 | core::equals(Z, other.Z, tolerance) && | 504 | core::equals(Z, other.Z, tolerance) && |
505 | core::equals(W, other.W, tolerance); | 505 | core::equals(W, other.W, tolerance); |
506 | } | 506 | } |
507 | 507 | ||
508 | 508 | ||
509 | // normalizes the quaternion | 509 | // normalizes the quaternion |
510 | inline quaternion& quaternion::normalize() | 510 | inline quaternion& quaternion::normalize() |
511 | { | 511 | { |
512 | const f32 n = X*X + Y*Y + Z*Z + W*W; | 512 | const f32 n = X*X + Y*Y + Z*Z + W*W; |
513 | 513 | ||
514 | if (n == 1) | 514 | if (n == 1) |
515 | return *this; | 515 | return *this; |
516 | 516 | ||
517 | //n = 1.0f / sqrtf(n); | 517 | //n = 1.0f / sqrtf(n); |
518 | return (*this *= reciprocal_squareroot ( n )); | 518 | return (*this *= reciprocal_squareroot ( n )); |
519 | } | 519 | } |
520 | 520 | ||
521 | 521 | ||
522 | // set this quaternion to the result of the linear interpolation between two quaternions | 522 | // set this quaternion to the result of the linear interpolation between two quaternions |
523 | inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time) | 523 | inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time) |
524 | { | 524 | { |
525 | const f32 scale = 1.0f - time; | 525 | const f32 scale = 1.0f - time; |
526 | return (*this = (q1*scale) + (q2*time)); | 526 | return (*this = (q1*scale) + (q2*time)); |
527 | } | 527 | } |
528 | 528 | ||
529 | 529 | ||
530 | // set this quaternion to the result of the interpolation between two quaternions | 530 | // set this quaternion to the result of the interpolation between two quaternions |
531 | inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold) | 531 | inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold) |
532 | { | 532 | { |
533 | f32 angle = q1.dotProduct(q2); | 533 | f32 angle = q1.dotProduct(q2); |
534 | 534 | ||
535 | // make sure we use the short rotation | 535 | // make sure we use the short rotation |
536 | if (angle < 0.0f) | 536 | if (angle < 0.0f) |
537 | { | 537 | { |
538 | q1 *= -1.0f; | 538 | q1 *= -1.0f; |
539 | angle *= -1.0f; | 539 | angle *= -1.0f; |
540 | } | 540 | } |
541 | 541 | ||
542 | if (angle <= (1-threshold)) // spherical interpolation | 542 | if (angle <= (1-threshold)) // spherical interpolation |
543 | { | 543 | { |
544 | const f32 theta = acosf(angle); | 544 | const f32 theta = acosf(angle); |
545 | const f32 invsintheta = reciprocal(sinf(theta)); | 545 | const f32 invsintheta = reciprocal(sinf(theta)); |
546 | const f32 scale = sinf(theta * (1.0f-time)) * invsintheta; | 546 | const f32 scale = sinf(theta * (1.0f-time)) * invsintheta; |
547 | const f32 invscale = sinf(theta * time) * invsintheta; | 547 | const f32 invscale = sinf(theta * time) * invsintheta; |
548 | return (*this = (q1*scale) + (q2*invscale)); | 548 | return (*this = (q1*scale) + (q2*invscale)); |
549 | } | 549 | } |
550 | else // linear interploation | 550 | else // linear interploation |
551 | return lerp(q1,q2,time); | 551 | return lerp(q1,q2,time); |
552 | } | 552 | } |
553 | 553 | ||
554 | 554 | ||
555 | // calculates the dot product | 555 | // calculates the dot product |
556 | inline f32 quaternion::dotProduct(const quaternion& q2) const | 556 | inline f32 quaternion::dotProduct(const quaternion& q2) const |
557 | { | 557 | { |
558 | return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W); | 558 | return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W); |
559 | } | 559 | } |
560 | 560 | ||
561 | 561 | ||
562 | //! axis must be unit length, angle in radians | 562 | //! axis must be unit length, angle in radians |
563 | inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis) | 563 | inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis) |
564 | { | 564 | { |
565 | const f32 fHalfAngle = 0.5f*angle; | 565 | const f32 fHalfAngle = 0.5f*angle; |
566 | const f32 fSin = sinf(fHalfAngle); | 566 | const f32 fSin = sinf(fHalfAngle); |
567 | W = cosf(fHalfAngle); | 567 | W = cosf(fHalfAngle); |
568 | X = fSin*axis.X; | 568 | X = fSin*axis.X; |
569 | Y = fSin*axis.Y; | 569 | Y = fSin*axis.Y; |
570 | Z = fSin*axis.Z; | 570 | Z = fSin*axis.Z; |
571 | return *this; | 571 | return *this; |
572 | } | 572 | } |
573 | 573 | ||
574 | 574 | ||
575 | inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const | 575 | inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const |
576 | { | 576 | { |
577 | const f32 scale = sqrtf(X*X + Y*Y + Z*Z); | 577 | const f32 scale = sqrtf(X*X + Y*Y + Z*Z); |
578 | 578 | ||
579 | if (core::iszero(scale) || W > 1.0f || W < -1.0f) | 579 | if (core::iszero(scale) || W > 1.0f || W < -1.0f) |
580 | { | 580 | { |
581 | angle = 0.0f; | 581 | angle = 0.0f; |
582 | axis.X = 0.0f; | 582 | axis.X = 0.0f; |
583 | axis.Y = 1.0f; | 583 | axis.Y = 1.0f; |
584 | axis.Z = 0.0f; | 584 | axis.Z = 0.0f; |
585 | } | 585 | } |
586 | else | 586 | else |
587 | { | 587 | { |
588 | const f32 invscale = reciprocal(scale); | 588 | const f32 invscale = reciprocal(scale); |
589 | angle = 2.0f * acosf(W); | 589 | angle = 2.0f * acosf(W); |
590 | axis.X = X * invscale; | 590 | axis.X = X * invscale; |
591 | axis.Y = Y * invscale; | 591 | axis.Y = Y * invscale; |
592 | axis.Z = Z * invscale; | 592 | axis.Z = Z * invscale; |
593 | } | 593 | } |
594 | } | 594 | } |
595 | 595 | ||
596 | inline void quaternion::toEuler(vector3df& euler) const | 596 | inline void quaternion::toEuler(vector3df& euler) const |
597 | { | 597 | { |
598 | const f64 sqw = W*W; | 598 | const f64 sqw = W*W; |
599 | const f64 sqx = X*X; | 599 | const f64 sqx = X*X; |
600 | const f64 sqy = Y*Y; | 600 | const f64 sqy = Y*Y; |
601 | const f64 sqz = Z*Z; | 601 | const f64 sqz = Z*Z; |
602 | const f64 test = 2.0 * (Y*W - X*Z); | 602 | const f64 test = 2.0 * (Y*W - X*Z); |
603 | 603 | ||
604 | if (core::equals(test, 1.0, 0.000001)) | 604 | if (core::equals(test, 1.0, 0.000001)) |
605 | { | 605 | { |
606 | // heading = rotation about z-axis | 606 | // heading = rotation about z-axis |
607 | euler.Z = (f32) (-2.0*atan2(X, W)); | 607 | euler.Z = (f32) (-2.0*atan2(X, W)); |
608 | // bank = rotation about x-axis | 608 | // bank = rotation about x-axis |
609 | euler.X = 0; | 609 | euler.X = 0; |
610 | // attitude = rotation about y-axis | 610 | // attitude = rotation about y-axis |
611 | euler.Y = (f32) (core::PI64/2.0); | 611 | euler.Y = (f32) (core::PI64/2.0); |
612 | } | 612 | } |
613 | else if (core::equals(test, -1.0, 0.000001)) | 613 | else if (core::equals(test, -1.0, 0.000001)) |
614 | { | 614 | { |
615 | // heading = rotation about z-axis | 615 | // heading = rotation about z-axis |
616 | euler.Z = (f32) (2.0*atan2(X, W)); | 616 | euler.Z = (f32) (2.0*atan2(X, W)); |
617 | // bank = rotation about x-axis | 617 | // bank = rotation about x-axis |
618 | euler.X = 0; | 618 | euler.X = 0; |
619 | // attitude = rotation about y-axis | 619 | // attitude = rotation about y-axis |
620 | euler.Y = (f32) (core::PI64/-2.0); | 620 | euler.Y = (f32) (core::PI64/-2.0); |
621 | } | 621 | } |
622 | else | 622 | else |
623 | { | 623 | { |
624 | // heading = rotation about z-axis | 624 | // heading = rotation about z-axis |
625 | euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)); | 625 | euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)); |
626 | // bank = rotation about x-axis | 626 | // bank = rotation about x-axis |
627 | euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)); | 627 | euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)); |
628 | // attitude = rotation about y-axis | 628 | // attitude = rotation about y-axis |
629 | euler.Y = (f32) asin( clamp(test, -1.0, 1.0) ); | 629 | euler.Y = (f32) asin( clamp(test, -1.0, 1.0) ); |
630 | } | 630 | } |
631 | } | 631 | } |
632 | 632 | ||
633 | 633 | ||
634 | inline vector3df quaternion::operator* (const vector3df& v) const | 634 | inline vector3df quaternion::operator* (const vector3df& v) const |
635 | { | 635 | { |
636 | // nVidia SDK implementation | 636 | // nVidia SDK implementation |
637 | 637 | ||
638 | vector3df uv, uuv; | 638 | vector3df uv, uuv; |
639 | vector3df qvec(X, Y, Z); | 639 | vector3df qvec(X, Y, Z); |
640 | uv = qvec.crossProduct(v); | 640 | uv = qvec.crossProduct(v); |
641 | uuv = qvec.crossProduct(uv); | 641 | uuv = qvec.crossProduct(uv); |
642 | uv *= (2.0f * W); | 642 | uv *= (2.0f * W); |
643 | uuv *= 2.0f; | 643 | uuv *= 2.0f; |
644 | 644 | ||
645 | return v + uv + uuv; | 645 | return v + uv + uuv; |
646 | } | 646 | } |
647 | 647 | ||
648 | // set quaternion to identity | 648 | // set quaternion to identity |
649 | inline core::quaternion& quaternion::makeIdentity() | 649 | inline core::quaternion& quaternion::makeIdentity() |
650 | { | 650 | { |
651 | W = 1.f; | 651 | W = 1.f; |
652 | X = 0.f; | 652 | X = 0.f; |
653 | Y = 0.f; | 653 | Y = 0.f; |
654 | Z = 0.f; | 654 | Z = 0.f; |
655 | return *this; | 655 | return *this; |
656 | } | 656 | } |
657 | 657 | ||
658 | inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to) | 658 | inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to) |
659 | { | 659 | { |
660 | // Based on Stan Melax's article in Game Programming Gems | 660 | // Based on Stan Melax's article in Game Programming Gems |
661 | // Copy, since cannot modify local | 661 | // Copy, since cannot modify local |
662 | vector3df v0 = from; | 662 | vector3df v0 = from; |
663 | vector3df v1 = to; | 663 | vector3df v1 = to; |
664 | v0.normalize(); | 664 | v0.normalize(); |
665 | v1.normalize(); | 665 | v1.normalize(); |
666 | 666 | ||
667 | const f32 d = v0.dotProduct(v1); | 667 | const f32 d = v0.dotProduct(v1); |
668 | if (d >= 1.0f) // If dot == 1, vectors are the same | 668 | if (d >= 1.0f) // If dot == 1, vectors are the same |
669 | { | 669 | { |
670 | return makeIdentity(); | 670 | return makeIdentity(); |
671 | } | 671 | } |
672 | else if (d <= -1.0f) // exactly opposite | 672 | else if (d <= -1.0f) // exactly opposite |
673 | { | 673 | { |
674 | core::vector3df axis(1.0f, 0.f, 0.f); | 674 | core::vector3df axis(1.0f, 0.f, 0.f); |
675 | axis = axis.crossProduct(v0); | 675 | axis = axis.crossProduct(v0); |
676 | if (axis.getLength()==0) | 676 | if (axis.getLength()==0) |
677 | { | 677 | { |
678 | axis.set(0.f,1.f,0.f); | 678 | axis.set(0.f,1.f,0.f); |
679 | axis.crossProduct(v0); | 679 | axis.crossProduct(v0); |
680 | } | 680 | } |
681 | // same as fromAngleAxis(core::PI, axis).normalize(); | 681 | // same as fromAngleAxis(core::PI, axis).normalize(); |
682 | return set(axis.X, axis.Y, axis.Z, 0).normalize(); | 682 | return set(axis.X, axis.Y, axis.Z, 0).normalize(); |
683 | } | 683 | } |
684 | 684 | ||
685 | const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt | 685 | const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt |
686 | const f32 invs = 1.f / s; | 686 | const f32 invs = 1.f / s; |
687 | const vector3df c = v0.crossProduct(v1)*invs; | 687 | const vector3df c = v0.crossProduct(v1)*invs; |
688 | return set(c.X, c.Y, c.Z, s * 0.5f).normalize(); | 688 | return set(c.X, c.Y, c.Z, s * 0.5f).normalize(); |
689 | } | 689 | } |
690 | 690 | ||
691 | 691 | ||
692 | } // end namespace core | 692 | } // end namespace core |
693 | } // end namespace irr | 693 | } // end namespace irr |
694 | 694 | ||
695 | #endif | 695 | #endif |
696 | 696 | ||