| /* |
| ** Copyright 2003-2010, VisualOn, Inc. |
| ** |
| ** Licensed under the Apache License, Version 2.0 (the "License"); |
| ** you may not use this file except in compliance with the License. |
| ** You may obtain a copy of the License at |
| ** |
| ** http://www.apache.org/licenses/LICENSE-2.0 |
| ** |
| ** Unless required by applicable law or agreed to in writing, software |
| ** distributed under the License is distributed on an "AS IS" BASIS, |
| ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| ** See the License for the specific language governing permissions and |
| ** limitations under the License. |
| */ |
| |
| /*___________________________________________________________________________ |
| | | |
| | This file contains mathematic operations in fixed point. | |
| | | |
| | Isqrt() : inverse square root (16 bits precision). | |
| | Pow2() : 2^x (16 bits precision). | |
| | Log2() : log2 (16 bits precision). | |
| | Dot_product() : scalar product of <x[],y[]> | |
| | | |
| | These operations are not standard double precision operations. | |
| | They are used where low complexity is important and the full 32 bits | |
| | precision is not necessary. For example, the function Div_32() has a | |
| | 24 bits precision which is enough for our purposes. | |
| | | |
| | In this file, the values use theses representations: | |
| | | |
| | Word32 L_32 : standard signed 32 bits format | |
| | Word16 hi, lo : L_32 = hi<<16 + lo<<1 (DPF - Double Precision Format) | |
| | Word32 frac, Word16 exp : L_32 = frac << exp-31 (normalised format) | |
| | Word16 int, frac : L_32 = int.frac (fractional format) | |
| |___________________________________________________________________________| |
| */ |
| #include "typedef.h" |
| #include "basic_op.h" |
| #include "math_op.h" |
| |
| /*___________________________________________________________________________ |
| | | |
| | Function Name : Isqrt | |
| | | |
| | Compute 1/sqrt(L_x). | |
| | if L_x is negative or zero, result is 1 (7fffffff). | |
| |---------------------------------------------------------------------------| |
| | Algorithm: | |
| | | |
| | 1- Normalization of L_x. | |
| | 2- call Isqrt_n(L_x, exponant) | |
| | 3- L_y = L_x << exponant | |
| |___________________________________________________________________________| |
| */ |
| Word32 Isqrt( /* (o) Q31 : output value (range: 0<=val<1) */ |
| Word32 L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */ |
| ) |
| { |
| Word16 exp; |
| Word32 L_y; |
| exp = norm_l(L_x); |
| L_x = (L_x << exp); /* L_x is normalized */ |
| exp = (31 - exp); |
| Isqrt_n(&L_x, &exp); |
| L_y = (L_x << exp); /* denormalization */ |
| return (L_y); |
| } |
| |
| /*___________________________________________________________________________ |
| | | |
| | Function Name : Isqrt_n | |
| | | |
| | Compute 1/sqrt(value). | |
| | if value is negative or zero, result is 1 (frac=7fffffff, exp=0). | |
| |---------------------------------------------------------------------------| |
| | Algorithm: | |
| | | |
| | The function 1/sqrt(value) is approximated by a table and linear | |
| | interpolation. | |
| | | |
| | 1- If exponant is odd then shift fraction right once. | |
| | 2- exponant = -((exponant-1)>>1) | |
| | 3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization. | |
| | 4- a = bit10-b24 | |
| | 5- i -=16 | |
| | 6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2 | |
| |___________________________________________________________________________| |
| */ |
| static Word16 table_isqrt[49] = |
| { |
| 32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214, |
| 25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155, |
| 21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539, |
| 19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674, |
| 17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384 |
| }; |
| |
| void Isqrt_n( |
| Word32 * frac, /* (i/o) Q31: normalized value (1.0 < frac <= 0.5) */ |
| Word16 * exp /* (i/o) : exponent (value = frac x 2^exponent) */ |
| ) |
| { |
| Word16 i, a, tmp; |
| |
| if (*frac <= (Word32) 0) |
| { |
| *exp = 0; |
| *frac = 0x7fffffffL; |
| return; |
| } |
| |
| if((*exp & 1) == 1) /*If exponant odd -> shift right */ |
| *frac = (*frac) >> 1; |
| |
| *exp = negate((*exp - 1) >> 1); |
| |
| *frac = (*frac >> 9); |
| i = extract_h(*frac); /* Extract b25-b31 */ |
| *frac = (*frac >> 1); |
| a = (Word16)(*frac); /* Extract b10-b24 */ |
| a = (Word16) (a & (Word16) 0x7fff); |
| i -= 16; |
| *frac = L_deposit_h(table_isqrt[i]); /* table[i] << 16 */ |
| tmp = vo_sub(table_isqrt[i], table_isqrt[i + 1]); /* table[i] - table[i+1]) */ |
| *frac = vo_L_msu(*frac, tmp, a); /* frac -= tmp*a*2 */ |
| |
| return; |
| } |
| |
| /*___________________________________________________________________________ |
| | | |
| | Function Name : Pow2() | |
| | | |
| | L_x = pow(2.0, exponant.fraction) (exponant = interger part) | |
| | = pow(2.0, 0.fraction) << exponant | |
| |---------------------------------------------------------------------------| |
| | Algorithm: | |
| | | |
| | The function Pow2(L_x) is approximated by a table and linear | |
| | interpolation. | |
| | | |
| | 1- i = bit10-b15 of fraction, 0 <= i <= 31 | |
| | 2- a = bit0-b9 of fraction | |
| | 3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2 | |
| | 4- L_x = L_x >> (30-exponant) (with rounding) | |
| |___________________________________________________________________________| |
| */ |
| static Word16 table_pow2[33] = |
| { |
| 16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911, |
| 20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726, |
| 25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706, |
| 31379, 32066, 32767 |
| }; |
| |
| Word32 Pow2( /* (o) Q0 : result (range: 0<=val<=0x7fffffff) */ |
| Word16 exponant, /* (i) Q0 : Integer part. (range: 0<=val<=30) */ |
| Word16 fraction /* (i) Q15 : Fractionnal part. (range: 0.0<=val<1.0) */ |
| ) |
| { |
| Word16 exp, i, a, tmp; |
| Word32 L_x; |
| |
| L_x = vo_L_mult(fraction, 32); /* L_x = fraction<<6 */ |
| i = extract_h(L_x); /* Extract b10-b16 of fraction */ |
| L_x =L_x >> 1; |
| a = (Word16)(L_x); /* Extract b0-b9 of fraction */ |
| a = (Word16) (a & (Word16) 0x7fff); |
| |
| L_x = L_deposit_h(table_pow2[i]); /* table[i] << 16 */ |
| tmp = vo_sub(table_pow2[i], table_pow2[i + 1]); /* table[i] - table[i+1] */ |
| L_x -= (tmp * a)<<1; /* L_x -= tmp*a*2 */ |
| |
| exp = vo_sub(30, exponant); |
| L_x = vo_L_shr_r(L_x, exp); |
| |
| return (L_x); |
| } |
| |
| /*___________________________________________________________________________ |
| | | |
| | Function Name : Dot_product12() | |
| | | |
| | Compute scalar product of <x[],y[]> using accumulator. | |
| | | |
| | The result is normalized (in Q31) with exponent (0..30). | |
| |---------------------------------------------------------------------------| |
| | Algorithm: | |
| | | |
| | dot_product = sum(x[i]*y[i]) i=0..N-1 | |
| |___________________________________________________________________________| |
| */ |
| |
| Word32 Dot_product12( /* (o) Q31: normalized result (1 < val <= -1) */ |
| Word16 x[], /* (i) 12bits: x vector */ |
| Word16 y[], /* (i) 12bits: y vector */ |
| Word16 lg, /* (i) : vector length */ |
| Word16 * exp /* (o) : exponent of result (0..+30) */ |
| ) |
| { |
| Word16 sft; |
| Word32 i, L_sum; |
| L_sum = 0; |
| for (i = 0; i < lg; i++) |
| { |
| Word32 tmp = (Word32) x[i] * (Word32) y[i]; |
| if (tmp == (Word32) 0x40000000L) { |
| tmp = MAX_32; |
| } |
| L_sum = L_add(L_sum, tmp); |
| } |
| L_sum = L_shl2(L_sum, 1); |
| L_sum = L_add(L_sum, 1); |
| /* Normalize acc in Q31 */ |
| sft = norm_l(L_sum); |
| L_sum = L_sum << sft; |
| *exp = 30 - sft; /* exponent = 0..30 */ |
| return (L_sum); |
| |
| } |
| |
| |
