fix.cpp

Tixys source code, include G.711, G.726, IMA-ADPCM etc.

/**
@file

@brief 32-bit fixed-point maths routines

For latest source code see http://www.tixy.clara.net/source/

Copyright (C) 2004-2005 J.D.Medhurst (a.k.a. Tixy)

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "common.h"
#include "fix.h"


/*
Helper functions
*/


static inline int Interpolate(const int* table,int value,int shift)
	{
	// get values from table (required value lies between b and c)
	int i = value>>shift;
	int a = table[i+0];
	int b = table[i+1];
	int c = table[i+2];
	int d = table[i+3];

	// interpolate
	int f = value&((1<<shift)-1);
	int cadb = ( (c-a) - (d-b) ) >> 2;
	int r = (c-b) + cadb - (cadb*f>>shift);
	r = (uint)(r*f)>>shift;  // cast to uint assumes table only contains increasing values (and gains us 1 more bit headroom)
	r += b;

	return r;
	}

	
/*
Members of class Fix
*/


EXPORT fix Fix::Add(fix a,fix b)
	{
	fix r = a+b;
	if((~(a^b) & (a^r)) < 0)
		r = (r>>31)^0x80000000;  // produce saturated result if overflow
	return r;
	}


EXPORT fix Fix::Sub(fix a,fix b)
	{
	fix r = a-b;
	if(((a^b) & (a^r)) < 0)
		r = (r>>31)^0x80000000;  // produce saturated result if overflow
	return r;
	}


EXPORT fix Fix::Mul(fix a,fix b)
	{
	// calculate sign result
	int sign = a^b;

	// calculate absolute values
	if(a<0) a=-a;
	if(b<0) b=-b;

	// do the multiply in four parts
	uint32 al = a&0xFFFF;
	uint32 bl = b&0xFFFF;
	uint32 c = al*bl;
	c += 0x8000;
	c >>= 16;
	uint32 c1 = bl*((uint)a>>16);
	c += c1;
	if(c>=c1) // No carry from addition
		{
		uint32 c2 = al*((uint)b>>16);
		c += c2;
		if(c>=c2) // No carry from addition
			{
			uint32 d = ((uint)a>>16)*((uint)b>>16);
			if(d<0x10000) // No overflow from multiply
				{
				uint dl = d<<16;
				c += dl;
				if(c>=dl) // No carry from addition
					{
					if(sign<0)
						{
						if(c<=0x80000000)
							return -(int32)c;
						}
					else
						{
						if(c<=0x7fffffff)
							return c;
						}
					}
				}
			}
		}

	// produce saturated result
	return (sign<0) ? 0x80000000 : 0x7fffffff;
	}


EXPORT fix Fix::MulNS(fix a,fix b)
	{
	uint32 al = a&0xFFFF;
	uint32 bl = b&0xFFFF;
	uint32 r = al*bl;
	r += 0x8000;
	r >>= 16;
	r += bl*(a>>16);
	r += al*(b>>16);
	r += ((a>>16)*(b>>16))<<16;
	return r;
	}


fix Fix::Div(fix a,fix b)
	{
	// calculate sign bit of result
	int32 r = a^b;
	r &= (1<<31);

	// calculate absolute values
	if(a<0) a = -a;
	if(b<0) b = -b;
	
	// calculate the number of integer bits is the result
	int32 intBits = 0;
	uint32 q = a;
	if((uint32)b<=((uint32)q>>16))
		intBits += 16, q >>= 16;
	if((uint32)b<=((uint32)q>>8))
		intBits += 8,  q >>= 8;
	if((uint32)b<=((uint32)q>>4))
		intBits += 4,  q >>= 4;
	if((uint32)b<=((uint32)q>>2))
		intBits += 2,  q >>= 2;
	if((uint32)b<=((uint32)q>>1))
		intBits += 1,  q >>= 1;

#if 0	// set true to use loops rather than inline code

	if(intBits>14)
		return (r<0) ? 0x80000000 : 0x7fffffff; // saturated result

	// calculate the integer part of result (bits 31 to 16)
	b <<= intBits;
	int32 bit = 1<<(intBits+16);
	while(bit>0x10000)
		{
		if((uint32)a>=(uint32)b)
			{
			a -= b;
			r += bit;
			}
		b >>= 1;
		bit >>= 1;
		}
	if((uint32)a>=(uint32)b)
		{
		a -= b;
		r += bit;
		}
	bit >>= 1;

	// calculate the fractional part of result (bits 15 to 0)
	do
		{
		a <<= 1;
		if((uint32)a>=(uint32)b)
			{
			a -= b;
			r += bit;
			}
		bit >>= 1;
		}
	while(bit);

#else

	// calculate the integer part of result (bits 31 to 16)
	switch(intBits)
		{
		case 14:
			if((uint32)a>=((uint32)b<<14))
				a -= (b<<14), r += 1<<(14+16);
		case 13:
			if((uint32)a>=((uint32)b<<13))
				a -= (b<<13), r += 1<<(13+16);
		case 12:
			if((uint32)a>=((uint32)b<<12))
				a -= (b<<12), r += 1<<(12+16);
		case 11:
			if((uint32)a>=((uint32)b<<11))
				a -= (b<<11), r += 1<<(11+16);
		case 10:
			if((uint32)a>=((uint32)b<<10))
				a -= (b<<10), r += 1<<(10+16);
		case 9:
			if((uint32)a>=((uint32)b<<9))
				a -= (b<<9), r += 1<<(9+16);
		case 8:
			if((uint32)a>=((uint32)b<<8))
				a -= (b<<8), r += 1<<(8+16);
		case 7:
			if((uint32)a>=((uint32)b<<7))
				a -= (b<<7), r += 1<<(7+16);
		case 6:
			if((uint32)a>=((uint32)b<<6))
				a -= (b<<6), r += 1<<(6+16);
		case 5:
			if((uint32)a>=((uint32)b<<5))
				a -= (b<<5), r += 1<<(5+16);
		case 4:
			if((uint32)a>=((uint32)b<<4))
				a -= (b<<4), r += 1<<(4+16);
		case 3:
			if((uint32)a>=((uint32)b<<3))
				a -= (b<<3), r += 1<<(3+16);
		case 2:
			if((uint32)a>=((uint32)b<<2))
				a -= (b<<2), r += 1<<(2+16);
		case 1:
			if((uint32)a>=((uint32)b<<1))
				a -= (b<<1), r += 1<<(1+16);
		case 0:
			if((uint32)a>=((uint32)b<<0))
				a -= (b<<0), r += 1<<(0+16);
			break;
	default:
		// produce saturated result
		return (r<0) ? 0x80000000 : 0x7fffffff; // saturated result
		}

	// calculate the fractional part of result (bits 15 to 0)
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<15;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<14;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<13;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<12;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<11;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<10;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<9;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<8;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<7;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<6;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<5;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<4;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<3;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<2;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<1;
	a <<= 1;
	if((uint32)a>=(uint32)b)
		a -= b, r += 1<<0;

#endif

	return (r<0) ? 0x80000000-r : r;

	}


EXPORT fix Fix::Sqrt(ufix a)
	{
	ufix r,nr,m;

	// calculate integer part (bits 31 to 16)
	r = 0;
	m = 0x40000000;
	do
		{
		nr = r+m;
		if(nr<=a)
			{
			a -= nr;
			r = nr+m;
			}
		r >>= 1;
		m >>= 2;
		}
	while(m);

	// calculate bits 15 to 8
	r <<= 8;
	a <<= 8;
	m = 0x40;
	do
		{
		nr = r+m;
		if(nr<=a)
			{
			a -= nr;
			r = nr+m;
			}
		r >>= 1;
		m >>= 2;
		}
	while(m);

	// calculate bits 7 to 0
	r <<= 8;
	a <<= 8;
	m = 0x40;
	do
		{
		nr = r+m;
		if(nr<=a)
			{
			a -= nr;
			r = nr+m;
			}
		r >>= 1;
		m >>= 2;
		}
	while(m);

	// round result
	if(r<a)
		r++;
	
	return r;
	}


EXPORT fix Fix::Log2(ufix a)
	{
	// trap 0
	if(a==0)
		return (fix)-0x80000000;

	// calculate integer part of result in i
	// and set n = normalised value of a
	int32 i=15*0x10000;
	uint32 n=a;
	if(n<(uint32)(1<<(32-16)))
		n <<= 16, i -= 16*0x10000;
	if(n<(uint32)(1<<(32-8)))
		n <<= 8, i -= 8*0x10000;
	if(n<(uint32)(1<<(32-4)))
		n <<= 4, i -= 4*0x10000;
	if(n<(uint32)(1<<(32-2)))
		n <<= 2, i -= 2*0x10000;
	if(n<(uint32)(1<<(32-1)))
		n <<= 1, i -= 1*0x10000;

	// reduce n to the 23 most significant bits and clear
	// the most significant bit, leaving a 22 bit value in n
	n = (n-0x80000000+(1<<8))>>9;

	// calculate fractional part of result (in f) by interpolation of lookup table values
	static const int32 LogTable[] =
		{
		0xffff45e1,
		0x00000000,0x0000b73d,0x00016bad,0x00021d67,0x0002cc7f,0x00037908,0x00042316,0x0004caba,
		0x00057007,0x0006130b,0x0006b3d8,0x0007527c,0x0007ef06,0x00088984,0x00092204,0x0009b892,
		0x000a4d3c,0x000ae00d,0x000b7111,0x000c0053,0x000c8ddd,0x000d19bb,0x000da3f6,0x000e2c98,
		0x000eb3aa,0x000f3935,0x000fbd43,0x00103fdb,0x0010c105,0x001140ca,0x0011bf31,0x00123c42,
		0x0012b803,0x0013327c,0x0013abb4,0x001423b0,0x00149a78,0x00151012,0x00158482,0x0015f7d0,
		0x00166a01,0x0016db19,0x00174b20,0x0017ba19,0x0018280a,0x001894f7,0x001900e6,0x00196bdb,
		0x0019d5da,0x001a3ee8,0x001aa709,0x001b0e41,0x001b7495,0x001bda07,0x001c3e9d,0x001ca259,
		0x001d053f,0x001d6754,0x001dc89a,0x001e2914,0x001e88c7,0x001ee7b4,0x001f45e1,0x001fa34e,
		0x00200000,0x00205bf9,0x0020b73d
		};
	int32 f = Interpolate(LogTable,n,16);

	// scale result to 16 bits (from the 22 bit precision used in lookup table)
	f = (f+(1<<(5-1)))>>5;

	// return sum of integer and fractional part
	return i+f;
	}


EXPORT ufix Fix::Exp2(fix a)
	{
	if(a>=0x00100000)
		return 0xffffffffu; // result will be too big so result max value

	// special cases for small values
	if(a<-0x000ead96)
		{
		if(a<-0x00110000)
			return 0;
		if(a<-0x000f6a3f)
			return 1;
		return 2;
		}

	// table of values for ((2^n)-1)<<32 for n in range 0x0000.0000 to 0x0000.ff00
	static const int32 Exp2TableFF00[] = 
		{
		0x00000000,0x00b1afa5,0x0163da9f,0x02168143,0x02c9a3e7,0x037d42e1,0x04315e86,0x04e5f72f,
		0x059b0d31,0x0650a0e3,0x0706b29d,0x07bd42b7,0x08745187,0x092bdf66,0x09e3ecac,0x0a9c79b1,
		0x0b5586cf,0x0c0f145e,0x0cc922b7,0x0d83b233,0x0e3ec32d,0x0efa55fd,0x0fb66aff,0x1073028d,
		0x11301d01,0x11edbab5,0x12abdc06,0x136a814f,0x1429aaea,0x14e95934,0x15a98c8a,0x166a4547,
		0x172b83c7,0x17ed4869,0x18af9388,0x19726583,0x1a35beb6,0x1af99f81,0x1bbe0840,0x1c82f952,
		0x1d487316,0x1e0e75eb,0x1ed5022f,0x1f9c1843,0x2063b886,0x212be357,0x21f49917,0x22bdda27,
		0x2387a6e7,0x2451ffb8,0x251ce4fb,0x25e85711,0x26b4565e,0x2780e341,0x284dfe1f,0x291ba759,
		0x29e9df51,0x2ab8a66d,0x2b87fd0d,0x2c57e397,0x2d285a6e,0x2df961f6,0x2ecafa93,0x2f9d24ab,
		0x306fe0a3,0x31432ede,0x32170fc4,0x32eb83ba,0x33c08b26,0x3496266e,0x356c55f9,0x36431a2d,
		0x371a7373,0x37f26231,0x38cae6d0,0x39a401b7,0x3a7db34e,0x3b57fbfe,0x3c32dc31,0x3d0e544e,
		0x3dea64c1,0x3ec70df1,0x3fa4504a,0x40822c36,0x4160a21f,0x423fb270,0x431f5d95,0x43ffa3f8,
		0x44e08606,0x45c2042a,0x46a41ed1,0x4786d668,0x486a2b5c,0x494e1e19,0x4a32af0d,0x4b17dea6,
		0x4bfdad53,0x4ce41b81,0x4dcb299f,0x4eb2d81d,0x4f9b2769,0x508417f4,0x516daa2c,0x5257de83,
		0x5342b569,0x542e2f4f,0x551a4ca5,0x56070dde,0x56f4736b,0x57e27dbe,0x58d12d49,0x59c0827f,
		0x5ab07dd4,0x5ba11fba,0x5c9268a5,0x5d845909,0x5e76f15a,0x5f6a320d,0x605e1b97,0x6152ae6c,
		0x6247eb03,0x633dd1d1,0x6434634c,0x652b9feb,0x66238825,0x671c1c70,0x68155d44,0x690f4b19,