Bayley Wang
/
foc-ed_in_the_bot_compact
robot
MathHelpers/MathHelpers.cpp
- Committer:
- bwang
- Date:
- 2016-12-03
- Revision:
- 39:80b38a8e1787
- Parent:
- 38:07cb4ae6c1bd
- Child:
- 64:b4175385d718
File content as of revision 39:80b38a8e1787:
#include "MathHelpers.h" float map(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } float constrain(float in, float min, float max) { if(in > max) return max; if(in < min) return min; return in; } float fminf(float a, float b) { if(a < b) return a; return b; } 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 aatan2(float q, float d) { 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);; } }