iatan.c

三角函数atan的快速算法

/**
@file	iatan.c
@brief	三角函数atan的快速算法，用泰勒级数逼近，结果化为以半周为单位的量乘以比例系数，
	用整数表示
 */
#include "iatan.h"
#include "mathspt.h"

//#define _DEBUG_LIB_

#define LOOPI_ATAN_CONSTSCALE	((1<<LOOPI_ATAN_SCALEBITS)/(PI))
#define LOOPI_ATAN_SCALE		(1<<LOOPI_ATAN_SCALEBITS)

/*!
@function	atan2i2q
@brief	三角函数二象限atan的快速算法实现
@para	y,	输入参数y
@para	x,	输入参数x
@return I4型，乘以比例系数(1<<LOOPI_ATAN_SCALEBITS)后的整周数(ccl:circle,ccl = rad/2pi)
值域为-0.25*(1<<LOOPI_ATAN_SCALEBITS) < res <= 0.25*(1<<LOOPI_ATAN_SCALEBITS)
 */
I4 atan2i2q(
	register I4 y,
	register I4 x
)
{
	register I8 t,t2,t3,t4;
	register I4 /*it,*/ty,tx,r;
	if((ty = abs(y)) <= ((tx = abs(x))>>1)) {
		r = 0;// + res; // 0 <= res <= 1/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)ty/((F4)tx));
	} else
	if(tx <= (ty>>1)) {
		r = (LOOPI_ATAN_SCALE>>1);// - res; // 3/8 < res <= 1/2
		t = -(I4)((F4)LOOPI_ATAN_SCALE*(F4)tx/((F4)ty));
	} else {
		r = (LOOPI_ATAN_SCALE>>2);// + res; // 1/8 < res <= 3/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)(ty - tx)/((F4)(ty + tx)));
	}
	t2 = (t*t)>>LOOPI_ATAN_SCALEBITS;
	t4 = (t2*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 = LOOPI_ATAN_SCALE
		- ((t2*(LOOPI_ATAN_SCALE/3))>>LOOPI_ATAN_SCALEBITS)
		+ ((t4*(LOOPI_ATAN_SCALE/5))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 -= ((t4*(LOOPI_ATAN_SCALE/7))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 += ((t4*(LOOPI_ATAN_SCALE/9))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 -= ((t4*(LOOPI_ATAN_SCALE/11))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 += ((t4*(LOOPI_ATAN_SCALE/13))>>LOOPI_ATAN_SCALEBITS);
	r += (I4)((((t*t3)>>LOOPI_ATAN_SCALEBITS)*(I8)(LOOPI_ATAN_CONSTSCALE))
					>>LOOPI_ATAN_SCALEBITS);
	r >>= 1;
	return ((y^x)>>31) != 0 ? -r : r;
}

/*!
@function	atan2i4q
@brief	三角函数四象限atan的快速算法实现
@para	y,	输入参数y
@para	x,	输入参数x
@return I4型，乘以比例系数(1<<LOOPI_ATAN_SCALEBITS)后的整周数(ccl:circle,ccl = rad/2pi)
值域为-1*(1<<LOOPI_ATAN_SCALEBITS) < res <= 1*(1<<LOOPI_ATAN_SCALEBITS)
 */
I4 atan2i4q(
	register I4 y,
	register I4 x
)
{
	register I8 t,t2,t3,t4;
	register I4 /*it,*/ty,tx,r;
	if((ty = abs(y)) <= ((tx = abs(x))>>1)) {
		r = 0;// + res; // 0 <= res <= 1/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)ty/((F4)tx));
	} else
	if(tx <= (ty>>1)) {
		r = (LOOPI_ATAN_SCALE>>1);// - res; // 3/8 < res <= 1/2
		t = -(I4)((F4)LOOPI_ATAN_SCALE*(F4)tx/((F4)ty));
	} else {
		r = (LOOPI_ATAN_SCALE>>2);// + res; // 1/8 < res <= 3/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)(ty - tx)/((F4)(ty + tx)));
	}
/*	if((ty = abs(y)) <= ((tx = abs(x))>>1)) {
		r = 0;// + res; // 0 <= res <= 1/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)ty/((F4)tx));
	} else 
	if(ty <= tx) {
		r = LOOPI_ATAN_SCALE/4;// - res; // 1/8 < res <= 1/4
		t = -(I4)((F4)LOOPI_ATAN_SCALE*(F4)(tx - ty)/((F4)(tx + ty)));
	} else
	if(tx <= ty) {
		r = (LOOPI_ATAN_SCALE>>1) - LOOPI_ATAN_SCALE/4;// + res; // 1/4 < res <= 3/8
		t = (I4)((F4)LOOPI_ATAN_SCALE*(F4)(ty - tx)/((F4)(ty + tx)));
	} else
	if(tx <= (ty>>1)) {
		r = (LOOPI_ATAN_SCALE>>1);// - res; // 3/8 < res <= 1/2
		t = -(I4)((F4)LOOPI_ATAN_SCALE*(F4)tx/((F4)ty));
	}
*/
	t2 = (t*t)>>LOOPI_ATAN_SCALEBITS;
	t4 = (t2*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 = LOOPI_ATAN_SCALE
		- ((t2*(LOOPI_ATAN_SCALE/3))>>LOOPI_ATAN_SCALEBITS)
		+ ((t4*(LOOPI_ATAN_SCALE/5))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 -= ((t4*(LOOPI_ATAN_SCALE/7))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 += ((t4*(LOOPI_ATAN_SCALE/9))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 -= ((t4*(LOOPI_ATAN_SCALE/11))>>LOOPI_ATAN_SCALEBITS);
	t4 = (t4*t2)>>LOOPI_ATAN_SCALEBITS;
	t3 += ((t4*(LOOPI_ATAN_SCALE/13))>>LOOPI_ATAN_SCALEBITS);
	r += (I4)((((t*t3)>>LOOPI_ATAN_SCALEBITS)*(I8)(LOOPI_ATAN_CONSTSCALE))
					>>LOOPI_ATAN_SCALEBITS);
/*	if((ty = abs(y)) <= (tx = abs(x))) {
		if(ty <= (tx>>1)) {
			t = (F4)LOOPI_ATAN_SCALE*(F4)ty/((F4)tx);
			t2 = (t*t)>>LOOPI_ATAN_SCALEBITS;
			t4 = (t2*t2)>>LOOPI_ATAN_SCALEBITS;
			t3 = LOOPI_ATAN_SCALE
				- ((t2*(LOOPI_ATAN_SCALE/3))>>LOOPI_ATAN_SCALEBITS)
				+ ((t4*(LOOPI_ATAN_SCALE/5))>>LOOPI_ATAN_SCALEBITS)
				- ((((t2*t4)>>LOOPI_ATAN_SCALEBITS)*(LOOPI_ATAN_SCALE/7))
					>>LOOPI_ATAN_SCALEBITS);
			r = (I4)((((t*t3)>>LOOPI_ATAN_SCALEBITS)*(I8)(LOOPI_ATAN_SCALE/PI))
					>>LOOPI_ATAN_SCALEBITS);
		} else {
			t = (F4)LOOPI_ATAN_SCALE*(F4)(tx - ty)/((F4)(tx + ty));
			t2 = (t*t)>>LOOPI_ATAN_SCALEBITS;
			t4 = (t2*t2)>>LOOPI_ATAN_SCALEBITS;
			t3 = LOOPI_ATAN_SCALE - ((t2*(LOOPI_ATAN_SCALE/3))>>LOOPI_ATAN_SCALEBITS)
				+ ((t4*(LOOPI_ATAN_SCALE/5))>>LOOPI_ATAN_SCALEBITS)
				- ((((t2*t4)>>LOOPI_ATAN_SCALEBITS)*(LOOPI_ATAN_SCALE/7))>>LOOPI_ATAN_SCALEBITS);
			r = LOOPI_ATAN_SCALE/4 - (I4)((((t*t3)>>LOOPI_ATAN_SCALEBITS)*(I8)(LOOPI_ATAN_SCALE/PI))>>LOOPI_ATAN_SCALEBITS);
		}
	} else {
		t = (F4)LOOPI_ATAN_SCALE*(F4)tx/((F4)ty);
		t2 = (t*t)>>LOOPI_ATAN_SCALEBITS;
		t3 = LOOPI_ATAN_SCALE - ((t2*(LOOPI_ATAN_SCALE/3))>>LOOPI_ATAN_SCALEBITS)
			+ ((((t2*t2)>>LOOPI_ATAN_SCALEBITS)*(LOOPI_ATAN_SCALE/5))>>LOOPI_ATAN_SCALEBITS);
		r = (LOOPI_ATAN_SCALE>>1)
			- (I4)((((t*t3)>>LOOPI_ATAN_SCALEBITS)*(I8)(LOOPI_ATAN_SCALE/PI))>>LOOPI_ATAN_SCALEBITS);
	}
*/	if(x < 0) r = LOOPI_ATAN_SCALE - r;
	if(y < 0) r = -r;
	r >>= 1;
	return r;
}

#ifdef	_DEBUG_LIB_

#include "mathspt.h"

I4 res1,res2,i,maxdiff = 0,maxi = -1;

#define TEST_CNTS	64

void main(void)
{
	for(i = 0; i < TEST_CNTS; i++) {
		res1 = (I4)(atan2f(i,TEST_CNTS)*(F4)LOOPI_ATAN_CONSTSCALE)/2;
		res2 = atan2i4q(i,TEST_CNTS);
		if(maxdiff < abs(res1 - res2)) {
			maxdiff = abs(res1 - res2);
			maxi = i;
		}
	}
}

#endif//_DEBUG_LIB_