1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9
10 /*! \file approximations.c
11 \brief Math approximations file
12
13 Significant efficiencies were found by creating a set of trig functions
14 which trade off precision for improved power/CPU performance. Full details
15 are included in Application Note AN5015: Trigonometry Approximations
16 */
17
18 #include "math.h"
19 #include "stdlib.h"
20 #include "stdint.h"
21
22 #include "approximations.h"
23
24 // function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1
25 // and returns -90 <= angle <= 90 deg
26
27 // maximum error is 10.29E-6 deg
fasin_deg(float x)28 float fasin_deg(float x)
29 {
30 // for robustness, check for invalid argument
31 if (x >= 1.0F) return 90.0F;
32 if (x <= -1.0F) return -90.0F;
33
34 // call the atan which will return an angle in the correct range -90 to 90 deg
35 // this line cannot fail from division by zero or negative square root since |x| < 1
36 return (fatan_deg(x / sqrtf(1.0F - x * x)));
37 }
38
39 // function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1
40 // and returns 0 <= angle <= 180 deg
41
42 // maximum error is 14.67E-6 deg
facos_deg(float x)43 float facos_deg(float x)
44 {
45 // for robustness, check for invalid arguments
46 if (x >= 1.0F) return 0.0F;
47 if (x <= -1.0F) return 180.0F;
48
49 // call the atan which will return an angle in the incorrect range -90 to 90 deg
50 // these lines cannot fail from division by zero or negative square root
51 if (x == 0.0F) return 90.0F;
52 if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x));
53 return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x));
54 }
55
56 // function returns angle in range -90 to 90 deg
57
58 // maximum error is 9.84E-6 deg
fatan_deg(float x)59 float fatan_deg(float x)
60 {
61 float fangledeg; // compute computed (deg)
62 int8_t ixisnegative; // argument x is negative
63 int8_t ixexceeds1; // argument x is greater than 1.0
64 int8_t ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0
65 #define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3)
66 #define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3)
67 // reset all flags
68 ixisnegative = ixexceeds1 = ixmapped = 0;
69
70 // test for negative argument to allow use of tan(-x)=-tan(x)
71 if (x < 0.0F)
72 {
73 x = -x;
74 ixisnegative = 1;
75 }
76
77 // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x)
78 if (x > 1.0F)
79 {
80 x = 1.0F / x;
81 ixexceeds1 = 1;
82 }
83
84 // at this point, x is in the range 0 to 1 inclusive
85 // map argument onto range -tan(15 deg) to tan(15 deg)
86 // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg))
87 // tan(15deg) maps to tan(-15 deg) = -tan(15 deg)
88 // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg)
89 if (x > TAN15DEG)
90 {
91 x = (x - TAN30DEG) / (1.0F + TAN30DEG * x);
92 ixmapped = 1;
93 }
94
95 // call the atan estimator to obtain -15 deg <= angle <= 15 deg
96 fangledeg = fatan_15deg(x);
97
98 // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg
99 if (ixmapped) fangledeg += 30.0F;
100 if (ixexceeds1) fangledeg = 90.0F - fangledeg;
101 if (ixisnegative) fangledeg = -fangledeg;
102
103 return (fangledeg);
104 }
105
106 // function returns approximate atan2 angle in range -180 to 180 deg
107
108 // maximum error is 14.58E-6 deg
fatan2_deg(float y,float x)109 float fatan2_deg(float y, float x)
110 {
111 // check for zero x to avoid division by zero
112 if (x == 0.0F)
113 {
114 // return 90 deg for positive y
115 if (y > 0.0F) return 90.0F;
116
117 // return -90 deg for negative y
118 if (y < 0.0F) return -90.0F;
119
120 // otherwise y= 0.0 and return 0 deg (invalid arguments)
121 return 0.0F;
122 }
123
124 // from here onwards, x is guaranteed to be non-zero
125 // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
126 if (x > 0.0F) return (fatan_deg(y / x));
127
128 // compute atan2 for quadrant 2 (90 to 180 deg)
129 if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x));
130
131 // compute atan2 for quadrant 3 (-180 to -90 deg)
132 return (-180.0F + fatan_deg(y / x));
133 }
134
135 // approximation to inverse tan function (deg) for x in range
136 // -tan(15 deg) to tan(15 deg) giving an output -15 deg <= angle <= 15 deg
137
138 // using modified Pade[3/2] approximation
fatan_15deg(float x)139 float fatan_15deg(float x)
140 {
141 float x2; // x^2
142 #define PADE_A 96.644395816F // theoretical Pade[3/2] value is 5/3*180/PI=95.49296
143 #define PADE_B 25.086941612F // theoretical Pade[3/2] value is 4/9*180/PI=25.46479
144 #define PADE_C 1.6867633134F // theoretical Pade[3/2] value is 5/3=1.66667
145 // compute the approximation to the inverse tangent
146 // the function is anti-symmetric as required for positive and negative arguments
147 x2 = x * x;
148 return (x * (PADE_A + x2 * PADE_B) / (PADE_C + x2));
149 }
150
