intatan2cordic.c

是codic算法实现atan的C程序

/*intAtan2Cordic returns the arctangent of sinTheta/cosTheta
* in the range –LONG_PI+1 to LONG_PI-1 radians. 
*(LONG_PI= 2147483648  // The second power of 31.) 
*atan2 uses the signs of both parameters to determine 
*the quadrant of the return value. 
*/

#include "headDefine.h"

int intAtan2Cordic(int Y, int X) {
	int i;  				// for iteration
	
	int bitZoomIn = 16;		// theta is magnified by (2^bitZoomIn)
	int N = bitZoomIn;  		// loop times
	
	int absX[16] = {0};
	int absY[16] = {0};

	// theta[i] = floor((atan(1 / pow(2, i)) * 2 ^ bitZoomIn);
	int theta[16] = {	51471,  30385,  16054,  8149,   4090,	2047,   1023,   511,
						255,	127,	63,		31,		15,		7,		3,		1	};
	int estiTheta = 0;	// the value of atan2(Y, X)
	
	if (X == 0 || Y == 0) {
		if (X ==0) {
			if (Y > 0) 
				estiTheta = LONG_PI / 2;
			else if(Y < 0)
				estiTheta = -LONG_PI / 2;
			else estiTheta = 0;
		}
		if (Y ==0) {
			if (X >= 0) 
				estiTheta = 0;
			else if(X < 0)
				estiTheta = LONG_PI;
		}
	}
	else {
		// get the abs(X) and abs(Y)
		absX[0] = abs(X);
		absY[0] = abs(Y);
	
		// get the theta array
		//for (i = 0; i < N; i++) {
		//	theta[i] = (int)(atan(1 / pow(2, i)) << bitZoomIn);
		//}
		//theta[16] = {51471,  30385,  16054,  8149,   4090,  2047,   1023,   511,
		//					255,	127,	 63,	31,		15,		7,		3,	   1};
	
		// main operation ------------------ 
		for (i = 0; i < N - 1; i++) {
			if (absY[i] > 0) {
				estiTheta = estiTheta + theta[i];
				absX[i + 1] = absX[i] + (absY[i] >> i);
				absY[i + 1] = absY[i] - (absX[i] >> i);
			}
			else if (absY[i] < 0) {
				estiTheta = estiTheta - theta[i];
				absX[i + 1] = absX[i] - (absY[i] >> i);
				absY[i + 1] = absY[i] + (absX[i] >> i);
			}
			else estiTheta = estiTheta;
		}
		
		// judge which quadrant estiTheta is 
		if (X > 0) {
			if (Y >0) {			// the first quadrant
				estiTheta = estiTheta >> bitZoomIn;
			}
			else if (Y < 0) {	// the forth quadrant
				estiTheta = -estiTheta >> bitZoomIn;
			}
		}
		else if (X < 0) {
			if (Y >0) {			// the second quadrant
				estiTheta = (205887 - estiTheta) >> bitZoomIn; //205887 = pi * 2 ^ 16
			}
			else if (Y < 0) {	// the third quadrant
				estiTheta = (estiTheta - 205887) >> bitZoomIn;
			}
		}
	}

	return(estiTheta);
}