1/*---------------------------------------------------------------------------*
2  Project:  Horizon
3  File:     math_Quaternion.ipp
4  Copyright (C)2009-2010 Nintendo Co., Ltd.  All rights reserved.
5  These coded instructions, statements, and computer programs contain
6  proprietary information of Nintendo of America Inc. and/or Nintendo
7  Company Ltd., and are protected by Federal copyright law. They may
8  not be disclosed to third parties or copied or duplicated in any form,
9  in whole or in part, without the prior written consent of Nintendo.
10  $Revision: 17112 $
11 *---------------------------------------------------------------------------
12
13
14*/
15
16#include <cmath>
17
18namespace nn {
19namespace math {
20
21/* Please see man pages for details
22
23
24*/
25
26#define NN_QUAT_EPSILON        0.00001F
27
28/*
29
30
31
32
33
34
35
36
37*/
38NN_MATH_INLINE QUAT*
39QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2)
40{
41    NN_NULL_ASSERT( pOut );
42    NN_NULL_ASSERT( q1 );
43    NN_NULL_ASSERT( q2 );
44
45    pOut->x = q1->x + q2->x;
46    pOut->y = q1->y + q2->y;
47    pOut->z = q1->z + q2->z;
48    pOut->w = q1->w + q2->w;
49
50    return pOut;
51}
52
53
54/*
55
56
57
58
59
60
61
62
63*/
64NN_MATH_INLINE QUAT*
65QUATSub(QUAT *pOut,  const QUAT *q1, const QUAT *q2)
66{
67    NN_NULL_ASSERT( pOut );
68    NN_NULL_ASSERT( q1 );
69    NN_NULL_ASSERT( q2 );
70
71    pOut->x = q1->x - q2->x;
72    pOut->y = q1->y - q2->y;
73    pOut->z = q1->z - q2->z;
74    pOut->w = q1->w - q2->w;
75
76    return pOut;
77}
78
79
80/*
81
82
83
84
85
86
87
88
89*/
90NN_MATH_INLINE QUAT*
91QUATDivide(QUAT* pOut, const QUAT* q1, const QUAT* q2)
92{
93    QUAT qtmp;
94
95    NN_NULL_ASSERT( pOut );
96    NN_NULL_ASSERT( q1 );
97    NN_NULL_ASSERT( q2 );
98
99    QUATInverse(&qtmp, q2);
100    QUATMult(pOut, &qtmp, q1);
101
102    return pOut;
103}
104
105
106/*
107
108
109
110
111
112
113*/
114NN_MATH_INLINE f32
115QUATDot(const QUAT* q1, const QUAT* q2)
116{
117    NN_NULL_ASSERT( q1 );
118    NN_NULL_ASSERT( q2 );
119
120    return (q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w);
121}
122
123
124/*
125
126
127
128
129
130
131
132
133*/
134NN_MATH_INLINE QUAT*
135QUATScale(QUAT* pOut, const QUAT* q, f32 scale)
136{
137    NN_NULL_ASSERT( pOut  );
138    NN_NULL_ASSERT( q  );
139
140    pOut->x = q->x * scale;
141    pOut->y = q->y * scale;
142    pOut->z = q->z * scale;
143    pOut->w = q->w * scale;
144
145    return pOut;
146}
147
148
149/*
150
151
152
153
154
155
156
157
158*/
159NN_MATH_INLINE QUAT*
160QUATExp(QUAT* pOut, const QUAT* __restrict q)
161{
162    f32 theta, scale;
163
164    NN_NULL_ASSERT( pOut );
165    NN_NULL_ASSERT( q );
166
167    // Pure quaternion check
168    NN_ASSERT( q->w == 0.0F );
169
170    theta = ::std::sqrtf( q->x * q->x + q->y * q->y + q->z * q->z );
171    scale = 1.0F;
172
173    if ( theta > NN_QUAT_EPSILON )
174    {
175        scale = (f32)::std::sinf(theta) / theta;
176    }
177
178    pOut->x = scale * q->x;
179    pOut->y = scale * q->y;
180    pOut->z = scale * q->z;
181    pOut->w = (f32)::std::cosf(theta);
182
183    return pOut;
184}
185
186
187/*
188
189
190
191
192
193
194
195*/
196NN_MATH_INLINE QUAT*
197QUATLogN(QUAT* pOut, const QUAT* __restrict q)
198{
199    f32 theta, scale;
200
201    NN_NULL_ASSERT( pOut );
202    NN_NULL_ASSERT( q );
203
204    scale = q->x * q->x + q->y * q->y + q->z * q->z;
205
206    scale = ::std::sqrtf(scale);
207    theta = ::std::atan2f( scale, q->w );
208
209    if ( scale > 0.0F )
210    {
211        scale = theta / scale;
212    }
213
214    pOut->x = scale * q->x;
215    pOut->y = scale * q->y;
216    pOut->z = scale * q->z;
217    pOut->w = 0.0F;
218
219    return pOut;
220}
221
222
223/*
224
225
226
227
228
229
230
231
232*/
233NN_MATH_INLINE QUAT*
234QUATLerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
235{
236    NN_NULL_ASSERT( pOut );
237    NN_NULL_ASSERT( q1 );
238    NN_NULL_ASSERT( q2 );
239
240    pOut->x = t * ( q2->x - q1->x ) + q1->x;
241    pOut->y = t * ( q2->y - q1->y ) + q1->y;
242    pOut->z = t * ( q2->z - q1->z ) + q1->z;
243    pOut->w = t * ( q2->w - q1->w ) + q1->w;
244
245    return pOut;
246}
247
248
249/*
250
251
252
253
254
255
256
257
258*/
259NN_MATH_INLINE QUAT*
260QUATSlerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
261{
262    f32 theta, sin_th, cos_th, tp, tq;
263
264    NN_NULL_ASSERT( pOut );
265    NN_NULL_ASSERT( q1 );
266    NN_NULL_ASSERT( q2 );
267
268    cos_th = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w;
269    tq     = 1.0F;
270
271    if ( cos_th < 0.0F )
272    {
273        cos_th = -cos_th;
274        tq     = -tq;
275    }
276
277    if ( cos_th <= 1.0F - NN_QUAT_EPSILON )
278    {
279        theta  = ::std::acosf(cos_th);
280        sin_th = ::std::sinf(theta);
281        tp     = ::std::sinf((1.0F - t) * theta) / sin_th;
282        tq    *= ::std::sinf( t * theta ) / sin_th;
283    }
284    else
285    {
286        // cos(theta) is close to 1.0F -> linear
287        tp = 1.0F - t;
288        tq = tq * t;
289    }
290
291    pOut->x = tp * q1->x + tq * q2->x;
292    pOut->y = tp * q1->y + tq * q2->y;
293    pOut->z = tp * q1->z + tq * q2->z;
294    pOut->w = tp * q1->w + tq * q2->w;
295
296    return pOut;
297}
298
299
300/*
301
302
303
304
305
306
307
308
309
310
311*/
312NN_MATH_INLINE QUAT*
313QUATSquad(QUAT* pOut, const QUAT* p, const QUAT* a, const QUAT* b, const QUAT* q, f32 t)
314{
315    QUAT pq, ab;
316    f32 t2;
317
318    NN_NULL_ASSERT( pOut );
319    NN_NULL_ASSERT( p );
320    NN_NULL_ASSERT( a );
321    NN_NULL_ASSERT( b );
322    NN_NULL_ASSERT( q );
323
324    t2 = 2 * t * ( 1.0F - t );
325    QUATSlerp(&pq, p, q, t);
326    QUATSlerp(&ab, a, b, t);
327    QUATSlerp(pOut, &pq, &ab, t2);
328
329    return pOut;
330}
331
332
333/*
334
335
336
337
338
339
340
341*/
342NN_MATH_INLINE QUAT*
343QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto)
344{
345    f32 dot;
346
347    NN_NULL_ASSERT(pOut);
348    NN_NULL_ASSERT(q   );
349    NN_NULL_ASSERT(qto );
350
351    dot =  q->x * qto->x + q->y * qto->y + q->z * qto->z + q->w * qto->w;
352
353    if ( dot < 0.0f )
354    {
355        pOut->x = -q->x;
356        pOut->y = -q->y;
357        pOut->z = -q->z;
358        pOut->w = -q->w;
359    }
360    else
361    {
362        *pOut = *q;
363    }
364
365    return pOut;
366}
367
368
369/*
370
371
372
373
374
375
376
377
378
379*/
380NN_MATH_INLINE QUAT*
381QUATRotAxisRad( QUAT *pOut, const VEC3 *axis, f32 rad )
382{
383    f32 half, sh, ch;
384    VEC3 nAxis;
385
386    NN_NULL_ASSERT( pOut );
387    NN_NULL_ASSERT( axis );
388
389    VEC3Normalize(&nAxis, axis);
390
391    half = rad * 0.50F;
392    sh   = ::std::sinf(half);
393    ch   = ::std::cosf(half);
394
395    pOut->x = sh * nAxis.x;
396    pOut->y = sh * nAxis.y;
397    pOut->z = sh * nAxis.z;
398    pOut->w = ch;
399
400    return pOut;
401}
402
403/*
404
405*/
406
407}  // namespace math
408}  // namespace nn
409
410