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);