Diff: RobotLeg.cpp

Revision:

10:dc1ba352667e

Parent:

9:a6d1502f0f20

Child:

11:9ee0214bd410

--- a/RobotLeg.cpp	Thu Jan 31 23:51:15 2013 +0000
+++ b/RobotLeg.cpp	Tue Apr 09 01:36:50 2013 +0000
@@ -128,25 +128,31 @@
 
 bool RobotLeg::update(const matrix4& deltaTransform)
 {
-    float t;
-    vector3 newPosition, newNDeltaPosition;
+    float t, d;
+    vector3 newPosition, newNDeltaPosition, v;
     const float eps = 0.000001f;
 
     switch (state)
     {
     case neutral:
+        // 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)
             nDeltaPosition = newNDeltaPosition;
         position = newPosition;
-        if (!move(position))
-        {
-            return false;
-        }
+        
+        // Check if new position is outside the step circle
+        v = position - circleCenter;
+        d = sqrt(v.x*v.x + v.y*v.y);
+        
+        // Attempt to move to the new position
+        if (!move(position) || d > circleRadius) return false;
+        
         break;
         
     case stepping:
+    // Compute new position along step trajectory
         t = stepTimer.read();
         newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
         newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));