1 /*---------------------------------------------------------------------------*
2 Project: TwlSDK - FX -
3 File: fx.c
4
5 Copyright 2003-2008 Nintendo. 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 $Date:: 2008-09-18#$
14 $Rev: 8573 $
15 $Author: okubata_ryoma $
16 *---------------------------------------------------------------------------*/
17
18 #include <nitro/fx/fx.h>
19 #include <nitro/fx/fx_const.h>
20 #include <nitro/fx/fx_trig.h>
21
22 //
23 // Workaround for no macro warnings on CW:
24 // - _MSL_USE_INLINE in MSL_C\MSL_ARM\Include\math.ARM.h
25 //
26 #ifdef __MWERKS__
27 # if !defined(_MSL_USE_INLINE) && __option(dont_inline)
28 # define _MSL_USE_INLINE 0
29 # endif
30 #endif
31
32 #include <math.h>
33
34 #define SDK_FX_INIT_2PI (4096 * 2)
35 #define SDK_FX_INIT_PI_2 (4096 * 2 / 4)
36 #define SDK_FX_INIT_PI (4096 * 2 / 2)
37
38
39 /*---------------------------------------------------------------------------*
40 Name: FX_Init
41
42 Description: This is an empty function if SDK_FX_DYNAMIC_TABLE is
43 not defined(default). Otherwise, initializes the sin/cos
44 table.
45
46 Arguments: none
47
48 Returns: none
49 *---------------------------------------------------------------------------*/
50 // Initialize a sin/cos table.
51 // To avoid increasing a margin of error, angle sum and difference relations are not used extensively.
FX_Init()52 void FX_Init()
53 {
54 SDK_ASSERT(2 == sizeof(fx16));
55 SDK_ASSERT(4 == sizeof(fx32));
56 SDK_ASSERT(8 == sizeof(fx64));
57 SDK_ASSERT(8 == sizeof(fx64c));
58 SDK_ASSERT(6 == sizeof(VecFx16));
59 SDK_ASSERT(12 == sizeof(VecFx32));
60
61 SDK_ASSERT(64 == sizeof(MtxFx44));
62 SDK_ASSERT(48 == sizeof(MtxFx43));
63 SDK_ASSERT(36 == sizeof(MtxFx33));
64 SDK_ASSERT(16 == sizeof(MtxFx22));
65
66 #ifdef SDK_FX_DYNAMIC_TABLE
67 #if 1
68 // FX_SinFx64c version:
69 // The table constructed is not the best because 'rad' cannot be precise enough.
70 // Each value may be different from the static one by 1/4096.
71
72 fx64c idx, ss;
73 fx32 rad;
74 fx16 s;
75 int i;
76
77 for (idx = 0, i = 0; i <= 4096 / 4; idx += (FX64C_TWOPI_65536 * (65536 / 4096)), ++i)
78 {
79 rad = (fx32)((idx + 0x80000LL) >> 20);
80 ss = FX_SinFx64c(rad);
81 s = (fx16)((ss + 0x80000LL) >> 20);
82
83 FX_SinCosTable_[i * 2] = s; // sin: 0 to pi/2
84 if (i != 0)
85 FX_SinCosTable_[SDK_FX_INIT_2PI - i * 2] = (fx16)-s; // sin: 0 to -pi/2
86 FX_SinCosTable_[SDK_FX_INIT_PI - i * 2] = s; // sin: pi/2 to pi
87 FX_SinCosTable_[SDK_FX_INIT_PI + i * 2] = (fx16)-s; // sin: -pi/2 to -pi
88 }
89
90 FX_SinCosTable_[1] = FX16_ONE; // cos(0)
91
92 for (i = 1; i <= SDK_FX_INIT_PI_2; ++i)
93 {
94 FX_SinCosTable_[i * 2 + 1] = FX_SinCosTable_[SDK_FX_INIT_PI_2 + i * 2]; // cos: cos(x) = sin(pi/2 + x)
95 FX_SinCosTable_[SDK_FX_INIT_2PI - i * 2 + 1] = FX_SinCosTable_[i * 2 + 1]; // cos: cos(x) = cos(-x)
96 }
97 #else
98 // Floating point version:
99 // The output is same to the static one.
100 // But, note that the code is slower and larger.
101
102 int i;
103 f32 rad;
104 fx16 s;
105 for (i = 0; i <= 4096 / 4; ++i)
106 {
107 rad = (f32)((3.14159265358979323846 * 2 / 4096) * i);
108 s = (fx16)FX_F32_TO_FX32(sinf(rad));
109 FX_SinCosTable_[i * 2] = s; // sin: 0 to pi/2
110 if (i != 0)
111 FX_SinCosTable_[SDK_FX_INIT_2PI - i * 2] = (fx16)-s; // sin: 0 to -pi/2
112 FX_SinCosTable_[SDK_FX_INIT_PI - i * 2] = s; // sin: pi/2 to pi
113 FX_SinCosTable_[SDK_FX_INIT_PI + i * 2] = (fx16)-s; // sin: -pi/2 to -pi
114 }
115
116 FX_SinCosTable_[1] = FX16_ONE; // cos(0)
117
118 for (i = 1; i <= SDK_FX_INIT_PI_2; ++i)
119 {
120 FX_SinCosTable_[i * 2 + 1] = FX_SinCosTable_[SDK_FX_INIT_PI_2 + i * 2]; // cos: cos(x) = sin(pi/2 + x)
121 FX_SinCosTable_[SDK_FX_INIT_2PI - i * 2 + 1] = FX_SinCosTable_[i * 2 + 1]; // cos: cos(x) = cos(-x)
122 }
123 #endif
124 #endif
125 }
126
127 #include <nitro/code32.h> // FX_MulFunc and FX_Mul32x64cFunc are always compiled into ARM binary.
128 /*---------------------------------------------------------------------------*
129 Name: FX_MulFunc
130
131 Description: Invoked from thumb code. This drops fractions.
132
133 Arguments: v1 fx32 value
134 v2 fx32 value
135
136 Returns: result in fx32 format
137 *---------------------------------------------------------------------------*/
FX_MulFunc(fx32 v1,fx32 v2)138 fx32 FX_MulFunc(fx32 v1, fx32 v2)
139 {
140 return FX32_CAST(((s64)(v1) * v2 + 0x800LL) >> FX32_SHIFT);
141 }
142
143
144 /*---------------------------------------------------------------------------*
145 Name: FX_Mul32x64cFunc
146
147 Description: Invoked from thumb code. This rounds up/down fractions.
148
149 Arguments: v32 fx32 value
150 v64c fx64c value
151
152 Returns: result in fx32 format
153 *---------------------------------------------------------------------------*/
FX_Mul32x64cFunc(fx32 v32,fx64c v64c)154 fx32 FX_Mul32x64cFunc(fx32 v32, fx64c v64c)
155 {
156 fx64c tmp = v64c * v32 + 0x80000000LL; // for better precision
157 return FX32_CAST(tmp >> FX64C_SHIFT);
158 }
159
160 #include <nitro/codereset.h>
161
162 /*---------------------------------------------------------------------------*
163 Name: FX_Modf
164
165 Description: Splits a fx32 number into integral part and fraction part.
166
167 Arguments: x input in fx32 format
168 iPtr a pointer to integral part in fx32 format
169
170 Returns: fraction part of x in fx32 format
171 *---------------------------------------------------------------------------*/
FX_Modf(fx32 x,fx32 * iPtr)172 fx32 FX_Modf(fx32 x, fx32 *iPtr)
173 {
174 if (x >= 0)
175 {
176 *iPtr = (x & FX32_INT_MASK);
177 return (x & FX32_DEC_MASK);
178 }
179 else
180 {
181 *iPtr = -((-x) & FX32_INT_MASK);
182 return -((-x) & FX32_DEC_MASK);
183 }
184 }
185