xavi
/
libfixmath
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added some deg/rad conversion functions. 

Fixed up some code style inconsistencies.
Added a modulo function to allow optimization on 8-bit machines.
This commit is contained in:
Flatmush 2012-07-25 09:54:58 +00:00
parent c2a48b8910
commit d9944df6d2
4 changed files with 497 additions and 446 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

734
libfixmath/fix16.c
View File

@ -8,51 +8,51 @@
#ifndef FIXMATH_NO_OVERFLOW
fix16_t fix16_add(fix16_t a, fix16_t b)
{
// Use unsigned integers because overflow with signed integers is
// an undefined operation (http://www.airs.com/blog/archives/120).
uint32_t _a = a, _b = b;
uint32_t sum = _a + _b;
// Use unsigned integers because overflow with signed integers is
// an undefined operation (http://www.airs.com/blog/archives/120).
uint32_t _a = a, _b = b;
uint32_t sum = _a + _b;
// Overflow can only happen if sign of a == sign of b, and then
// it causes sign of sum != sign of a.
if (!((_a ^ _b) & 0x80000000) && ((_a ^ sum) & 0x80000000))
return fix16_overflow;
return sum;
// Overflow can only happen if sign of a == sign of b, and then
// it causes sign of sum != sign of a.
if (!((_a ^ _b) & 0x80000000) && ((_a ^ sum) & 0x80000000))
return fix16_overflow;
return sum;
}
fix16_t fix16_sub(fix16_t a, fix16_t b)
{
uint32_t _a = a, _b = b;
uint32_t diff = _a - _b;
uint32_t _a = a, _b = b;
uint32_t diff = _a - _b;
// Overflow can only happen if sign of a != sign of b, and then
// it causes sign of diff != sign of a.
if (((_a ^ _b) & 0x80000000) && ((_a ^ diff) & 0x80000000))
return fix16_overflow;
return diff;
// Overflow can only happen if sign of a != sign of b, and then
// it causes sign of diff != sign of a.
if (((_a ^ _b) & 0x80000000) && ((_a ^ diff) & 0x80000000))
return fix16_overflow;
return diff;
}
/* Saturating arithmetic */
fix16_t fix16_sadd(fix16_t a, fix16_t b)
{
fix16_t result = fix16_add(a, b);
fix16_t result = fix16_add(a, b);
if (result == fix16_overflow)
return (a > 0) ? fix16_max : fix16_min;
if (result == fix16_overflow)
return (a > 0) ? fix16_max : fix16_min;
return result;
}
return result;
}
fix16_t fix16_ssub(fix16_t a, fix16_t b)
{
fix16_t result = fix16_sub(a, b);
fix16_t result = fix16_sub(a, b);
if (result == fix16_overflow)
return (a > 0) ? fix16_max : fix16_min;
if (result == fix16_overflow)
return (a > 0) ? fix16_max : fix16_min;
return result;
return result;
}
#endif
@ -67,41 +67,41 @@ fix16_t fix16_ssub(fix16_t a, fix16_t b)
#if !defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT)
fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
{
int64_t product = (int64_t)inArg0 * inArg1;
#ifndef FIXMATH_NO_OVERFLOW
// The upper 17 bits should all be the same (the sign).
uint32_t upper = (product >> 47);
#endif
if (product < 0)
{
#ifndef FIXMATH_NO_OVERFLOW
if (~upper)
return fix16_overflow;
#endif
#ifndef FIXMATH_NO_ROUNDING
// This adjustment is required in order to round -1/2 correctly
product--;
#endif
}
else
{
#ifndef FIXMATH_NO_OVERFLOW
if (upper)
return fix16_overflow;
#endif
}
#ifdef FIXMATH_NO_ROUNDING
return product >> 16;
#else
fix16_t result = product >> 16;
result += (product & 0x8000) >> 15;
return result;
#endif
int64_t product = (int64_t)inArg0 * inArg1;
#ifndef FIXMATH_NO_OVERFLOW
// The upper 17 bits should all be the same (the sign).
uint32_t upper = (product >> 47);
#endif
if (product < 0)
{
#ifndef FIXMATH_NO_OVERFLOW
if (~upper)
return fix16_overflow;
#endif
#ifndef FIXMATH_NO_ROUNDING
// This adjustment is required in order to round -1/2 correctly
product--;
#endif
}
else
{
#ifndef FIXMATH_NO_OVERFLOW
if (upper)
return fix16_overflow;
#endif
}
#ifdef FIXMATH_NO_ROUNDING
return product >> 16;
#else
fix16_t result = product >> 16;
result += (product & 0x8000) >> 15;
return result;
#endif
}
#endif
@ -112,56 +112,56 @@ fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
#if defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT)
fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
{
// Each argument is divided to 16-bit parts.
// AB
// * CD
// -----------
// BD 16 * 16 -> 32 bit products
// CB
// AD
// AC
// |----| 64 bit product
int32_t A = (inArg0 >> 16), C = (inArg1 >> 16);
uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF);
int32_t AC = A*C;
int32_t AD_CB = A*D + C*B;
uint32_t BD = B*D;
int32_t product_hi = AC + (AD_CB >> 16);
// Handle carry from lower 32 bits to upper part of result.
uint32_t ad_cb_temp = AD_CB << 16;
uint32_t product_lo = BD + ad_cb_temp;
if (product_lo < BD)
product_hi++;
// Each argument is divided to 16-bit parts.
// AB
// * CD
// -----------
// BD 16 * 16 -> 32 bit products
// CB
// AD
// AC
// |----| 64 bit product
int32_t A = (inArg0 >> 16), C = (inArg1 >> 16);
uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF);
int32_t AC = A*C;
int32_t AD_CB = A*D + C*B;
uint32_t BD = B*D;
int32_t product_hi = AC + (AD_CB >> 16);
// Handle carry from lower 32 bits to upper part of result.
uint32_t ad_cb_temp = AD_CB << 16;
uint32_t product_lo = BD + ad_cb_temp;
if (product_lo < BD)
product_hi++;
#ifndef FIXMATH_NO_OVERFLOW
// The upper 17 bits should all be the same (the sign).
if (product_hi >> 31 != product_hi >> 15)
return fix16_overflow;
// The upper 17 bits should all be the same (the sign).
if (product_hi >> 31 != product_hi >> 15)
return fix16_overflow;
#endif
#ifdef FIXMATH_NO_ROUNDING
return (product_hi << 16) | (product_lo >> 16);
return (product_hi << 16) | (product_lo >> 16);
#else
// Subtracting 0x8000 (= 0.5) and then using signed right shift
// achieves proper rounding to result-1, except in the corner
// case of negative numbers and lowest word = 0x8000.
// To handle that, we also have to subtract 1 for negative numbers.
uint32_t product_lo_tmp = product_lo;
product_lo -= 0x8000;
product_lo -= (uint32_t)product_hi >> 31;
if (product_lo > product_lo_tmp)
product_hi--;
// Discard the lowest 16 bits. Note that this is not exactly the same
// as dividing by 0x10000. For example if product = -1, result will
// also be -1 and not 0. This is compensated by adding +1 to the result
// and compensating this in turn in the rounding above.
fix16_t result = (product_hi << 16) | (product_lo >> 16);
result += 1;
return result;
// Subtracting 0x8000 (= 0.5) and then using signed right shift
// achieves proper rounding to result-1, except in the corner
// case of negative numbers and lowest word = 0x8000.
// To handle that, we also have to subtract 1 for negative numbers.
uint32_t product_lo_tmp = product_lo;
product_lo -= 0x8000;
product_lo -= (uint32_t)product_hi >> 31;
if (product_lo > product_lo_tmp)
product_hi--;
// Discard the lowest 16 bits. Note that this is not exactly the same
// as dividing by 0x10000. For example if product = -1, result will
// also be -1 and not 0. This is compensated by adding +1 to the result
// and compensating this in turn in the rounding above.
fix16_t result = (product_hi << 16) | (product_lo >> 16);
result += 1;
return result;
#endif
}
#endif
@ -173,97 +173,98 @@ fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
#if defined(FIXMATH_OPTIMIZE_8BIT)
fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
{
uint32_t _a = (inArg0 >= 0) ? inArg0 : (-inArg0);
uint32_t _b = (inArg1 >= 0) ? inArg1 : (-inArg1);
uint8_t va[4] = {_a, (_a >> 8), (_a >> 16), (_a >> 24)};
uint8_t vb[4] = {_b, (_b >> 8), (_b >> 16), (_b >> 24)};
uint32_t low = 0;
uint32_t mid = 0;
// Result column i depends on va[0..i] and vb[i..0]
uint32_t _a = (inArg0 >= 0) ? inArg0 : (-inArg0);
uint32_t _b = (inArg1 >= 0) ? inArg1 : (-inArg1);
uint8_t va[4] = {_a, (_a >> 8), (_a >> 16), (_a >> 24)};
uint8_t vb[4] = {_b, (_b >> 8), (_b >> 16), (_b >> 24)};
uint32_t low = 0;
uint32_t mid = 0;
// Result column i depends on va[0..i] and vb[i..0]
#ifndef FIXMATH_NO_OVERFLOW
// i = 6
if (va[3] && vb[3]) return fix16_overflow;
#endif
// i = 5
if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3];
if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2];
mid <<= 8;
// i = 4
if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3];
if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2];
if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1];
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0xFF000000) return fix16_overflow;
#endif
mid <<= 8;
// i = 3
if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3];
if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2];
if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1];
if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0];
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0xFF000000) return fix16_overflow;
#endif
mid <<= 8;
// i = 2
if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2];
if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1];
if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0];
// i = 1
if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1];
if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0];
low <<= 8;
// i = 0
if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0];
#ifndef FIXMATH_NO_ROUNDING
low += 0x8000;
#endif
mid += (low >> 16);
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0x80000000)
return fix16_overflow;
#endif
fix16_t result = mid;
/* Figure out the sign of result */
if ((inArg0 >= 0) != (inArg1 >= 0))
{
result = -result;
}
return result;
#ifndef FIXMATH_NO_OVERFLOW
// i = 6
if (va[3] && vb[3]) return fix16_overflow;
#endif
// i = 5
if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3];
if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2];
mid <<= 8;
// i = 4
if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3];
if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2];
if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1];
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0xFF000000) return fix16_overflow;
#endif
mid <<= 8;
// i = 3
if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3];
if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2];
if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1];
if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0];
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0xFF000000) return fix16_overflow;
#endif
mid <<= 8;
// i = 2
if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2];
if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1];
if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0];
// i = 1
if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1];
if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0];
low <<= 8;
// i = 0
if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0];
#ifndef FIXMATH_NO_ROUNDING
low += 0x8000;
#endif
mid += (low >> 16);
#ifndef FIXMATH_NO_OVERFLOW
if (mid & 0x80000000)
return fix16_overflow;
#endif
fix16_t result = mid;
/* Figure out the sign of result */
if ((inArg0 >= 0) != (inArg1 >= 0))
{
result = -result;
}
return result;
}
#endif
#ifndef FIXMATH_NO_OVERFLOW
/* Wrapper around fix16_mul to add saturating arithmetic. */
fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) {
fix16_t result = fix16_mul(inArg0, inArg1);
if (result == fix16_overflow)
{
if ((inArg0 >= 0) == (inArg1 >= 0))
return fix16_max;
else
return fix16_min;
}
return result;
fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1)
{
fix16_t result = fix16_mul(inArg0, inArg1);
if (result == fix16_overflow)
{
if ((inArg0 >= 0) == (inArg1 >= 0))
return fix16_max;
else
return fix16_min;
}
return result;
}
#endif
@ -278,84 +279,84 @@ fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) {
#else
static uint8_t clz(uint32_t x)
{
uint8_t result = 0;
if (x == 0) return 32;
while (!(x & 0xF0000000)) { result += 4; x <<= 4; }
while (!(x & 0x80000000)) { result += 1; x <<= 1; }
return result;
uint8_t result = 0;
if (x == 0) return 32;
while (!(x & 0xF0000000)) { result += 4; x <<= 4; }
while (!(x & 0x80000000)) { result += 1; x <<= 1; }
return result;
}
#endif
fix16_t fix16_div(fix16_t a, fix16_t b)
{
// This uses a hardware 32/32 bit division multiple times, until we have
// computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations.
if (b == 0)
return fix16_min;
uint32_t remainder = (a >= 0) ? a : (-a);
uint32_t divider = (b >= 0) ? b : (-b);
uint32_t quotient = 0;
int bit_pos = 17;
// Kick-start the division a bit.
// This improves speed in the worst-case scenarios where N and D are large
// It gets a lower estimate for the result by N/(D >> 17 + 1).
if (divider & 0xFFF00000)
{
uint32_t shifted_div = ((divider >> 17) + 1);
quotient = remainder / shifted_div;
remainder -= ((uint64_t)quotient * divider) >> 17;
}
// If the divider is divisible by 2^n, take advantage of it.
while (!(divider & 0xF) && bit_pos >= 4)
{
divider >>= 4;
bit_pos -= 4;
}
while (remainder && bit_pos >= 0)
{
// Shift remainder as much as we can without overflowing
int shift = clz(remainder);
if (shift > bit_pos) shift = bit_pos;
remainder <<= shift;
bit_pos -= shift;
uint32_t div = remainder / divider;
remainder = remainder % divider;
quotient += div << bit_pos;
// This uses a hardware 32/32 bit division multiple times, until we have
// computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations.
if (b == 0)
return fix16_min;
uint32_t remainder = (a >= 0) ? a : (-a);
uint32_t divider = (b >= 0) ? b : (-b);
uint32_t quotient = 0;
int bit_pos = 17;
// Kick-start the division a bit.
// This improves speed in the worst-case scenarios where N and D are large
// It gets a lower estimate for the result by N/(D >> 17 + 1).
if (divider & 0xFFF00000)
{
uint32_t shifted_div = ((divider >> 17) + 1);
quotient = remainder / shifted_div;
remainder -= ((uint64_t)quotient * divider) >> 17;
}
// If the divider is divisible by 2^n, take advantage of it.
while (!(divider & 0xF) && bit_pos >= 4)
{
divider >>= 4;
bit_pos -= 4;
}
while (remainder && bit_pos >= 0)
{
// Shift remainder as much as we can without overflowing
int shift = clz(remainder);
if (shift > bit_pos) shift = bit_pos;
remainder <<= shift;
bit_pos -= shift;
uint32_t div = remainder / divider;
remainder = remainder % divider;
quotient += div << bit_pos;
#ifndef FIXMATH_NO_OVERFLOW
if (div & ~(0xFFFFFFFF >> bit_pos))
return fix16_overflow;
#endif
remainder <<= 1;
bit_pos--;
}
#ifndef FIXMATH_NO_ROUNDING
// Quotient is always positive so rounding is easy
quotient++;
#endif
fix16_t result = quotient >> 1;
// Figure out the sign of the result
if ((a ^ b) & 0x80000000)
{
#ifndef FIXMATH_NO_OVERFLOW
if (result == fix16_min)
return fix16_overflow;
#endif
result = -result;
}
return result;
#ifndef FIXMATH_NO_OVERFLOW
if (div & ~(0xFFFFFFFF >> bit_pos))
return fix16_overflow;
#endif
remainder <<= 1;
bit_pos--;
}
#ifndef FIXMATH_NO_ROUNDING
// Quotient is always positive so rounding is easy
quotient++;
#endif
fix16_t result = quotient >> 1;
// Figure out the sign of the result
if ((a ^ b) & 0x80000000)
{
#ifndef FIXMATH_NO_OVERFLOW
if (result == fix16_min)
return fix16_overflow;
#endif
result = -result;
}
return result;
}
#endif
@ -366,107 +367,131 @@ fix16_t fix16_div(fix16_t a, fix16_t b)
#if defined(FIXMATH_OPTIMIZE_8BIT)
fix16_t fix16_div(fix16_t a, fix16_t b)
{
// This uses the basic binary restoring division algorithm.
// It appears to be faster to do the whole division manually than
// trying to compose a 64-bit divide out of 32-bit divisions on
// platforms without hardware divide.
if (b == 0)
return fix16_min;
uint32_t remainder = (a >= 0) ? a : (-a);
uint32_t divider = (b >= 0) ? b : (-b);
// This uses the basic binary restoring division algorithm.
// It appears to be faster to do the whole division manually than
// trying to compose a 64-bit divide out of 32-bit divisions on
// platforms without hardware divide.
if (b == 0)
return fix16_min;
uint32_t remainder = (a >= 0) ? a : (-a);
uint32_t divider = (b >= 0) ? b : (-b);
uint32_t quotient = 0;
uint32_t bit = 0x10000;
/* The algorithm requires D >= R */
while (divider < remainder)
{
divider <<= 1;
bit <<= 1;
}
#ifndef FIXMATH_NO_OVERFLOW
if (!bit)
return fix16_overflow;
#endif
if (divider & 0x80000000)
{
// Perform one step manually to avoid overflows later.
// We know that divider's bottom bit is 0 here.
if (remainder >= divider)
{
quotient |= bit;
remainder -= divider;
}
divider >>= 1;
bit >>= 1;
}
/* Main division loop */
while (bit && remainder)
{
if (remainder >= divider)
{
quotient |= bit;
remainder -= divider;
}
remainder <<= 1;
bit >>= 1;
}
#ifndef FIXMATH_NO_ROUNDING
if (remainder >= divider)
{
quotient++;
}
#endif
fix16_t result = quotient;
/* Figure out the sign of result */
if ((a ^ b) & 0x80000000)
{
#ifndef FIXMATH_NO_OVERFLOW
if (result == fix16_min)
return fix16_overflow;
#endif
result = -result;
}
return result;
uint32_t quotient = 0;
uint32_t bit = 0x10000;
/* The algorithm requires D >= R */
while (divider < remainder)
{
divider <<= 1;
bit <<= 1;
}
#ifndef FIXMATH_NO_OVERFLOW
if (!bit)
return fix16_overflow;
#endif
if (divider & 0x80000000)
{
// Perform one step manually to avoid overflows later.
// We know that divider's bottom bit is 0 here.
if (remainder >= divider)
{
quotient |= bit;
remainder -= divider;
}
divider >>= 1;
bit >>= 1;
}
/* Main division loop */
while (bit && remainder)
{
if (remainder >= divider)
{
quotient |= bit;
remainder -= divider;
}
remainder <<= 1;
bit >>= 1;
}
#ifndef FIXMATH_NO_ROUNDING
if (remainder >= divider)
{
quotient++;
}
#endif
fix16_t result = quotient;
/* Figure out the sign of result */
if ((a ^ b) & 0x80000000)
{
#ifndef FIXMATH_NO_OVERFLOW
if (result == fix16_min)
return fix16_overflow;
#endif
result = -result;
}
return result;
}
#endif
#ifndef FIXMATH_NO_OVERFLOW
/* Wrapper around fix16_div to add saturating arithmetic. */
fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) {
fix16_t result = fix16_div(inArg0, inArg1);
if (result == fix16_overflow)
{
if ((inArg0 >= 0) == (inArg1 >= 0))
return fix16_max;
else
return fix16_min;
}
return result;
fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1)
{
fix16_t result = fix16_div(inArg0, inArg1);
if (result == fix16_overflow)
{
if ((inArg0 >= 0) == (inArg1 >= 0))
return fix16_max;
else
return fix16_min;
}
return result;
}
#endif
fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) {
fix16_t fix16_mod(fix16_t x, fix16_t y)
{
#ifdef FIXMATH_OPTIMIZE_8BIT
/* The reason we do this, rather than use a modulo operator
* is that if you don't have a hardware divider, this will result
* in faster operations when the angles are close to the bounds.
*/
while(x >= y) x -= y;
while(x <= -y) x += y;
#else
/* Note that in C90, the sign of result of the modulo operation is
* undefined. in C99, it's the same as the dividend (aka numerator).
*/
x %= y;
#endif
return x;
}
fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract)
{
int64_t tempOut = int64_mul_i32_i32(inArg0, ((1 << 8) - inFract));
tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract));
tempOut = int64_shift(tempOut, -8);
return (fix16_t)int64_lo(tempOut);
}
fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) {
fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract)
{
int64_t tempOut = int64_mul_i32_i32(inArg0, ((1 << 16) - inFract));
tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract));
tempOut = int64_shift(tempOut, -16);
@ -474,10 +499,11 @@ fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) {
}
#ifndef FIXMATH_NO_64BIT
fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) {
fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract)
{
int64_t tempOut;
tempOut = ((int64_t)inArg0 * (0 - inFract));
tempOut += ((int64_t)inArg1 * inFract);
tempOut = ((int64_t)inArg0 * (0 - inFract));
tempOut += ((int64_t)inArg1 * inFract);
tempOut >>= 32;
return (fix16_t)tempOut;
}

47
libfixmath/fix16.h
View File

@ -31,8 +31,8 @@ static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.22
static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */
static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */
static const fix16_t fix16_max = 0x7FFFFFFF; /*!< the maximum value of fix16_t */
static const fix16_t fix16_min = 0x80000000; /*!< the minimum value of fix16_t */
static const fix16_t fix16_max = 0x7FFFFFFF; /*!< the maximum value of fix16_t */
static const fix16_t fix16_min = 0x80000000; /*!< the minimum value of fix16_t */
static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */
static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */
@ -42,38 +42,37 @@ static const fix16_t fix16_one = 0x00010000; /*!< fix16_t value of 1 */
/* Conversion functions between fix16_t and float/integer.
* These are inlined to allow compiler to optimize away constant numbers
*/
static inline fix16_t fix16_from_int(int a) { return a * fix16_one; }
static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; }
static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; }
static inline fix16_t fix16_from_int(int a) { return a * fix16_one; }
static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; }
static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; }
static inline int fix16_to_int(fix16_t a)
{
#ifdef FIXMATH_NO_ROUNDING
return a >> 16;
return (a >> 16);
#else
if (a >= 0)
return (a + fix16_one / 2) / fix16_one;
else
return (a - fix16_one / 2) / fix16_one;
if (a >= 0)
return (a + (fix16_one >> 1)) / fix16_one;
return (a - (fix16_one >> 1)) / fix16_one;
#endif
}
static inline fix16_t fix16_from_float(float a)
{
float temp = a * fix16_one;
float temp = a * fix16_one;
#ifndef FIXMATH_NO_ROUNDING
temp += (temp >= 0) ? 0.5f : -0.5f;
temp += (temp >= 0) ? 0.5f : -0.5f;
#endif
return (fix16_t)temp;
return (fix16_t)temp;
}
static inline fix16_t fix16_from_dbl(double a)
{
double temp = a * fix16_one;
double temp = a * fix16_one;
#ifndef FIXMATH_NO_ROUNDING
temp += (temp >= 0) ? 0.5f : -0.5f;
temp += (temp >= 0) ? 0.5f : -0.5f;
#endif
return (fix16_t)temp;
return (fix16_t)temp;
}
/* Subtraction and addition with (optional) overflow detection. */
@ -111,6 +110,12 @@ extern fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
extern fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
#endif
/*! Divides the first given fix16_t by the second and returns the result.
*/
extern fix16_t fix16_mod(fix16_t x, fix16_t y) FIXMATH_FUNC_ATTRS;
/*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract)
*/
extern fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS;
@ -119,6 +124,8 @@ extern fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) FI
extern fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS;
#endif
/*! Returns the sine of the given fix16_t.
*/
extern fix16_t fix16_sin_parabola(fix16_t inAngle) FIXMATH_FUNC_ATTRS;
@ -151,6 +158,14 @@ extern fix16_t fix16_atan(fix16_t inValue) FIXMATH_FUNC_ATTRS;
*/
extern fix16_t fix16_atan2(fix16_t inY, fix16_t inX) FIXMATH_FUNC_ATTRS;
static const fix16_t fix16_rad_to_deg_mult = 3754936;
static inline fix16_t fix16_rad_to_deg(fix16_t radians)
{ return fix16_mul(radians, fix16_rad_to_deg_mult); }
static const fix16_t fix16_deg_to_rad_mult = 1144;
static inline fix16_t fix16_deg_to_rad(fix16_t degrees)
{ return fix16_mul(degrees, fix16_deg_to_rad_mult); }
/*! Returns the square root of the given fix16_t.

137
libfixmath/fix16_sqrt.c
View File

@ -9,75 +9,76 @@
* Not sure if someone relies on this behaviour, but not going
* to break it for now. It doesn't slow the code much overall.
*/
fix16_t fix16_sqrt(fix16_t inValue) {
uint8_t neg = (inValue < 0);
uint32_t num = (neg ? -inValue : inValue);
uint32_t result = 0;
uint32_t bit;
uint8_t n;
// Many numbers will be less than 15, so
// this gives a good balance between time spent
// in if vs. time spent in the while loop
// when searching for the starting value.
if (num & 0xFFF00000)
bit = (uint32_t)1 << 30;
else
bit = (uint32_t)1 << 18;
while (bit > num) bit >>= 2;
// The main part is executed twice, in order to avoid
// using 64 bit values in computations.
for (n = 0; n < 2; n++)
{
// First we get the top 24 bits of the answer.
while (bit)
{
if (num >= result + bit)
{
num -= result + bit;
result = (result >> 1) + bit;
}
else
{
result = (result >> 1);
}
bit >>= 2;
}
if (n == 0)
{
// Then process it again to get the lowest 8 bits.
if (num > 65535)
{
// The remainder 'num' is too large to be shifted left
// by 16, so we have to add 1 to result manually and
// adjust 'num' accordingly.
// num = a - (result + 0.5)^2
// = num + result^2 - (result + 0.5)^2
// = num - result - 0.5
num -= result;
num = (num << 16) - 0x8000;
result = (result << 16) + 0x8000;
}
else
{
num <<= 16;
result <<= 16;
}
bit = 1 << 14;
}
}
fix16_t fix16_sqrt(fix16_t inValue)
{
uint8_t neg = (inValue < 0);
uint32_t num = (neg ? -inValue : inValue);
uint32_t result = 0;
uint32_t bit;
uint8_t n;
// Many numbers will be less than 15, so
// this gives a good balance between time spent
// in if vs. time spent in the while loop
// when searching for the starting value.
if (num & 0xFFF00000)
bit = (uint32_t)1 << 30;
else
bit = (uint32_t)1 << 18;
while (bit > num) bit >>= 2;
// The main part is executed twice, in order to avoid
// using 64 bit values in computations.
for (n = 0; n < 2; n++)
{
// First we get the top 24 bits of the answer.
while (bit)
{
if (num >= result + bit)
{
num -= result + bit;
result = (result >> 1) + bit;
}
else
{
result = (result >> 1);
}
bit >>= 2;
}
if (n == 0)
{
// Then process it again to get the lowest 8 bits.
if (num > 65535)
{
// The remainder 'num' is too large to be shifted left
// by 16, so we have to add 1 to result manually and
// adjust 'num' accordingly.
// num = a - (result + 0.5)^2
// = num + result^2 - (result + 0.5)^2
// = num - result - 0.5
num -= result;
num = (num << 16) - 0x8000;
result = (result << 16) + 0x8000;
}
else
{
num <<= 16;
result <<= 16;
}
bit = 1 << 14;
}
}
#ifndef FIXMATH_NO_ROUNDING
// Finally, if next bit would have been 1, round the result upwards.
if (num > result)
{
result++;
}
// Finally, if next bit would have been 1, round the result upwards.
if (num > result)
{
result++;
}
#endif
return (neg ? -result : result);
return (neg ? -result : result);
}

25
libfixmath/fix16_trig.c
View File

@ -44,7 +44,8 @@ fix16_t fix16_sin_parabola(fix16_t inAngle)
return retval;
}
fix16_t fix16_sin(fix16_t inAngle) {
fix16_t fix16_sin(fix16_t inAngle)
{
fix16_t tempAngle = inAngle % (fix16_pi << 1);
#ifdef FIXMATH_SIN_LUT
@ -105,17 +106,22 @@ fix16_t fix16_sin(fix16_t inAngle) {
return tempOut;
}
fix16_t fix16_cos(fix16_t inAngle) {
fix16_t fix16_cos(fix16_t inAngle)
{
return fix16_sin(inAngle + (fix16_pi >> 1));
}
fix16_t fix16_tan(fix16_t inAngle) {
fix16_t fix16_tan(fix16_t inAngle)
{
return fix16_sdiv(fix16_sin(inAngle), fix16_cos(inAngle));
}
fix16_t fix16_asin(fix16_t inValue) {
if((inValue > fix16_one) || (inValue < -fix16_one))
fix16_t fix16_asin(fix16_t inValue)
{
if((inValue > fix16_one)
|| (inValue < -fix16_one))
return 0;
fix16_t tempOut;
tempOut = (fix16_one - fix16_mul(inValue, inValue));
tempOut = fix16_div(inValue, fix16_sqrt(tempOut));
@ -123,11 +129,13 @@ fix16_t fix16_asin(fix16_t inValue) {
return tempOut;
}
fix16_t fix16_acos(fix16_t inValue) {
fix16_t fix16_acos(fix16_t inValue)
{
return ((fix16_pi >> 1) - fix16_asin(inValue));
}
fix16_t fix16_atan2(fix16_t inY , fix16_t inX) {
fix16_t fix16_atan2(fix16_t inY , fix16_t inX)
{
fix16_t abs_inY, mask, angle, r, r_3;
#ifndef FIXMATH_NO_CACHE
@ -166,6 +174,7 @@ fix16_t fix16_atan2(fix16_t inY , fix16_t inX) {
return angle;
}
fix16_t fix16_atan(fix16_t inValue) {
fix16_t fix16_atan(fix16_t inValue)
{
return fix16_atan2(inValue, fix16_one);
}