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