Initial working implementation of SmarterWalkAnimation

4 files changed, 212 insertions, 5 deletions

diff --git a/src/DumbWalkAnimation.java b/src/DumbWalkAnimation.java
index 7ff44d1..5dc2ac1 100644
--- a/src/DumbWalkAnimation.java
+++ b/src/DumbWalkAnimation.java
@@ -8,10 +8,16 @@
 public class DumbWalkAnimation implements WalkAnimation {
 
     private double robot_pos_meters;
+    private final double legLength;
+
+    DumbWalkAnimation(float legTopLength, float legBottomLength) {
+        this.legLength = legTopLength + legBottomLength;
+    }
 
     /**
      * Sets the new position for the robot.
      */
+    @Override
     public void updatePosition(double pos) {
         this.robot_pos_meters = pos;
     }
@@ -35,6 +41,7 @@ public class DumbWalkAnimation implements WalkAnimation {
         return 75.0 + Math.abs(Math.cos(getTime()) * 90.0);
     }
 
+    @Override
     public double getKneeAngleRight() {
         return getKneeAngleLeft();
     }
@@ -50,4 +57,9 @@ public class DumbWalkAnimation implements WalkAnimation {
         // static non-moving arms.
         return 0;
     }
+
+    @Override
+    public double getBottomOffset() {
+        return legLength;
+    }
 }
diff --git a/src/Robot.java b/src/Robot.java
index 5e24a36..612ecec 100644
--- a/src/Robot.java
+++ b/src/Robot.java
@@ -87,7 +87,8 @@ class Robot extends BetterBase {
         this.boneSize = 0.02f;
         this.depth = 0.24f;
         this.laneNo = laneNo;
-        this.walkAnim = new DumbWalkAnimation();
+        this.walkAnim = new SmarterWalkAnimation(legLength / 2,
+                legLength / 2 + footHeight);
     }
 
     /**
@@ -102,14 +103,16 @@ class Robot extends BetterBase {
         gl.glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, material.diffuse, 0);
         gl.glMaterialfv(GL_FRONT, GL_SPECULAR, material.specular, 0);
 
+        // calculate rotation angles and positions for movements.
+        walkAnim.updatePosition(robot_pos_meters);
+
         // save positions so it can be restored easily later
         gl.glPushMatrix();
         // position the center of the torso above the Z axis such that the foot
         // stands on the XY plane.
-        gl.glTranslatef(0, 0, torsoHeight / 2 + legLength + footHeight);
-
-        // calculate rotation angles and positions for movements.
-        walkAnim.updatePosition(robot_pos_meters);
+        gl.glTranslatef(0, 0, torsoHeight / 2);
+        // the length of the legs and foot are included.
+        gl.glTranslated(0, 0, walkAnim.getBottomOffset());
 
         // Draw the robot, everything is relative to the center of torso.
 
diff --git a/src/SmarterWalkAnimation.java b/src/SmarterWalkAnimation.java
new file mode 100644
index 0000000..9b4d7f0
--- /dev/null
+++ b/src/SmarterWalkAnimation.java
@@ -0,0 +1,185 @@
+
+import static java.lang.Math.*;
+
+/**
+ * A WalkAnimation that tries to be a bit more natural than the
+ * DumbWalkAnimation. It does so by making the foot follow a sine curve, from
+ * which the knee angles are also derived.
+ *
+ * @author Peter Wu
+ */
+public class SmarterWalkAnimation implements WalkAnimation {
+
+    /**
+     * Length of the upper leg from the groin to the knee.
+     */
+    private final float legTopLength;
+
+    /**
+     * Length of the lower leg from knee to the floor.
+     */
+    private final float legBottomLength;
+    /**
+     * Total length of the leg.
+     */
+    private final double legLength;
+
+    private final double cycleLength;
+    private final double MAX_LEG_ANGLE_DEG = 40.0;
+    /**
+     * Maximum percentage of the legs length to lift the feet.
+     */
+    private static final double FOOT_MAX_LIFT = .20;
+    /**
+     * Angles in radians.
+     */
+    private double leg_angle_left;
+    private double leg_angle_right;
+    private double knee_angle_left;
+    private double knee_angle_right;
+    private double bodyOffset;
+
+    SmarterWalkAnimation(float legTopLength, float legBottomLength) {
+        this.legTopLength = legTopLength;
+        this.legBottomLength = legBottomLength;
+        this.legLength = legTopLength + legBottomLength;
+
+        double maxLegAngle = MAX_LEG_ANGLE_DEG * PI / 180;
+        // single step = sin(maxLegAngle) * legsLength
+        cycleLength = 2 * sin(maxLegAngle) * (legTopLength + legBottomLength);
+        System.err.println("Cycle length (2 steps): " + cycleLength + " meter");
+    }
+
+    @Override
+    public void updatePosition(double pos) {
+        // adjust speed
+        pos /= cycleLength;
+        // whether the robot touches the ground with its left foot
+        boolean supported_by_left = pos % 2.0 <= 1.0;
+        double t = pos % 1.0;
+        assert t >= 0.0 : "Time went negative?!";
+
+        /**
+         * At t=0, the body is at x=0; the right foot at x=-1/4 and the left
+         * foot at x=1/4. During the transition from t=0 to t=1/2, the right
+         * foot accelerates to x=3/4 while the left foot stays at x=-1/4
+         * (something has to support the robot while it is moving...).
+         */
+        Point body = new Point(t / 2.0, 1.0);
+        Point foot_l = new Point(.25, 0.0);
+        Point foot_r = new Point(.25, 0.0);
+        double support_dist;
+        if (supported_by_left) {
+            foot_r.x = -.25 + t;
+            foot_r.y = FOOT_MAX_LIFT * sin(t * PI);
+            support_dist = foot_r.x - body.x;
+        } else {
+            foot_l.x = -.25 + t;
+            foot_l.y = FOOT_MAX_LIFT * sin(t * PI);
+            support_dist = foot_l.x - body.x;
+        }
+        // base the robot body bottom position on the foot that is supporting
+        // the robot.
+        body.y = sqrt(1.0 - support_dist * support_dist);
+
+        // distance between a bent leg and the body
+        double dist_leg_r, dist_leg_l;
+        dist_leg_l = legLength * distance(body, foot_l);
+        dist_leg_r = legLength * distance(body, foot_r);
+
+        // base rotation for legs
+        leg_angle_right = calcAngle(body, foot_r);
+        leg_angle_left = calcAngle(body, foot_l);
+        // bend the knees
+
+        knee_angle_right = cosineRule(legTopLength, legBottomLength, dist_leg_r);
+        knee_angle_left = cosineRule(legTopLength, legBottomLength, dist_leg_l);
+        // correct the foot position due to the bent leg
+        leg_angle_left += cosineRule(legTopLength, dist_leg_l, legBottomLength);
+        leg_angle_right += cosineRule(legTopLength, dist_leg_r, legBottomLength);
+
+        bodyOffset = legLength * body.y;
+    }
+
+    @Override
+    public double getLegAngleLeft() {
+        return leg_angle_left * 180 / PI;
+    }
+
+    @Override
+    public double getLegAngleRight() {
+        return leg_angle_right * 180 / PI;
+    }
+
+    @Override
+    public double getKneeAngleLeft() {
+        return knee_angle_left * 180 / PI;
+    }
+
+    @Override
+    public double getKneeAngleRight() {
+        return knee_angle_right * 180 / PI;
+    }
+
+    @Override
+    public double getArmAngleLeft() {
+        return 0;
+    }
+
+    @Override
+    public double getArmAngleRight() {
+        return 0;
+    }
+
+    @Override
+    public double getBottomOffset() {
+        return bodyOffset;
+    }
+
+    /*-
+     * Given a trangle:
+     * y|      a   a
+     *  |    / |   | \
+     *  |  /___|   |___\
+     *  |  b  (c) (c)   b
+     * -+------------------- x
+     * calculate angle a for length bc and ac. In the left case, the
+     * returned angle (in radians) is negative.
+     */
+    private static double calcAngle(Point a, Point b) {
+        double ac = a.y - b.y;
+        double cb = b.x - a.x;
+        return atan(cb / ac);
+    }
+
+    /**
+     * Calculates the distance between two points.
+     */
+    private static double distance(Point a, Point b) {
+        double side1 = a.x - b.x;
+        double side2 = a.y - b.y;
+        return sqrt(side1 * side1 + side2 * side2);
+    }
+
+    /**
+     * Given a triangle with sides a, b and c, calculate the angle opposite to
+     * side c.
+     *
+     * @return Angle opposed to side c in radians.
+     */
+    private static double cosineRule(double a, double b, double c) {
+        // cosine rule: angle c = arccos( (aa + bb - cc) / 2ab )
+        return acos(((a * a) + (b * b) - (c * c)) / (2 * a * b));
+    }
+
+    private class Point {
+
+        private double x;
+        private double y;
+
+        Point(double x, double y) {
+            this.x = x;
+            this.y = y;
+        }
+    }
+}
diff --git a/src/WalkAnimation.java b/src/WalkAnimation.java
index f66e7df..33cbce1 100644
--- a/src/WalkAnimation.java
+++ b/src/WalkAnimation.java
@@ -62,4 +62,11 @@ interface WalkAnimation {
      * @return angle in degrees.
      */
     public double getArmAngleRight();
+
+    /**
+     * Finds the distance between the floor and the bottom of the body.
+     *
+     * @return A distance in meters.
+     */
+    public double getBottomOffset();
 }