00001 /* 00002 * SpanDSP - a series of DSP components for telephony 00003 * 00004 * arctan2.h - A quick rough approximate arc tan 00005 * 00006 * Written by Steve Underwood <steveu@coppice.org> 00007 * 00008 * Copyright (C) 2003 Steve Underwood 00009 * 00010 * All rights reserved. 00011 * 00012 * This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU Lesser General Public License version 2.1, 00014 * as published by the Free Software Foundation. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Lesser General Public License for more details. 00020 * 00021 * You should have received a copy of the GNU Lesser General Public 00022 * License along with this program; if not, write to the Free Software 00023 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00024 */ 00025 00026 /*! \file */ 00027 00028 #if !defined(_SPANDSP_ARCTAN2_H_) 00029 #define _SPANDSP_ARCTAN2_H_ 00030 00031 /*! \page arctan2_page Fast approximate four quadrant arc-tangent 00032 \section arctan2_page_sec_1 What does it do? 00033 This module provides a fast approximate 4-quadrant arc tangent function, 00034 based on something at dspguru.com. The worst case error is about 4.07 degrees. 00035 This is fine for many "where am I" type evaluations in comms. work. 00036 00037 \section arctan2_page_sec_2 How does it work? 00038 ???. 00039 */ 00040 00041 #if defined(__cplusplus) 00042 extern "C" 00043 { 00044 #endif 00045 00046 /* This returns its answer as a signed 32 bit integer phase value. */ 00047 static __inline__ int32_t arctan2(float y, float x) 00048 { 00049 float abs_y; 00050 float angle; 00051 00052 if (y == 0.0f) 00053 { 00054 if (x < 0.0f) 00055 return 0x80000000; 00056 return 0x00000000; 00057 } 00058 if (x == 0.0f) 00059 { 00060 if (y < 0.0f) 00061 return 0xc0000000; 00062 return 0x40000000; 00063 } 00064 00065 abs_y = fabsf(y); 00066 00067 /* If we are in quadrant II or III, flip things around */ 00068 if (x < 0.0f) 00069 angle = 3.0f - (x + abs_y)/(abs_y - x); 00070 else 00071 angle = 1.0f - (x - abs_y)/(abs_y + x); 00072 angle *= 536870912.0f; 00073 00074 /* If we are in quadrant III or IV, negate to return an 00075 answer in the range +-pi */ 00076 if (y < 0.0f) 00077 angle = -angle; 00078 return (int32_t) angle; 00079 } 00080 /*- End of function --------------------------------------------------------*/ 00081 00082 #if 0 00083 /* This returns its answer in radians, in the range +-pi. */ 00084 static __inline__ float arctan2f(float y, float x) 00085 { 00086 float angle; 00087 float fx; 00088 float fy; 00089 00090 if (y == 0.0f) 00091 { 00092 if (x < 0.0f) 00093 return 3.1415926f; 00094 return 0.0f; 00095 } 00096 if (x == 0.0f) 00097 { 00098 if (y < 0.0f) 00099 return 3.1415926f*1.5f; 00100 return 3.1415926f*0.5f; 00101 } 00102 fx = fabsf(x); 00103 fy = fabsf(y); 00104 /* Deal with the octants */ 00105 /* N.B. 0.28125 == (1/4 + 1/32) */ 00106 if (fy > fx) 00107 angle = 3.1415926f/2.0f - fx*fy/(y*y + 0.28125f*x*x); 00108 else 00109 angle = fy*fx/(x*x + 0.28125f*y*y); 00110 00111 /* Deal with the quadrants, to bring the final answer to the range +-pi */ 00112 if (x < 0.0f) 00113 angle = 3.1415926f - angle; 00114 if (y < 0.0f) 00115 angle = -angle; 00116 return angle; 00117 } 00118 /*- End of function --------------------------------------------------------*/ 00119 #endif 00120 00121 #if defined(__cplusplus) 00122 } 00123 #endif 00124 00125 #endif 00126 /*- End of file ------------------------------------------------------------*/
1.6.1