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: 13623 $
14 *---------------------------------------------------------------------------*/
15
16#include <cmath>
17
18namespace nn {
19namespace math {
20namespace ARMv6 {
21/*!
22    @name    クォータニオン
23    @{
24*/
25
26#define NN_QUAT_EPSILON        0.00001F
27
28
29/*!--------------------------------------------------------------------------*
30  @brief        クォータニオンの積を計算します。
31
32  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。q1, q2 と同じクォータニオンを指していても構いません。
33  @param[in]    q1    左辺値へのポインタ
34  @param[in]    q2    右辺値へのポインタ
35
36  @return       pOut を返します。
37 *---------------------------------------------------------------------------*/
38NN_MATH_INLINE QUAT*
39QUATMultC(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2)
40{
41    QUAT * __restrict pDst;
42    QUAT  tmp;
43
44    NN_NULL_ASSERT( pOut  );
45    NN_NULL_ASSERT( q1  );
46    NN_NULL_ASSERT( q2 );
47
48    if ( q1 == pOut || q2 == pOut )
49    {
50        pDst = &tmp;
51    }
52    else
53    {
54        pDst = pOut;
55    }
56
57    pDst->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
58    pDst->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
59    pDst->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
60    pDst->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
61
62    if ( pDst == &tmp )
63    {
64        *pOut = tmp;
65    }
66
67    return pOut;
68}
69NN_MATH_INLINE QUAT*
70QUATMultC_FAST(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2)
71{
72    register f32 x, y, z, w;
73    register f32 q1x, q1y, q1z, q1w;
74    register f32 q2x, q2y, q2z, q2w;
75
76    q1x = q1->x;
77    q1y = q1->y;
78    q1z = q1->z;
79    q1w = q1->w;
80
81    q2x = q2->x;
82    q2y = q2->y;
83    q2z = q2->z;
84    q2w = q2->w;
85
86    x = q1w * q2x + q1x * q2w + q1y * q2z - q1z * q2y;
87    y = q1w * q2y + q1y * q2w + q1z * q2x - q1x * q2z;
88    z = q1w * q2z + q1z * q2w + q1x * q2y - q1y * q2x;
89    w = q1w * q2w - q1x * q2x - q1y * q2y - q1z * q2z;
90
91    pOut->x = x;
92    pOut->y = y;
93    pOut->z = z;
94    pOut->w = w;
95
96    return pOut;
97
98}
99
100
101/*!--------------------------------------------------------------------------*
102  @brief        クォータニオンを正規化します。
103
104  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。q と同じクォータニオンを指していても構いません。
105  @param[in]    q     正規化するクォータニオンへのポインタ
106
107  @return       pOut を返します。
108 *---------------------------------------------------------------------------*/
109NN_MATH_INLINE QUAT*
110QUATNormalizeC(QUAT* pOut, const QUAT* __restrict q)
111{
112    f32 mag;
113
114    NN_NULL_ASSERT( q );
115    NN_NULL_ASSERT( pOut );
116
117    mag = (q->x * q->x) + (q->y * q->y) + (q->z * q->z) + (q->w * q->w);
118
119    if ( mag >= NN_QUAT_EPSILON )
120    {
121        mag = 1.0F / ::std::sqrtf(mag);
122
123        pOut->x = q->x * mag;
124        pOut->y = q->y * mag;
125        pOut->z = q->z * mag;
126        pOut->w = q->w * mag;
127    }
128    else
129    {
130        pOut->x = pOut->y = pOut->z = pOut->w = 0.0F;
131    }
132
133    return pOut;
134}
135
136NN_MATH_INLINE QUAT*
137QUATNormalizeC_FAST(QUAT* pOut, const QUAT* __restrict q)
138{
139    f32 mag;
140
141    NN_NULL_ASSERT( q );
142    NN_NULL_ASSERT( pOut );
143
144    register f32 x, y, z, w;
145
146    x = q->x;
147    y = q->y;
148    z = q->z;
149    w = q->w;
150
151    mag = (x * x) + (y * y) + (z * z) + (w * w);
152
153    if ( mag >= NN_QUAT_EPSILON )
154    {
155        mag = 1.0F / ::std::sqrtf(mag);
156
157        x = x * mag;
158        y = y * mag;
159        z = z * mag;
160        w = w * mag;
161
162        pOut->x = x;
163        pOut->y = y;
164        pOut->z = z;
165        pOut->w = w;
166
167    }
168    else
169    {
170        pOut->x = pOut->y = pOut->z = pOut->w = 0.0F;
171    }
172
173    return pOut;
174}
175
176/*!--------------------------------------------------------------------------*
177  @brief        クォータニオンの逆数を計算します。
178
179  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。q と同じクォータニオンを指していても構いません。
180  @param[in]    q     左辺値へのポインタ
181
182  @return       pOut を返します。
183 *---------------------------------------------------------------------------*/
184NN_MATH_INLINE QUAT*
185QUATInverseC(QUAT* pOut, const QUAT* __restrict q)
186{
187    f32 mag, norminv;
188
189    NN_NULL_ASSERT( pOut );
190    NN_NULL_ASSERT( q );
191
192    mag = ( q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w );
193
194    if ( mag == 0.0f )
195    {
196        mag = 1.0f;
197    }
198
199    // [Inverse] = [Conjugate] / [Magnitude]
200    norminv = 1.0f / mag;
201    pOut->x = -q->x * norminv;
202    pOut->y = -q->y * norminv;
203    pOut->z = -q->z * norminv;
204    pOut->w =  q->w * norminv;
205
206    return pOut;
207}
208NN_MATH_INLINE QUAT*
209QUATInverseC_FAST(QUAT* pOut, const QUAT* __restrict q)
210{
211    f32 mag, norminv;
212
213    NN_NULL_ASSERT( pOut );
214    NN_NULL_ASSERT( q );
215
216    register f32 x, y, z, w;
217
218    x = q->x;
219    y = q->y;
220    z = q->z;
221    w = q->w;
222
223    mag = ( x * x + y * y + z * z + w * w );
224
225    if ( mag == 0.0f )
226    {
227        mag = 1.0f;
228    }
229
230    // [Inverse] = [Conjugate] / [Magnitude]
231    norminv = 1.0f / mag;
232    x = -x * norminv;
233    y = -y * norminv;
234    z = -z * norminv;
235    w =  w * norminv;
236
237    pOut->x = x;
238    pOut->y = y;
239    pOut->z = z;
240    pOut->w = w;
241
242    return pOut;
243}
244/*!
245    @}
246*/
247}  //namespace ARMv6
248}  // namespace math
249}  // namespace nn
250
251