/* | |
** 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++) | |
{ | |
L_sum += x[i] * y[i]; | |
} | |
L_sum = (L_sum << 1) + 1; | |
/* Normalize acc in Q31 */ | |
sft = norm_l(L_sum); | |
L_sum = L_sum << sft; | |
*exp = 30 - sft; /* exponent = 0..30 */ | |
return (L_sum); | |
} | |