spandsp  0.0.6
arctan2.h
Go to the documentation of this file.
1 /*
2  * SpanDSP - a series of DSP components for telephony
3  *
4  * arctan2.h - A quick rough approximate arc tan
5  *
6  * Written by Steve Underwood <steveu@coppice.org>
7  *
8  * Copyright (C) 2003 Steve Underwood
9  *
10  * All rights reserved.
11  *
12  * This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU Lesser General Public License version 2.1,
14  * as published by the Free Software Foundation.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Lesser General Public License for more details.
20  *
21  * You should have received a copy of the GNU Lesser General Public
22  * License along with this program; if not, write to the Free Software
23  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24  */
25 
26 /*! \file */
27 
28 #if !defined(_SPANDSP_ARCTAN2_H_)
29 #define _SPANDSP_ARCTAN2_H_
30 
31 /*! \page arctan2_page Fast approximate four quadrant arc-tangent
32 \section arctan2_page_sec_1 What does it do?
33 This module provides a fast approximate 4-quadrant arc tangent function,
34 based on something at dspguru.com. The worst case error is about 4.07 degrees.
35 This is fine for many "where am I" type evaluations in comms. work.
36 
37 \section arctan2_page_sec_2 How does it work?
38 ???.
39 */
40 
41 #if defined(__cplusplus)
42 extern "C"
43 {
44 #endif
45 
46 /* This returns its answer as a signed 32 bit integer phase value. */
47 static __inline__ int32_t arctan2(float y, float x)
48 {
49  float abs_y;
50  float angle;
51 
52  if (y == 0.0f)
53  {
54  if (x < 0.0f)
55  return 0x80000000;
56  return 0x00000000;
57  }
58  if (x == 0.0f)
59  {
60  if (y < 0.0f)
61  return 0xc0000000;
62  return 0x40000000;
63  }
64 
65  abs_y = fabsf(y);
66 
67  /* If we are in quadrant II or III, flip things around */
68  if (x < 0.0f)
69  angle = 3.0f - (x + abs_y)/(abs_y - x);
70  else
71  angle = 1.0f - (x - abs_y)/(abs_y + x);
72  angle *= 536870912.0f;
73 
74  /* If we are in quadrant III or IV, negate to return an
75  answer in the range +-pi */
76  if (y < 0.0f)
77  angle = -angle;
78  return (int32_t) angle;
79 }
80 /*- End of function --------------------------------------------------------*/
81 
82 #if 0
83 /* This returns its answer in radians, in the range +-pi. */
84 static __inline__ float arctan2f(float y, float x)
85 {
86  float angle;
87  float fx;
88  float fy;
89 
90  if (y == 0.0f)
91  {
92  if (x < 0.0f)
93  return 3.1415926f;
94  return 0.0f;
95  }
96  if (x == 0.0f)
97  {
98  if (y < 0.0f)
99  return 3.1415926f*1.5f;
100  return 3.1415926f*0.5f;
101  }
102  fx = fabsf(x);
103  fy = fabsf(y);
104  /* Deal with the octants */
105  /* N.B. 0.28125 == (1/4 + 1/32) */
106  if (fy > fx)
107  angle = 3.1415926f/2.0f - fx*fy/(y*y + 0.28125f*x*x);
108  else
109  angle = fy*fx/(x*x + 0.28125f*y*y);
110 
111  /* Deal with the quadrants, to bring the final answer to the range +-pi */
112  if (x < 0.0f)
113  angle = 3.1415926f - angle;
114  if (y < 0.0f)
115  angle = -angle;
116  return angle;
117 }
118 /*- End of function --------------------------------------------------------*/
119 #endif
120 
121 #if defined(__cplusplus)
122 }
123 #endif
124 
125 #endif
126 /*- End of file ------------------------------------------------------------*/