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: 45840 $
14 *---------------------------------------------------------------------------*/
15
16#include <cmath>
17
18namespace nn {
19namespace math {
20
21#define NN_QUAT_EPSILON        0.00001F
22
23NN_MATH_INLINE QUAT*
24QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2)
25{
26    NN_NULL_ASSERT( pOut );
27    NN_NULL_ASSERT( q1 );
28    NN_NULL_ASSERT( q2 );
29
30    pOut->x = q1->x + q2->x;
31    pOut->y = q1->y + q2->y;
32    pOut->z = q1->z + q2->z;
33    pOut->w = q1->w + q2->w;
34
35    return pOut;
36}
37
38NN_MATH_INLINE QUAT*
39QUATDivide(QUAT* pOut, const QUAT* q1, const QUAT* q2)
40{
41    QUAT qtmp;
42
43    NN_NULL_ASSERT( pOut );
44    NN_NULL_ASSERT( q1 );
45    NN_NULL_ASSERT( q2 );
46
47    QUATInverse(&qtmp, q2);
48    QUATMult(pOut, &qtmp, q1);
49
50    return pOut;
51}
52
53NN_MATH_INLINE f32
54QUATDot(const QUAT* q1, const QUAT* q2)
55{
56    NN_NULL_ASSERT( q1 );
57    NN_NULL_ASSERT( q2 );
58
59    return (q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w);
60}
61
62NN_MATH_INLINE QUAT*
63QUATExp(QUAT* pOut, const QUAT* __restrict q)
64{
65    f32 theta, scale;
66
67    NN_NULL_ASSERT( pOut );
68    NN_NULL_ASSERT( q );
69
70    // pure quaternion check
71    NN_ASSERT( q->w == 0.0F );
72
73    theta = ::std::sqrtf( q->x * q->x + q->y * q->y + q->z * q->z );
74    scale = 1.0F;
75
76    if ( theta > NN_QUAT_EPSILON )
77    {
78        scale = (f32)::std::sinf(theta) / theta;
79    }
80
81    pOut->x = scale * q->x;
82    pOut->y = scale * q->y;
83    pOut->z = scale * q->z;
84    pOut->w = (f32)::std::cosf(theta);
85
86    return pOut;
87}
88
89NN_MATH_INLINE QUAT*
90QUATLerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
91{
92    NN_NULL_ASSERT( pOut );
93    NN_NULL_ASSERT( q1 );
94    NN_NULL_ASSERT( q2 );
95
96    pOut->x = t * ( q2->x - q1->x ) + q1->x;
97    pOut->y = t * ( q2->y - q1->y ) + q1->y;
98    pOut->z = t * ( q2->z - q1->z ) + q1->z;
99    pOut->w = t * ( q2->w - q1->w ) + q1->w;
100
101    return pOut;
102}
103
104NN_MATH_INLINE QUAT*
105QUATLogN(QUAT* pOut, const QUAT* __restrict q)
106{
107    f32 theta, scale;
108
109    NN_NULL_ASSERT( pOut );
110    NN_NULL_ASSERT( q );
111
112    scale = q->x * q->x + q->y * q->y + q->z * q->z;
113
114    scale = ::std::sqrtf(scale);
115    theta = ::std::atan2f( scale, q->w );
116
117    if ( scale > 0.0F )
118    {
119        scale = theta / scale;
120    }
121
122    pOut->x = scale * q->x;
123    pOut->y = scale * q->y;
124    pOut->z = scale * q->z;
125    pOut->w = 0.0F;
126
127    return pOut;
128}
129
130NN_MATH_INLINE QUAT*
131QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto)
132{
133    f32 dot;
134
135    NN_NULL_ASSERT(pOut);
136    NN_NULL_ASSERT(q   );
137    NN_NULL_ASSERT(qto );
138
139    dot =  q->x * qto->x + q->y * qto->y + q->z * qto->z + q->w * qto->w;
140
141    if ( dot < 0.0f )
142    {
143        pOut->x = -q->x;
144        pOut->y = -q->y;
145        pOut->z = -q->z;
146        pOut->w = -q->w;
147    }
148    else
149    {
150        *pOut = *q;
151    }
152
153    return pOut;
154}
155
156NN_MATH_INLINE QUAT*
157QUATRotAxisRad( QUAT *pOut, const VEC3 *axis, f32 rad )
158{
159    f32 half, sh, ch;
160    VEC3 nAxis;
161
162    NN_NULL_ASSERT( pOut );
163    NN_NULL_ASSERT( axis );
164
165    VEC3Normalize(&nAxis, axis);
166
167    half = rad * 0.50F;
168    sh   = ::std::sinf(half);
169    ch   = ::std::cosf(half);
170
171    pOut->x = sh * nAxis.x;
172    pOut->y = sh * nAxis.y;
173    pOut->z = sh * nAxis.z;
174    pOut->w = ch;
175
176    return pOut;
177}
178
179NN_MATH_INLINE QUAT*
180QUATScale(QUAT* pOut, const QUAT* q, f32 scale)
181{
182    NN_NULL_ASSERT( pOut  );
183    NN_NULL_ASSERT( q  );
184
185    pOut->x = q->x * scale;
186    pOut->y = q->y * scale;
187    pOut->z = q->z * scale;
188    pOut->w = q->w * scale;
189
190    return pOut;
191}
192
193NN_MATH_INLINE QUAT*
194QUATSlerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
195{
196    f32 theta, sin_th, cos_th, tp, tq;
197
198    NN_NULL_ASSERT( pOut );
199    NN_NULL_ASSERT( q1 );
200    NN_NULL_ASSERT( q2 );
201
202    cos_th = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w;
203    tq     = 1.0F;
204
205    if ( cos_th < 0.0F )
206    {
207        cos_th = -cos_th;
208        tq     = -tq;
209    }
210
211    if ( cos_th <= 1.0F - NN_QUAT_EPSILON )
212    {
213        theta  = ::std::acosf(cos_th);
214        sin_th = ::std::sinf(theta);
215        tp     = ::std::sinf((1.0F - t) * theta) / sin_th;
216        tq    *= ::std::sinf( t * theta ) / sin_th;
217    }
218    else
219    {
220        // cos(theta) is close to 1.0F -> linear
221        tp = 1.0F - t;
222        tq = tq * t;
223    }
224
225    pOut->x = tp * q1->x + tq * q2->x;
226    pOut->y = tp * q1->y + tq * q2->y;
227    pOut->z = tp * q1->z + tq * q2->z;
228    pOut->w = tp * q1->w + tq * q2->w;
229
230    return pOut;
231}
232
233NN_MATH_INLINE QUAT*
234QUATSquad(QUAT* pOut, const QUAT* p, const QUAT* a, const QUAT* b, const QUAT* q, f32 t)
235{
236    QUAT pq, ab;
237    f32 t2;
238
239    NN_NULL_ASSERT( pOut );
240    NN_NULL_ASSERT( p );
241    NN_NULL_ASSERT( a );
242    NN_NULL_ASSERT( b );
243    NN_NULL_ASSERT( q );
244
245    t2 = 2 * t * ( 1.0F - t );
246    QUATSlerp(&pq, p, q, t);
247    QUATSlerp(&ab, a, b, t);
248    QUATSlerp(pOut, &pq, &ab, t2);
249
250    return pOut;
251}
252
253NN_MATH_INLINE QUAT*
254QUATSub(QUAT *pOut,  const QUAT *q1, const QUAT *q2)
255{
256    NN_NULL_ASSERT( pOut );
257    NN_NULL_ASSERT( q1 );
258    NN_NULL_ASSERT( q2 );
259
260    pOut->x = q1->x - q2->x;
261    pOut->y = q1->y - q2->y;
262    pOut->z = q1->z - q2->z;
263    pOut->w = q1->w - q2->w;
264
265    return pOut;
266}
267
268}  // namespace math
269}  // namespace nn
270
271