Diff: RobotLeg.cpp

Revision:

12:a952bd74d363

Parent:

11:9ee0214bd410

Child:

13:1c5d255835ce

--- a/RobotLeg.cpp	Wed May 15 17:22:22 2013 +0000
+++ b/RobotLeg.cpp	Tue May 21 04:27:58 2013 +0000
@@ -76,8 +76,8 @@
     // Calculate new angles
     L = sqrt(dest.x*dest.x + dest.y*dest.y - c*c) - d;
     thetaAngle = atan2( ((L + d)*dest.y - c*dest.x), ((L + d)*dest.x + c*dest.y) ) - oth;
-    phiAngle = atan2(dest.z, L) + acos((a*a + L*L + dest.z*dest.z - b*b)/(2.f*a*sqrt(L*L + dest.z*dest.z))) - oph;
-    psiAngle = acos((a*a + b*b - L*L - dest.z*dest.z)/(2*a*b)) - ops - pi2;
+    phiAngle = atan2(dest.z, L) + acos((a*a + L*L + dest.z*dest.z - b*b)/(2.0f*a*sqrt(L*L + dest.z*dest.z))) - oph;
+    psiAngle = acos((a*a + b*b - L*L - dest.z*dest.z)/(2.0f*a*b)) - ops - pi2;
     
     // Convert radians to degrees
     thetaAngle *= rad2deg;
@@ -138,7 +138,8 @@
         // Calculate new position and position delta
         newPosition = deltaTransform*position;
         newNDeltaPosition = position - newPosition;
-        if (fabs(newNDeltaPosition.x) > eps || fabs(newNDeltaPosition.y) > eps || fabs(newNDeltaPosition.z) > eps)
+        newNDeltaPosition.z = 0.0f;
+        if (fabs(newNDeltaPosition.x) > eps || fabs(newNDeltaPosition.y) > eps)
             nDeltaPosition = newNDeltaPosition;
         
         // Check if new position is outside the step circle
@@ -170,6 +171,7 @@
         return true;
     }
     
+    // Should never reach here
     return false;
 }