-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathnustdfuncs.c
104 lines (87 loc) · 2.71 KB
/
nustdfuncs.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
#include "nustdfuncs.h"
#include <nusys.h>
float fabsf(float x) {
if (x < 0) {
return -x;
}
return x;
}
// Copied from the nustd library
int strcmp(const char *str1, const char *str2)
{
unsigned char c1 = 1,c2;
while(c1) {
c1=*str1++;
c2=*str2++;
if (c1 != c2) return c1 - c2;
}
return 0;
}
size_t _nstrlen(const char *string)
{
size_t len = 0;
while(*string++) len++;
return len;
}
char *strncpy(char *str1, const char *str2, size_t n)
{
char *p = str1;
while((*str2) && n--) *p++ = *str2++;
*p = '\0';
return str1;
}
/* copy-paste from the nusnake example */
#define NRECTAB 100
static float reciprocal_table_f[NRECTAB] = {
0,1.0/1.0,1.0/2.0,1.0/3.0,1.0/4.0,1.0/5.0,1.0/6.0,1.0/7.0,1.0/8.0,1.0/9.0,1.0/10.0,
1.0/11.0,1.0/12.0,1.0/13.0,1.0/14.0,1.0/15.0,1.0/16.0,1.0/17.0,1.0/18.0,1.0/19.0,1.0/20.0,
1.0/21.0,1.0/22.0,1.0/23.0,1.0/24.0,1.0/25.0,1.0/26.0,1.0/27.0,1.0/28.0,1.0/29.0,1.0/30.0,
1.0/31.0,1.0/32.0,1.0/33.0,1.0/34.0,1.0/35.0,1.0/36.0,1.0/37.0,1.0/38.0,1.0/39.0,1.0/40.0,
1.0/41.0,1.0/42.0,1.0/43.0,1.0/44.0,1.0/45.0,1.0/46.0,1.0/47.0,1.0/48.0,1.0/49.0,1.0/50.0,
1.0/51.0,1.0/52.0,1.0/53.0,1.0/54.0,1.0/55.0,1.0/56.0,1.0/57.0,1.0/58.0,1.0/59.0,1.0/60.0,
1.0/61.0,1.0/62.0,1.0/63.0,1.0/64.0,1.0/65.0,1.0/66.0,1.0/67.0,1.0/68.0,1.0/69.0,1.0/70.0,
1.0/71.0,1.0/72.0,1.0/73.0,1.0/74.0,1.0/75.0,1.0/76.0,1.0/77.0,1.0/78.0,1.0/79.0,1.0/80.0,
1.0/81.0,1.0/82.0,1.0/83.0,1.0/84.0,1.0/85.0,1.0/86.0,1.0/87.0,1.0/88.0,1.0/89.0,1.0/90.0,
1.0/91.0,1.0/92.0,1.0/93.0,1.0/94.0,1.0/95.0,1.0/96.0,1.0/97.0,1.0/89.0,1.0/99.0
};
float atan2bodyf(float y,float x)
{
float arg,ys,old;
float power, term, sum, z;
int i;
if ( y == 0.0f )
return 0.0f;
if ( x == 0.0f )
return (y > 0.0f)? (float)M_PI_2 : -(float)M_PI_2;
arg = y / x;
if ( arg == 1.0f )
return (y > 0.0f)? (float)M_PI_4 : -3.0f * (float)M_PI_4;
if ( arg == -1.0f )
return (x > 0.0f) ? -(float)M_PI_4 : 3.0f * (float)M_PI_4;
if ( arg > 1.0f || arg < -1.0f) {
sum = atan2bodyf(x, y);
if( x > 0.0f )
return (float)M_PI_2 - sum;
else
return (y > 0.0f) ? (float)M_PI_2 - sum: -3.0f * (float)M_PI_2 -sum;
}
ys = arg * arg;
old = 1.0f / (1.0f + ys);
z = ys * old;
sum = 1.0f;
i = 4;
power = z * 2.0f / 3.0f;
while( i < NRECTAB-1) {
term = power;
sum += term;
if ( term >= -TOL && term <= TOL) break;
power *= ( z * (float)i * reciprocal_table_f[i+1] );
i += 2;
}
sum *= arg * old;
return ( x > 0.0f )? sum : ( y > 0.0f ) ? (float)M_PI + sum :-(float)M_PI + sum;
}
float nu_atan2(float y, float x)
{
return atan2bodyf(y, x);
}