Added some deg/rad conversion functions.

Fixed up some code style inconsistencies.
Added a modulo function to allow optimization on 8-bit machines.

4 files changed, 497 insertions, 446 deletions

diff --git a/libfixmath/fix16.c b/libfixmath/fix16.c
index 72e1b4a..810efe6 100644
--- a/libfixmath/fix16.c
+++ b/libfixmath/fix16.c
@@ -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;

 }

diff --git a/libfixmath/fix16.h b/libfixmath/fix16.h
index 6e5c428..b396eaf 100644
--- a/libfixmath/fix16.h
+++ b/libfixmath/fix16.h
@@ -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.

diff --git a/libfixmath/fix16_sqrt.c b/libfixmath/fix16_sqrt.c
index 13d31a8..abf57f7 100644
--- a/libfixmath/fix16_sqrt.c
+++ b/libfixmath/fix16_sqrt.c
@@ -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);

 }

diff --git a/libfixmath/fix16_trig.c b/libfixmath/fix16_trig.c
index 6b53682..864dc22 100644
--- a/libfixmath/fix16_trig.c
+++ b/libfixmath/fix16_trig.c
@@ -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);

 }