auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_BasicEm / Phase.c
1 /*
2  * Copyright (C) 2008 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 /* ---- includes ----------------------------------------------------------- */
18
19 #include "b_BasicEm/Phase.h"
20 #include "b_BasicEm/Math.h"
21
22 /* ------------------------------------------------------------------------- */
23
24 /* ========================================================================= */
25 /*                                                                           */
26 /* ---- \ghd{ auxiliary functions } ---------------------------------------- */
27 /*                                                                           */
28 /* ========================================================================= */
29
30 /* ------------------------------------------------------------------------- */
31
32 /* ========================================================================= */
33 /*                                                                           */
34 /* ---- \ghd{ constructor / destructor } ----------------------------------- */
35 /*                                                                           */
36 /* ========================================================================= */
37
38 /* ------------------------------------------------------------------------- */
39 /* ========================================================================= */
40 /*                                                                           */
41 /* ---- \ghd{ operators } -------------------------------------------------- */
42 /*                                                                           */
43 /* ========================================================================= */
44
45 /* ------------------------------------------------------------------------- */
46
47 /* ========================================================================= */
48 /*                                                                           */
49 /* ---- \ghd{ query functions } -------------------------------------------- */
50 /*                                                                           */
51 /* ========================================================================= */
52
53 /* ------------------------------------------------------------------------- */
54
55 /* ========================================================================= */
56 /*                                                                           */
57 /* ---- \ghd{ modify functions } ------------------------------------------- */
58 /*                                                                           */
59 /* ========================================================================= */
60
61 /* ------------------------------------------------------------------------- */
62         
63 /* ========================================================================= */
64 /*                                                                           */
65 /* ---- \ghd{ I/O } -------------------------------------------------------- */
66 /*                                                                           */
67 /* ========================================================================= */
68
69 /* ------------------------------------------------------------------------- */
70         
71 /* ========================================================================= */
72 /*                                                                           */
73 /* ---- \ghd{ exec functions } --------------------------------------------- */
74 /*                                                                           */
75 /* ========================================================================= */
76
77 /* ------------------------------------------------------------------------- */
78
79 /* ------------------------------------------------------------------------- */
80
81 #ifndef bbs_SIN_INTERPOLATION_METHOD_2
82 const int32 bbs_sin32_table1G[] = 
83 {
84         0,                      1608,   411648,         1607,   823040,         1606,   1234176,        1602, 
85         1644288,        1599,   2053632,        1594,   2461696,        1588,   2868224,        1581, 
86         3272960,        1574,   3675904,        1564,   4076288,        1556,   4474624,        1545, 
87         4870144,        1533,   5262592,        1521,   5651968,        1508,   6038016,        1493, 
88         6420224,        1478,   6798592,        1463,   7173120,        1445,   7543040,        1428, 
89         7908608,        1409,   8269312,        1390,   8625152,        1369,   8975616,        1348, 
90         9320704,        1327,   9660416,        1303,   9993984,        1280,   10321664,       1256, 
91         10643200,       1231,   10958336,       1205,   11266816,       1178,   11568384,       1151, 
92         11863040,       1124,   12150784,       1094,   12430848,       1066,   12703744,       1036, 
93         12968960,       1005,   13226240,       974,    13475584,       942,    13716736,       910, 
94         13949696,       877,    14174208,       844,    14390272,       810,    14597632,       775, 
95         14796032,       741,    14985728,       705,    15166208,       670,    15337728,       634, 
96         15500032,       597,    15652864,       561,    15796480,       523,    15930368,       486, 
97         16054784,       448,    16169472,       409,    16274176,       372,    16369408,       333, 
98         16454656,       295,    16530176,       255,    16595456,       217,    16651008,       177, 
99         16696320,       138,    16731648,       99,             16756992,       59,             16772096,       20, 
100         16777216,       -20,    16772096,       -59,    16756992,       -99,    16731648,       -138, 
101         16696320,       -177,   16651008,       -217,   16595456,       -255,   16530176,       -295, 
102         16454656,       -333,   16369408,       -372,   16274176,       -409,   16169472,       -448, 
103         16054784,       -486,   15930368,       -523,   15796480,       -561,   15652864,       -597, 
104         15500032,       -634,   15337728,       -670,   15166208,       -705,   14985728,       -741, 
105         14796032,       -775,   14597632,       -810,   14390272,       -844,   14174208,       -877, 
106         13949696,       -910,   13716736,       -942,   13475584,       -974,   13226240,       -1005, 
107         12968960,       -1036,  12703744,       -1066,  12430848,       -1094,  12150784,       -1124, 
108         11863040,       -1151,  11568384,       -1178,  11266816,       -1205,  10958336,       -1231,
109         10643200,       -1256,  10321664,       -1280,  9993984,        -1303,  9660416,        -1327, 
110         9320704,        -1348,  8975616,        -1369,  8625152,        -1390,  8269312,        -1409, 
111         7908608,        -1428,  7543040,        -1445,  7173120,        -1463,  6798592,        -1478, 
112         6420224,        -1493,  6038016,        -1508,  5651968,        -1521,  5262592,        -1533, 
113         4870144,        -1545,  4474624,        -1556,  4076288,        -1564,  3675904,        -1574, 
114         3272960,        -1581,  2868224,        -1588,  2461696,        -1594,  2053632,        -1599, 
115         1644288,        -1602,  1234176,        -1606,  823040,         -1607,  411648,         -1608
116 };
117 #else
118 const int32 bbs_sin32_table2G[] = 
119 {
120         0,                      12907,  -122, 
121         209469440,      12662,  -368, 
122         410894336,      11926,  -596, 
123         596525056,      10733,  -802, 
124         759234560,      9129,   -978, 
125         892780544,      7168,   -1112, 
126         992002048,      4939,   -1210, 
127         1053097984, 2516,       -1256, 
128         1073741824, -4,         -1256, 
129         1053097984, -2519,      -1210, 
130         992002048,      -4944,  -1112, 
131         892780544,      -7173,  -978, 
132         759234560,      -9129,  -802, 
133         596525056,      -10734, -596, 
134         410894336,      -11926, -368, 
135         209469440,      -12663, -122
136 };
137 #endif
138
139 int32 bbs_sin32( phase16 phaseA )
140 {
141 #ifndef bbs_SIN_INTERPOLATION_METHOD_2
142
143         int32 oL = ( phaseA & 0x00FF );
144         uint16  indexL = ( ( phaseA & 0x7F00 ) >> 8 ) << 1;
145         int32 sinL = bbs_sin32_table1G[ indexL ] + oL * bbs_sin32_table1G[ indexL + 1 ];
146
147         if( ( phaseA & 0x8000 ) != 0 )
148         {
149                 return -sinL;
150         }
151         else
152         {
153                 return sinL;
154         }
155
156 #else /*bbs_SIN_INTERPOLATION_METHOD_2*/
157
158         int32 o1L = ( phaseA & 0x07FF );
159         int32 o2L = ( o1L * o1L ) >> 8;
160         uint16 indexL = ( ( phaseA & 0x7800 ) >> 11 ) * 3;
161         int32 sinL = bbs_sin32_table2G[ indexL ] + ( ( o1L * bbs_sin32_table2G[ indexL + 1 ] )  << 3 ) + o2L * bbs_sin32_table2G[ indexL + 2 ];
162
163         if( ( phaseA & 0x8000 ) != 0 )
164         {
165                 return -sinL >> 6;
166         }
167         else
168         {
169                 return sinL >> 6;
170         }
171
172 #endif /*bbs_SIN_INTERPOLATION_METHOD_2*/
173 }
174
175 /** computation of sine tables (do not uncomment or remove)
176 void sin1Table()
177 {
178         long iL;
179         for( iL = 0; iL < 128; iL++ )
180         {
181                 int32 phase1L = iL * 256;
182                 int32 phase2L = phase1L + 256;
183                 double angle1L = ( M_PI * phase1L ) / 32768;
184                 double angle2L = ( M_PI * phase2L ) / 32768;
185                 int32 sin1L = ( sin( angle1L ) * 65536 );
186                 int32 sin2L = ( sin( angle2L ) * 65536 );
187                 int32 diffL = sin2L - sin1L;
188                 eout << iL << ": " << ( sin1L << 8 ) << " + oL * " << diffL << endl;
189         }
190 }
191
192 void sin2Table()
193 {
194         long iL;
195         for( iL = 0; iL < 16; iL++ )
196         {
197                 int32 p0L = iL  * ( 1 << 11 );
198                 int32 p1L = p0L + ( 1 << 10 );
199                 int32 p2L = p0L + ( 1 << 11 );
200
201                 double a0L = ( M_PI * p0L ) / ( 1 << 15 );
202                 double a1L = ( M_PI * p1L ) / ( 1 << 15 );
203                 double a2L = ( M_PI * p2L ) / ( 1 << 15 );
204
205                 int32 s0L = ( sin( a0L ) * ( 1 << 16 ) );
206                 int32 s1L = ( sin( a1L ) * ( 1 << 16 ) );
207                 int32 s2L = ( sin( a2L ) * ( 1 << 16 ) );
208
209                 int32 aL = 4 * s1L - 3 * s0L - s2L;
210                 int32 bL = 2 * s2L + 2 * s0L - 4 * s1L;
211
212                 eout << iL << ": " << ( s0L << 14 ) << " + ( ( o1L * " << aL << " ) << 3 )"
213                          << " + o2L * " << bL << endl;
214         }
215 }
216 */
217
218 /* ------------------------------------------------------------------------- */
219
220 int32 bbs_cos32( phase16 phaseA )
221 {
222         return bbs_sin32( ( phase16 )( phaseA + bbs_M_PI_2_16 ) );
223 }
224
225 /* ------------------------------------------------------------------------- */
226
227 int16 bbs_sin16( phase16 phaseA )
228 {
229         return bbs_sin32( phaseA ) >> 10;
230 }
231
232 /* ------------------------------------------------------------------------- */
233
234 int16 bbs_cos16( phase16 phaseA )
235 {
236         return bbs_sin32( ( phase16 )( phaseA + bbs_M_PI_2_16 ) ) >> 10;
237 }
238
239 /* ------------------------------------------------------------------------- */
240
241 const int32 bbs_atan16_tableG[] =
242 {
243         0,                      325,    332800,         326,    666624,         326,    1000448,        325, 
244         1333248,        324,    1665024,        323,    1995776,        323,    2326528,        322, 
245         2656256,        320,    2983936,        319,    3310592,        317,    3635200,        316, 
246         3958784,        314,    4280320,        312,    4599808,        310,    4917248,        308, 
247         5232640,        306,    5545984,        303,    5856256,        301,    6164480,        298, 
248         6469632,        296,    6772736,        292,    7071744,        291,    7369728,        287, 
249         7663616,        284,    7954432,        281,    8242176,        279,    8527872,        275, 
250         8809472,        272,    9088000,        269,    9363456,        265,    9634816,        263, 
251         9904128,        259,    10169344,       256,    10431488,       252,    10689536,       249, 
252         10944512,       246,    11196416,       243,    11445248,       239,    11689984,       236, 
253         11931648,       233,    12170240,       230,    12405760,       226,    12637184,       223, 
254         12865536,       219,    13089792,       217,    13312000,       213,    13530112,       210, 
255         13745152,       207,    13957120,       204,    14166016,       201,    14371840,       198, 
256         14574592,       195,    14774272,       192,    14970880,       189,    15164416,       186, 
257         15354880,       183,    15542272,       180,    15726592,       178,    15908864,       175, 
258         16088064,       172,    16264192,       169,    16437248,       167,    16608256,       165
259 };
260
261 phase16 bbs_atan16( uint32 valA )
262 {
263         uint32 oL = valA & 0x03FF;
264         uint16 indexL = ( valA >> 10 ) << 1;
265         uint32 phaseL = bbs_atan16_tableG[ indexL ] + oL * bbs_atan16_tableG[ indexL + 1 ];
266         return ( phase16 )( phaseL >> 11 );
267 }
268
269 /* ------------------------------------------------------------------------- */
270
271 phase16 bbs_phase16( int32 xA, int32 yA )
272 {
273         uint32 xL = ( xA > 0 ) ? xA : -xA;
274         uint32 yL = ( yA > 0 ) ? yA : -yA;
275         phase16 phaseL;
276
277         if( xL == 0 && yL == 0 ) return 0;
278
279         if( xL == yL )
280         {
281                 phaseL = bbs_M_PI_4_16; /*PI/4*/
282         }
283         else if( xL > yL )
284         {
285                 if( yL >= 65536 ) /* avoid overflow (1 << 16) */
286                 {
287                         uint32 shiftL = bbs_intLog2( yL ) - 15;
288                         xL >>= shiftL;
289                         yL >>= shiftL;
290                 }
291                 phaseL = bbs_atan16( ( yL << 16 ) / xL );
292         }
293         else
294         {
295                 if( xL >= 65536 ) /* avoid overflow (1 << 16) */
296                 {
297                         uint32 shiftL = bbs_intLog2( xL ) - 15;
298                         xL >>= shiftL;
299                         yL >>= shiftL;
300                 }
301                 phaseL = bbs_M_PI_2_16 - bbs_atan16( ( xL << 16 ) / yL );
302         }
303
304         if( xA >= 0 )
305         {
306                 if( yA >= 0 )
307                 {
308                         return phaseL;
309                 }
310                 else
311                 {
312                         return -phaseL;
313                 }
314         }
315         else
316         {
317                 if( yA >= 0 )
318                 {
319                         return bbs_M_PI_16 - phaseL;
320                 }
321                 else
322                 {
323                         return phaseL - bbs_M_PI_16;
324                 }
325         }
326 }
327
328 /* ------------------------------------------------------------------------- */
329
330 /* ========================================================================= */
331
332