Bayley Wang
/
foc-ed_in_the_bot_compact
robot
MathHelpers/MathHelpers.cpp@39:80b38a8e1787, 2016-12-03 (annotated)
- Committer:
- bwang
- Date:
- Sat Dec 03 17:49:02 2016 +0000
- Revision:
- 39:80b38a8e1787
- Parent:
- 38:07cb4ae6c1bd
- Child:
- 64:b4175385d718
lastest updates; loads of stubs for broken FW and optimization functions
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 19:a6cf15f89f3d | 1 | #include "MathHelpers.h" |
bwang | 19:a6cf15f89f3d | 2 | |
bwang | 19:a6cf15f89f3d | 3 | float map(float x, float in_min, float in_max, float out_min, float out_max) |
bwang | 19:a6cf15f89f3d | 4 | { |
bwang | 19:a6cf15f89f3d | 5 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
bwang | 19:a6cf15f89f3d | 6 | } |
bwang | 19:a6cf15f89f3d | 7 | |
bwang | 19:a6cf15f89f3d | 8 | float constrain(float in, float min, float max) |
bwang | 19:a6cf15f89f3d | 9 | { |
bwang | 19:a6cf15f89f3d | 10 | if(in > max) return max; |
bwang | 19:a6cf15f89f3d | 11 | if(in < min) return min; |
bwang | 19:a6cf15f89f3d | 12 | return in; |
bwang | 19:a6cf15f89f3d | 13 | } |
bwang | 19:a6cf15f89f3d | 14 | |
bwang | 19:a6cf15f89f3d | 15 | float fminf(float a, float b) { |
bwang | 19:a6cf15f89f3d | 16 | if(a < b) return a; |
bwang | 19:a6cf15f89f3d | 17 | return b; |
bwang | 19:a6cf15f89f3d | 18 | } |
bwang | 19:a6cf15f89f3d | 19 | |
bwang | 19:a6cf15f89f3d | 20 | float fmaxf(float a, float b) { |
bwang | 19:a6cf15f89f3d | 21 | if(a > b) return a; |
bwang | 19:a6cf15f89f3d | 22 | return b; |
bwang | 38:07cb4ae6c1bd | 23 | } |
bwang | 38:07cb4ae6c1bd | 24 | |
bwang | 38:07cb4ae6c1bd | 25 | float acbrt(float x0) { |
bwang | 38:07cb4ae6c1bd | 26 | union { int ix; float x; }; |
bwang | 38:07cb4ae6c1bd | 27 | |
bwang | 38:07cb4ae6c1bd | 28 | x = x0; |
bwang | 38:07cb4ae6c1bd | 29 | ix = 0x2a5137a0 + ix / 3; |
bwang | 38:07cb4ae6c1bd | 30 | x = 0.33333333f*(2.0f*x + x0 / (x * x)); |
bwang | 38:07cb4ae6c1bd | 31 | |
bwang | 38:07cb4ae6c1bd | 32 | return x; |
bwang | 38:07cb4ae6c1bd | 33 | } |
bwang | 38:07cb4ae6c1bd | 34 | |
bwang | 39:80b38a8e1787 | 35 | float aatan2(float q, float d) { |
bwang | 38:07cb4ae6c1bd | 36 | if (q > 0.f && d < 0.f && -d < q) { |
bwang | 38:07cb4ae6c1bd | 37 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 38 | } |
bwang | 38:07cb4ae6c1bd | 39 | else if (q > 0.f && d < 0.f && -d > q) { |
bwang | 38:07cb4ae6c1bd | 40 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 41 | } |
bwang | 38:07cb4ae6c1bd | 42 | else if (q < 0.f && d < 0.f && -d > -q) { |
bwang | 38:07cb4ae6c1bd | 43 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q); |
bwang | 38:07cb4ae6c1bd | 44 | } |
bwang | 38:07cb4ae6c1bd | 45 | else if (q < 0.f && d < 0.f && -d < -q) { |
bwang | 38:07cb4ae6c1bd | 46 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 47 | } |
bwang | 38:07cb4ae6c1bd | 48 | else if (q < 0.f && d > 0.f && d < -q) { |
bwang | 38:07cb4ae6c1bd | 49 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 50 | } |
bwang | 38:07cb4ae6c1bd | 51 | else if (q < 0.f && d > 0.f && d > -q) { |
bwang | 38:07cb4ae6c1bd | 52 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 53 | } |
bwang | 38:07cb4ae6c1bd | 54 | else if (q > 0.f && d > 0.f && d > q) { |
bwang | 38:07cb4ae6c1bd | 55 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 56 | } |
bwang | 38:07cb4ae6c1bd | 57 | else { |
bwang | 38:07cb4ae6c1bd | 58 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 59 | } |
bwang | 19:a6cf15f89f3d | 60 | } |