1  /* ----------------------------------------------------------------------
  2   * Project:      CMSIS DSP Library
  3   * Title:        arm_atan2_f32.c
  4   * Description:  float32 Arc tangent of y/x
  5   *
  6   * $Date:        22 April 2022
  7   * $Revision:    V1.10.0
  8   *
  9   * Target Processor: Cortex-M and Cortex-A cores
 10   * -------------------------------------------------------------------- */
 11  /*
 12   * Copyright (C) 2010-2022 ARM Limited or its affiliates. All rights reserved.
 13   *
 14   * SPDX-License-Identifier: Apache-2.0
 15   *
 16   * Licensed under the Apache License, Version 2.0 (the License); you may
 17   * not use this file except in compliance with the License.
 18   * You may obtain a copy of the License at
 19   *
 20   * www.apache.org/licenses/LICENSE-2.0
 21   *
 22   * Unless required by applicable law or agreed to in writing, software
 23   * distributed under the License is distributed on an AS IS BASIS, WITHOUT
 24   * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 25   * See the License for the specific language governing permissions and
 26   * limitations under the License.
 27   */
 28  
 29  #include "dsp/fast_math_functions.h"        
 30  
 31  /*
 32  
 33  atan for argument between in [0, 1.0]
 34  
 35  
 36  */
 37  
 38  #define ATANHALFF32 0.463648f
 39  #define PIHALFF32 1.5707963267948966192313f
 40  
 41  #define ATAN2_NB_COEFS_F32 10
 42  
 43  static const float32_t atan2_coefs_f32[ATAN2_NB_COEFS_F32]={0.0f
 44  ,1.0000001638308195518f
 45  ,-0.0000228941363602264f
 46  ,-0.3328086544578890873f
 47  ,-0.004404814619311061f
 48  ,0.2162217461808173258f
 49  ,-0.0207504842057097504f
 50  ,-0.1745263362250363339f
 51  ,0.1340557235283553386f
 52  ,-0.0323664125927477625f
 53  };
 54  
 55  __STATIC_FORCEINLINE float32_t arm_atan_limited_f32(float32_t x)
 56  {
 57      float32_t res=atan2_coefs_f32[ATAN2_NB_COEFS_F32-1];
 58      int i=1;
 59      for(i=1;i<ATAN2_NB_COEFS_F32;i++)
 60      {
 61          res = x*res + atan2_coefs_f32[ATAN2_NB_COEFS_F32-1-i];
 62      }
 63  
 64  
 65      return(res);
 66  }
 67  
 68  __STATIC_FORCEINLINE float32_t arm_atan_f32(float32_t x)
 69  {
 70     int sign=0;
 71     float32_t res=0.0f;
 72  
 73     if (x < 0.0f)
 74     {
 75        sign=1;
 76        x=-x;
 77     }
 78  
 79     if (x > 1.0f)
 80     {
 81        x = 1.0f / x;
 82        res = PIHALFF32 - arm_atan_limited_f32(x);
 83     }
 84     else
 85     {
 86       res += arm_atan_limited_f32(x);
 87     }
 88  
 89  
 90     if (sign)
 91     {
 92       res = -res;
 93     }
 94  
 95     return(res);
 96  }
 97  
 98  
 99  /**
100    @ingroup groupFastMath
101   */
102  
103  /**
104    @defgroup atan2 ArcTan2
105  
106    Computing Arc tangent only using the ratio y/x is not enough to determine the angle
107    since there is an indeterminacy. Opposite quadrants are giving the same ratio.
108  
109    ArcTan2 is not using y/x to compute the angle but y and x and use the sign of y and x
110    to determine the quadrant.
111  
112   */
113  
114  /**
115    @addtogroup atan2
116    @{
117   */
118  
119  /**
120    @brief       Arc Tangent of y/x using sign of y and x to get right quadrant
121    @param[in]   y  y coordinate
122    @param[in]   x  x coordinate
123    @param[out]  result  Result
124    @return  error status.
125   
126    @par         Compute the Arc tangent of y/x:
127                     The sign of y and x are used to determine the right quadrant
128                     and compute the right angle.
129  */
130  
131  
132  arm_status arm_atan2_f32(float32_t y,float32_t x,float32_t *result)
133  {
134      if (x > 0.0f)
135      {
136          *result=arm_atan_f32(y/x);
137          return(ARM_MATH_SUCCESS);
138      }
139      if (x < 0.0f)
140      {
141          if (y > 0.0f)
142          {
143             *result=arm_atan_f32(y/x) + PI;
144          }
145          else if (y < 0.0f)
146          {
147             *result=arm_atan_f32(y/x) - PI;
148          }
149          else
150          {
151              if (signbit(y))
152              {
153                 *result= -PI;
154              }
155              else
156              {
157                 *result= PI;
158              }
159          }
160          return(ARM_MATH_SUCCESS);
161      }
162      if (x == 0.0f)
163      {
164          if (y > 0.0f)
165          {
166              *result=PIHALFF32;
167              return(ARM_MATH_SUCCESS);
168          }
169          if (y < 0.0f)
170          {
171              *result=-PIHALFF32;
172              return(ARM_MATH_SUCCESS);
173          }
174      }
175      
176  
177      return(ARM_MATH_NANINF);
178  
179  }
180  
181  /**
182    @} end of atan2 group
183   */