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/*!
22    @name    クォータニオン
23    @{
24*/
25
26#define NN_QUAT_EPSILON        0.00001F
27
28/*!--------------------------------------------------------------------------*
29  @brief        クォータニオンの和を計算します。
30
31  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。
32                      q1, q2 と同じクォータニオンを指していても構いません。
33  @param[in]    q1    左辺値へのポインタ
34  @param[in]    q2    右辺値へのポインタ
35
36  @return       pOut を返します。
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  @brief        クォータニオンの差を計算します。
56
57  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。
58                      q1, q2 と同じクォータニオンを指していても構いません。
59  @param[in]    q1    左辺値へのポインタ
60  @param[in]    q2    右辺値へのポインタ
61
62  @return       pOut を返します。
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  @brief        クォータニオンの商を計算します。
82
83  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。
84                q1, q2 と同じクォータニオンを指していても構いません。
85  @param[in]    q1    左辺値へのポインタ
86  @param[in]    q2    右辺値へのポインタ
87
88  @return       pOut を返します。
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  @brief        クォータニオンの内積を計算します。
108
109  @param[in]    q1    左辺値へのポインタ
110  @param[in]    q2    右辺値へのポインタ
111
112  @return       q1 と q2 の内積を返します。
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  @brief        クォータニオンの実数倍を計算します。
126
127  @param[out]   pOut   計算結果を受け取るバッファへのポインタ。
128                       q と同じクォータニオンを指していても構いません。
129  @param[in]    q      左辺値へのポインタ
130  @param[in]    scale  掛ける数。
131
132  @return       pOut を返します。
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  @brief        自然対数の底 e のクォータニオン乗を計算します。
151
152  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。
153                      q と同じクォータニオンを指していても構いません。
154  @param[in]    q     指数値となるクォータニオンへのポインタ。
155                      純粋なクォータニオン(w=0)でなければなりません。
156
157  @return       pOut を返します。
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  @brief        クォータニオンの自然対数を計算します。
189
190  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。
191                      q と同じクォータニオンを指していても構いません。
192  @param[in]    q     真数となるクォータニオンへのポインタ。
193
194  @return       pOut を返します。
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  @brief        2つのクォータニオンの線形補間を計算します。
225
226  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。q1, q2 と同じクォータニオンを指していても構いません。
227  @param[in]    q1    線形補間の始点となるクォータニオンへのポインタ
228  @param[in]    q2    線形補間の終点となるクォータニオンへのポインタ
229  @param[in]    t     線形補間のパラメータ。0.0 であれば q1 が 1.0 であれば q2 が結果となります。
230
231  @return       pOut を返します。
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  @brief        2つのクォータニオン間の球面線形補間を計算します。
251
252  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。q1, q2 と同じクォータニオンを指していても構いません。
253  @param[in]    q1    球面線形補間の始点となるクォータニオンへのポインタ
254  @param[in]    q2    球面線形補間の終点となるクォータニオンへのポインタ
255  @param[in]    t     球面線形補間のパラメータ。0.0 であれば q1 が 1.0 であれば q2 が結果となります。
256
257  @return       pOut を返します。
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  @brief        2つのクォータニオン間の球面3次補間を計算します。
302
303  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。p, a, b, q と同じクォータニオンを指していても構いません。
304  @param[in]    p     球面3次補間の始点となるクォータニオンへのポインタ
305  @param[in]    a     球面3次補間の制御用クォータニオンへのポインタ。
306  @param[in]    b     球面3次補間の制御用クォータニオンへのポインタ。
307  @param[in]    q     球面3次補間の終点となるクォータニオンへのポインタ
308  @param[in]    t     球面線形補間のパラメータ。0.0 であれば q1 が 1.0 であれば q2 が結果となります。
309
310  @return       pOut を返します。
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  @brief       Modify q so it is on the same side of the hypersphere as qto
335
336  @param[out]  pOut  resultant modified quaternion
337  @param[in]   q     quaternion
338  @param[in]   qto   quaternion to be close to
339
340  @return      NONE
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  @brief       axisによって指定された( x, y, z )からなる任意の軸に関する回転を
371               おこなうクォータニオンを設定します。
372
373  @param[out]  pOut  結果のクォータニオンへのポインタ。
374  @param[in]   axis  回転軸の成分を持つVec構造体へのポインタ。
375                       axisは、単位ベクトルである必要はありません。
376  @param[in]   rad   回転値(ラジアン)。反時計回りが正です。
377
378  @return      NONE
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