1/*---------------------------------------------------------------------------*
2  Project:  Horizon
3  File:     math_Quaternion.ipp
4
5  Copyright (C)2009-2010 Nintendo Co., Ltd.  All rights reserved.
6
7  These coded instructions, statements, and computer programs contain
8  proprietary information of Nintendo of America Inc. and/or Nintendo
9  Company Ltd., and are protected by Federal copyright law.  They may
10  not be disclosed to third parties or copied or duplicated in any form,
11  in whole or in part, without the prior written consent of Nintendo.
12
13  $Revision: 17112 $
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