1 /*---------------------------------------------------------------------------*
2   Project:  TwlSDK - FX -
3   File:     fx_trig.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 //-----------------------------------------------------------------------------
19 //  A Margin of Error:
20 //  the worst case: about 0.000000025 (sin(1.25pi), cos(1.75pi))
21 //  average       : about 0.000000001
22 //
23 //  |(sin(dx) - FX_SinFx64c(x) / 4294967296.0) / sin(dx)| < 0.00004 in the worst case(around cos(pi/2))
24 //-----------------------------------------------------------------------------
25 
26 #include <nitro/fx/fx_trig.h>
27 #include <nitro/fx/fx_const.h>
28 
29 #define SDK_FOUR_PI     ((fx64c) 0x0000000145f306ddLL)
30 
31 #define SDK_SINCOEFF_1  ((u64) 3373259426LL)    // pi/4
32 #define SDK_SINCOEFF_2  ((u64) 346799334LL)     // ((pi/4)^3)/6
33 #define SDK_SINCOEFF_3  ((u64) 132467588LL)     // ((pi./4)^2)/20
34 #define SDK_SINCOEFF_4  ((u64) 63079804LL)      // ((pi/4)^2)/42
35 #define SDK_SINCOEFF_5  ((u64) 36796552LL)      // ((pi/4)^2)/72
36 
37 #define SDK_COSCOEFF_1  ((u64) 1324675879LL)    // ((pi/4)^2)/2
38 #define SDK_COSCOEFF_2  ((u64) 220779313LL)     // ((pi/4)^2)/12
39 #define SDK_COSCOEFF_3  ((u64) 88311725LL)      // ((pi/4)^2)/30
40 #define SDK_COSCOEFF_4  ((u64) 47309853LL)      // ((pi/4)^2)/56
41 
42 static u64 FX_SinFx64c_internal(u64 y);
43 static u64 FX_CosFx64c_internal(u64 y);
44 
45 #include <nitro/code32.h>              // Always generate ARM binary for efficiency
46 // calc: rad - (rad^3)/3! + (rad^5)/5! - (rad^7)/7! + (rad^9)/9!
FX_SinFx64c_internal(u64 y)47 static u64 FX_SinFx64c_internal(u64 y)
48 {
49     u64     tmp;
50     u64     yy;
51     if (y == 0x100000000LL)
52     {
53         return (u64)FX64C_SQRT1_2;
54     }
55     yy = y * y >> 32;
56 
57     tmp = FX64C_ONE - (SDK_SINCOEFF_5 * yy >> 32);
58     tmp = FX64C_ONE - ((SDK_SINCOEFF_4 * yy >> 32) * tmp >> 32);
59     tmp = FX64C_ONE - ((SDK_SINCOEFF_3 * yy >> 32) * tmp >> 32);
60     tmp = SDK_SINCOEFF_1 - ((SDK_SINCOEFF_2 * yy >> 32) * tmp >> 32);
61 
62     return tmp * y >> 32;
63 }
64 
65 // calc: 1 - (rad^2)/2! + (rad^4)/4! - (rad^6)/6! + (rad^8)/8!
FX_CosFx64c_internal(u64 y)66 static u64 FX_CosFx64c_internal(u64 y)
67 {
68     u64     tmp;
69     u64     yy;
70     if (y == 0x100000000LL)
71     {
72         return (u64)FX64C_SQRT1_2;
73     }
74     yy = y * y >> 32;
75 
76     tmp = FX64C_ONE - (SDK_COSCOEFF_4 * yy >> 32);
77     tmp = FX64C_ONE - ((SDK_COSCOEFF_3 * yy >> 32) * tmp >> 32);
78     tmp = FX64C_ONE - ((SDK_COSCOEFF_2 * yy >> 32) * tmp >> 32);
79     tmp = FX64C_ONE - ((SDK_COSCOEFF_1 * yy >> 32) * tmp >> 32);
80 
81     return tmp;
82 }
83 
84 #include <nitro/codereset.h>
85 
86 /*---------------------------------------------------------------------------*
87   Name:         FX_SinFx64c
88 
89   Description:  Returns sine of 'rad', computing a Taylor series expansion
90                 of it.
91 
92   Arguments:    rad          a value in fx32 format
93 
94   Returns:      sine of 'rad' in fx64c format
95  *---------------------------------------------------------------------------*/
FX_SinFx64c(fx32 rad)96 fx64c FX_SinFx64c(fx32 rad)
97 {
98     fx64c   y;
99     fx64c   rval;
100     int     n;
101 
102     if (rad < 0)
103     {
104         return -FX_SinFx64c(-rad);
105     }
106     y = (fx64c)((SDK_FOUR_PI * rad) >> 12);
107     n = (int)(y >> 32);
108     y = y & 0xffffffff;
109 
110     if (n & 1)
111     {
112         y = 0x100000000LL - y;
113     }
114     if ((n + 1) & 2)
115     {
116         rval = (fx64c)FX_CosFx64c_internal((u64)y);
117     }
118     else
119     {
120         rval = (fx64c)FX_SinFx64c_internal((u64)y);
121     }
122     if ((n & 7) > 3)
123     {
124         rval = -rval;
125     }
126     return rval;
127 }
128 
129 
130 /*---------------------------------------------------------------------------*
131   Name:         FX_CosFx64c
132 
133   Description:  Returns cosine of 'rad', computing a Taylor series expansion
134                 of it.
135 
136   Arguments:    rad          a value in fx32 format
137 
138   Returns:      cosine of 'rad' in fx64c format
139  *---------------------------------------------------------------------------*/
FX_CosFx64c(fx32 rad)140 fx64c FX_CosFx64c(fx32 rad)
141 {
142     fx64c   y;
143     fx64c   rval;
144     int     n;
145 
146     if (rad < 0)
147     {
148         return FX_CosFx64c(-rad);
149     }
150     y = (fx64c)((SDK_FOUR_PI * rad) >> 12);
151     n = (int)(y >> 32);
152     y = y & 0xffffffff;
153 
154     if (n & 1)
155     {
156         y = 0x100000000LL - y;
157     }
158     if ((n + 1) & 2)
159     {
160         rval = (fx64c)FX_SinFx64c_internal((u64)y);
161     }
162     else
163     {
164         rval = (fx64c)FX_CosFx64c_internal((u64)y);
165     }
166     if (((n + 2) & 7) > 3)
167     {
168         rval = -rval;
169     }
170     return rval;
171 }
172