Bayley Wang
/
foc-ed_in_the_bot_compact
robot
MathHelpers/MathHelpers.cpp
- Committer:
- bwang
- Date:
- 2018-11-13
- Revision:
- 252:38644631ed97
- Parent:
- 181:d3510c8beab6
File content as of revision 252:38644631ed97:
#include "MathHelpers.h" #include "math.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 constrain_norm(float *x, float *y, float coeffx, float coeffy, float max) { float norm = sqrtf(coeffx * *x * *x + coeffy * *y * *y); if (norm > max) { *x /= (norm / max); *y /= (norm / max); } return norm; } 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 update_filter(float old, float x, float str) { return str * old + (1.0f - str) * x; } 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);; } }