1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 ///////////////////////////////////////////////////////////////////////////
15 // neQ inlines
16 ///////////////////////////////////////////////////////////////////////////
17
18 //=========================================================================
19
neQ(void)20 NEINLINE neQ::neQ( void )
21 {
22 Identity();
23 }
24
25 //=========================================================================
26
neQ(f32 x,f32 y,f32 z,f32 w)27 NEINLINE neQ::neQ( f32 x, f32 y, f32 z, f32 w )
28 {
29 X = x; Y = y; Z = z; W = w;
30 }
31
32 //==========================================================================
33
neQ(const neM4 & M)34 NEINLINE neQ::neQ( const neM4& M )
35 {
36 SetupFromMatrix( M );
37 }
38
39 //==========================================================================
40
Zero(void)41 NEINLINE void neQ::Zero( void )
42 {
43 X = Y = Z = W = 0;
44 }
45
46 //==========================================================================
47
Identity(void)48 NEINLINE void neQ::Identity( void )
49 {
50 X = Y = Z = 0; W = 1;
51 }
52
53 //==========================================================================
54
Invert(void)55 NEINLINE neQ& neQ::Invert( void )
56 {
57 // This can be done also be negating (X,Y,Z) stead of W
58 W = -W;
59 return *this;
60 }
61
62 //=========================================================================
63
BuildMatrix(void)64 NEINLINE neM4 neQ::BuildMatrix( void ) const
65 {
66 neM4 M;
67
68 const f32 tx = 2.0f*X;
69 const f32 ty = 2.0f*Y;
70 const f32 tz = 2.0f*Z;
71 const f32 twx = tx*W;
72 const f32 twy = ty*W;
73 const f32 twz = tz*W;
74 const f32 txx = tx*X;
75 const f32 txy = ty*X;
76 const f32 txz = tz*X;
77 const f32 tyy = ty*Y;
78 const f32 tyz = tz*Y;
79 const f32 tzz = tz*Z;
80
81 M.M[0][0] = 1.0f-(tyy+tzz);
82 M.M[1][0] = txy-twz;
83 M.M[2][0] = txz+twy;
84 M.M[0][1] = txy+twz;
85 M.M[1][1] = 1.0f-(txx+tzz);
86 M.M[2][1] = tyz-twx;
87 M.M[0][2] = txz-twy;
88 M.M[1][2] = tyz+twx;
89 M.M[2][2] = 1.0f-(txx+tyy);
90
91 M.M[3][0] = M.M[3][1] = M.M[3][2] =
92 M.M[0][3] = M.M[1][3] = M.M[2][3] = 0.0f;
93 M.M[3][3] = 1.0f;
94
95 return M;
96 }
97
BuildMatrix3(void)98 NEINLINE neM3 neQ::BuildMatrix3( void ) const
99 {
100 neM3 M;
101
102 const f32 tx = 2.0f*X;
103 const f32 ty = 2.0f*Y;
104 const f32 tz = 2.0f*Z;
105 const f32 twx = tx*W;
106 const f32 twy = ty*W;
107 const f32 twz = tz*W;
108 const f32 txx = tx*X;
109 const f32 txy = ty*X;
110 const f32 txz = tz*X;
111 const f32 tyy = ty*Y;
112 const f32 tyz = tz*Y;
113 const f32 tzz = tz*Z;
114
115 M.M[0][0] = 1.0f-(tyy+tzz);
116 M.M[1][0] = txy-twz;
117 M.M[2][0] = txz+twy;
118 M.M[0][1] = txy+twz;
119 M.M[1][1] = 1.0f-(txx+tzz);
120 M.M[2][1] = tyz-twx;
121 M.M[0][2] = txz-twy;
122 M.M[1][2] = tyz+twx;
123 M.M[2][2] = 1.0f-(txx+tyy);
124
125 return M;
126 }
127
128 //=========================================================================
129
SetupFromMatrix(const neM4 & Matrix)130 NEINLINE void neQ::SetupFromMatrix( const neM4& Matrix )
131 {
132 // squared magniudes of quaternion components
133 // first compute squared magnitudes of quaternion components - at least one
134 // will be greater than 0 since quaternion is unit magnitude
135 const f32 qs2 = 0.25f * (Matrix.M[0][0] + Matrix.M[1][1] + Matrix.M[2][2] + 1.0f );
136 const f32 qx2 = qs2 - 0.5f * (Matrix.M[1][1] + Matrix.M[2][2]);
137 const f32 qy2 = qs2 - 0.5f * (Matrix.M[2][2] + Matrix.M[0][0]);
138 const f32 qz2 = qs2 - 0.5f * (Matrix.M[0][0] + Matrix.M[1][1]);
139
140
141 // find maximum magnitude component
142 const s32 i = (qs2 > qx2 ) ?
143 ((qs2 > qy2) ? ((qs2 > qz2) ? 0 : 3) : ((qy2 > qz2) ? 2 : 3)) :
144 ((qx2 > qy2) ? ((qx2 > qz2) ? 1 : 3) : ((qy2 > qz2) ? 2 : 3));
145
146 // compute signed quaternion components using numerically stable method
147 switch(i)
148 {
149 case 0:
150 {
151 W = (f32)sqrt(qs2);
152 const f32 tmp = 0.25f / W;
153 X = (Matrix.M[1][2] - Matrix.M[2][1]) * tmp;
154 Y = (Matrix.M[2][0] - Matrix.M[0][2]) * tmp;
155 Z = (Matrix.M[0][1] - Matrix.M[1][0]) * tmp;
156 break;
157 }
158 case 1:
159 {
160 X = (f32)sqrt(qx2);
161 const f32 tmp = 0.25f / X;
162 W = (Matrix.M[1][2] - Matrix.M[2][1]) * tmp;
163 Y = (Matrix.M[1][0] + Matrix.M[0][1]) * tmp;
164 Z = (Matrix.M[2][0] + Matrix.M[0][2]) * tmp;
165 break;
166 }
167 case 2:
168 {
169 Y = (f32)sqrt(qy2);
170 const f32 tmp = 0.25f / Y;
171 W = (Matrix.M[2][0] - Matrix.M[0][2]) * tmp;
172 Z = (Matrix.M[2][1] + Matrix.M[1][2]) * tmp;
173 X = (Matrix.M[0][1] + Matrix.M[1][0]) * tmp;
174 break;
175 }
176 case 3:
177 {
178 Z = (f32)sqrt(qz2);
179 const f32 tmp = 0.25f / Z;
180 W = (Matrix.M[0][1] - Matrix.M[1][0]) * tmp;
181 X = (Matrix.M[0][2] + Matrix.M[2][0]) * tmp;
182 Y = (Matrix.M[1][2] + Matrix.M[2][1]) * tmp;
183 break;
184 }
185 }
186
187 // for consistency, force positive scalar component [ (s; v) = (-s; -v) ]
188 if( W < 0) *this = -*this;
189
190 // normalize, just to be safe
191 Normalize();
192 }
193
SetupFromMatrix3(const neM3 & Matrix)194 NEINLINE void neQ::SetupFromMatrix3( const neM3& Matrix )
195 {
196 neM4 m;
197
198 m.SetIdentity();
199
200 u32 i,j;
201
202 for (i = 0; i < 3; i++)
203 for (j = 0; j < 3; j++)
204 m.M[i][j] = Matrix.M[i][j];
205
206 SetupFromMatrix(m);
207 }
208
209 //=========================================================================
210
Dot(const neQ & Q)211 NEINLINE f32 neQ::Dot( const neQ& Q ) const
212 {
213 return Q.X*X + Q.Y*Y + Q.Z*Z + Q.W*W;
214 }
215
216 //=========================================================================
217
Normalize(void)218 NEINLINE neQ& neQ::Normalize( void )
219 {
220 f32 t;
221 f32 norm = Dot(*this);
222
223 // if (neRealsEqual(norm, 1.0f))
224 // return *this;
225
226 ASSERT(norm >= 0.0f);
227
228 t = 1.0f / (float)sqrt(norm);
229
230 X *= t;
231 Y *= t;
232 Z *= t;
233 W *= t;
234
235 return *this;
236 }
237
238 //=========================================================================
239
240 NEINLINE neQ& neQ::operator *= ( const neQ& Q )
241 {
242 *this = *this * Q;
243 return *this;
244 }
245
246 //=========================================================================
247
248 NEINLINE neQ& neQ::operator *= ( f32 S )
249 {
250 *this = *this * S;
251 return *this;
252 }
253
254 //==========================================================================
255
256 NEINLINE neQ& neQ::operator += ( const neQ& Q )
257 {
258 *this = *this + Q;
259 return *this;
260 }
261
262 //==========================================================================
263
264 NEINLINE neQ& neQ::operator -= ( const neQ& Q )
265 {
266 *this = *this - Q;
267 return *this;
268 }
269
270 //=========================================================================
271
272 NEINLINE neQ operator - ( const neQ& Q )
273 {
274 return neQ( -Q.X, -Q.Y, -Q.Z, -Q.W );
275 }
276
277 //==========================================================================
278
279 NEINLINE neQ operator * ( const neQ& q1, const neQ& q2 )
280 {
281 f32 tmp_0, //temporary variables
282 tmp_1,
283 tmp_2,
284 tmp_3,
285 tmp_4,
286 tmp_5,
287 tmp_6,
288 tmp_7,
289 tmp_8,
290 tmp_9;
291
292 neQ zq;
293
294 tmp_0 = (q1.Z - q1.Y) * (q2.Y - q2.Z);
295 tmp_1 = (q1.W + q1.X) * (q2.W + q2.X);
296 tmp_2 = (q1.W - q1.X) * (q2.Y + q2.Z);
297 tmp_3 = (q1.Y + q1.Z) * (q2.W - q2.X);
298 tmp_4 = (q1.Z - q1.X) * (q2.X - q2.Y);
299 tmp_5 = (q1.Z + q1.X) * (q2.X + q2.Y);
300 tmp_6 = (q1.W + q1.Y) * (q2.W - q2.Z);
301 tmp_7 = (q1.W - q1.Y) * (q2.W + q2.Z);
302 tmp_8 = tmp_5 + tmp_6 + tmp_7;
303 tmp_9 = 0.5f * (tmp_4 + tmp_8);
304
305
306 zq.X =tmp_1 + tmp_9 - tmp_8;
307 zq.Y =tmp_2 + tmp_9 - tmp_7;
308 zq.Z =tmp_3 + tmp_9 - tmp_6;
309 zq.W =tmp_0 + tmp_9 - tmp_5;
310
311 return zq;
312
313 }
314
315 //==========================================================================
316
GetAxisAngle(neV3 & Axis,neRadian & Angle)317 NEINLINE void neQ::GetAxisAngle( neV3& Axis, neRadian& Angle ) const
318 {
319 f32 sum = X*X + Y*Y + Z*Z;
320
321 if (neIsConsiderZero(sum))
322 {
323 Angle = 0.0f;
324
325 Axis.Set(1.0f, 0.0f, 0.0f);
326
327 return;
328 }
329 f32 OneOver = 1.0f/(f32)sqrtf( sum );
330
331 Axis.Set( OneOver * X, OneOver * Y, OneOver * Z );
332
333 f32 w = W;
334
335 if (neIsConsiderZero(W - 1.0f))
336 {
337 Angle = 0.0f;
338 }
339 else
340 {
341 Angle = 2.0f * (f32)acosf( w );
342 }
343 }
344
345 NEINLINE neV3 operator * ( const neQ& Q, const neV3& V )
346 {
347 neV3 Axis; Axis.Set(Q.X, Q.Y, Q.Z);
348 neV3 uv; uv.Set( Axis.Cross( V ) );
349 neV3 uuv; uuv.Set( Axis.Cross( uv ) * 2.0f );
350
351 return V + uuv + ( uv * Q.W * 2.0f );
352 }
353
354 //==========================================================================
355
356 NEINLINE neQ operator * ( const neQ& Q, f32 S )
357 {
358 return neQ( Q.X*S, Q.Y*S, Q.Z*S, Q.W*S );
359 }
360
361 //==========================================================================
362
363 NEINLINE neQ operator * ( f32 S, const neQ& Q )
364 {
365 return Q * S;
366 }
367
368 //==========================================================================
369
370 NEINLINE neQ operator + ( const neQ& Qa, const neQ& Qb )
371 {
372 return neQ( Qa.X + Qb.X, Qa.Y + Qb.Y, Qa.Z + Qb.Z, Qa.W + Qb.W );
373 }
374
375 //==========================================================================
376
377 NEINLINE neQ operator - ( const neQ& Qa, const neQ& Qb )
378 {
379 return neQ( Qa.X - Qb.X, Qa.Y - Qb.Y, Qa.Z - Qb.Z, Qa.W - Qb.W );
380 }
381
Set(f32 _X,f32 _Y,f32 _Z,f32 _W)382 NEINLINE neQ& neQ::Set(f32 _X, f32 _Y, f32 _Z, f32 _W)
383 {
384 X = _X;
385 Y = _Y;
386 Z = _Z;
387 W = _W;
388 return (*this);
389 }
390
Set(const neV3 & V,f32 W)391 NEINLINE neQ& neQ::Set(const neV3 & V, f32 W)
392 {
393 return Set(V[0], V[1], V[2], W);
394 }
395
Set(f32 angle,const neV3 & axis)396 NEINLINE neQ& neQ::Set(f32 angle, const neV3 & axis)
397 {
398 f32 halfAngle = angle * 0.5f;
399
400 f32 sinHalfAngle = sinf(halfAngle);
401
402 f32 cosHalfAngle = cosf(halfAngle);
403
404 neV3 tmp = axis * sinHalfAngle;
405
406 Set(tmp, cosHalfAngle);
407
408 return (*this);
409 }
410
IsFinite()411 NEINLINE neBool neQ::IsFinite()
412 {
413 return (neFinite(X) &&
414 neFinite(Y) &&
415 neFinite(Z) &&
416 neFinite(W) );
417 }