Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: MathHelpers/MathHelpers.cpp
- Revision:
- 38:07cb4ae6c1bd
- Parent:
- 19:a6cf15f89f3d
- Child:
- 39:80b38a8e1787
--- a/MathHelpers/MathHelpers.cpp Sun Nov 27 11:36:54 2016 +0000 +++ b/MathHelpers/MathHelpers.cpp Tue Nov 29 00:31:18 2016 +0000 @@ -20,4 +20,41 @@ float fmaxf(float a, float b) { if(a > b) return a; return b; +} + +float acbrt(float x0) { + union { int ix; float x; }; + + x = x0; + ix = 0x2a5137a0 + ix / 3; + x = 0.33333333f*(2.0f*x + x0 / (x * x)); + + return x; +} + +float aatan(float d, float q) { + if (q > 0.f && d < 0.f && -d < q) { + return d * q / (q * q + 0.28125f * d * d);; + } + else if (q > 0.f && d < 0.f && -d > q) { + return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; + } + else if (q < 0.f && d < 0.f && -d > -q) { + return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q); + } + else if (q < 0.f && d < 0.f && -d < -q) { + return d * q / (q * q + 0.28125f * d * d);; + } + else if (q < 0.f && d > 0.f && d < -q) { + return d * q / (q * q + 0.28125f * d * d);; + } + else if (q < 0.f && d > 0.f && d > -q) { + return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; + } + else if (q > 0.f && d > 0.f && d > q) { + return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; + } + else { + return d * q / (q * q + 0.28125f * d * d);; + } } \ No newline at end of file