Mercurial > tote
changeset 162:5acf28524e7f
Make IK robust against out of range movements
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Mon, 06 Jul 2015 10:00:37 +0200 |
| parents | 387236216765 |
| children | ebac7ad21078 |
| files | start/leg.ino |
| diffstat | 1 files changed, 4 insertions(+), 2 deletions(-) [+] |
line wrap: on
line diff
--- a/start/leg.ino Sat Jul 04 16:22:27 2015 +0200 +++ b/start/leg.ino Mon Jul 06 10:00:37 2015 +0200 @@ -21,7 +21,9 @@ a = abs(a); b = abs(b); c = abs(c); - if (a + b < c || a + c < b || b + c < a) { return NaN; } + c = min(c, a + b); + b = min(b, a + c); + a = min(a, b + c); return acos((a * a + b * b - c * c) / (2 * a * b)); } @@ -36,7 +38,7 @@ // Return true on success, and false if x and y are out of range. double f = _norm(x, y) - COXA; double d = _norm(f, z); - if (d > FEMUR + TIBIA) { return false; } + d = min(d, FEMUR + TIBIA); *hip = atan2(y, x); if (isnan(*hip)) { return false; } *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2(-z, f);
