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/* 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*
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
103
104
105
106
107
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
178
179
180
181
182
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