ref: 5164b5505f256c06eb3a84f4d8a8a93055af9d41
dir: /amr-wb/math_op.c/
/*___________________________________________________________________________
| |
| 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"
#include "count.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_shl(L_x, exp); /* L_x is normalized */
exp = sub(31, exp);
Isqrt_n(&L_x, &exp);
L_y = L_shl(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;
test();
if (*frac <= (Word32) 0)
{
*exp = 0; move16();
*frac = 0x7fffffffL; move32();
return;
}
test();logic16();
if (sub((Word16) (*exp & 1), 1) == 0) /* If exponant odd -> shift right */
*frac = L_shr(*frac, 1);
*exp = negate(shr(sub(*exp, 1), 1)); move16();
*frac = L_shr(*frac, 9); move32();
i = extract_h(*frac); /* Extract b25-b31 */
*frac = L_shr(*frac, 1); move32();
a = extract_l(*frac); /* Extract b10-b24 */
a = (Word16) (a & (Word16) 0x7fff); logic16();
i = sub(i, 16);
move32();
*frac = L_deposit_h(table_isqrt[i]); /* table[i] << 16 */
tmp = sub(table_isqrt[i], table_isqrt[i + 1]); /* table[i] - table[i+1]) */
move32();
*frac = 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 = L_mult(fraction, 32); /* L_x = fraction<<6 */
i = extract_h(L_x); /* Extract b10-b16 of fraction */
L_x = L_shr(L_x, 1);
a = extract_l(L_x); /* Extract b0-b9 of fraction */
a = (Word16) (a & (Word16) 0x7fff); logic16();
L_x = L_deposit_h(table_pow2[i]); /* table[i] << 16 */
tmp = sub(table_pow2[i], table_pow2[i + 1]); /* table[i] - table[i+1] */
L_x = L_msu(L_x, tmp, a); /* L_x -= tmp*a*2 */
exp = sub(30, exponant);
L_x = 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 i, sft;
Word32 L_sum;
L_sum = 1L; move32();
for (i = 0; i < lg; i++)
L_sum = L_mac(L_sum, x[i], y[i]);
/* Normalize acc in Q31 */
sft = norm_l(L_sum);
L_sum = L_shl(L_sum, sft);
*exp = sub(30, sft); move16(); /* exponent = 0..30 */
return (L_sum);
}