WIP: robot movements, adapt Helicopter view to new code

Misc fixes: fix up vector when changing to default view, fix wrong
scale for getPointForLane, fix comment typo in
Robot.thisFunctionDoesAbsolutelyNothing.

New bugs: robot does not look forward; to be done: do not use
static speed.

4 files changed, 129 insertions, 48 deletions

diff --git a/src/Camera.java b/src/Camera.java
index c0ea5cd..fd29482 100644
--- a/src/Camera.java
+++ b/src/Camera.java
@@ -17,9 +17,6 @@ class Camera {
     /** The up vector. */
     public Vector up = Vector.Z;
     
-    /** The array with robot positions. */
-    private double[] robotPositions;
-    
     /** Race track used. */
     private RaceTrack track;
 
@@ -28,11 +25,15 @@ class Camera {
      */
     private final GlobalState gs;
 
-    public Camera(GlobalState gs, double[] positions, RaceTrack track) {
-        // Set the global state and the robot positions and track
+    /**
+     * Robots that are to be tracked by the camera.
+     */
+    private final Robot[] robots;
+
+    public Camera(GlobalState gs, RaceTrack track, Robot[] robots) {
         this.gs = gs;
-        this.robotPositions = positions;
         this.track = track;
+        this.robots = robots;
     }
 
     /**
@@ -90,6 +91,9 @@ class Camera {
         Cz = gs.cnt.z();
         center = new Vector(Cx, Cy, Cz);
         eye = eye.add(center);
+
+        // just look straight forward
+        up = Vector.Z;
     }
 
     /**
@@ -97,29 +101,25 @@ class Camera {
      * on the helicopter mode.
      */
     private void setHelicopterMode() {
-        // Choose a robot to view
-        int robot = 0;
-        
-        /*
-         * First get the inner track position of a robot, then multiply this
-         * vector so that we have the actual position on the track. 
-         * 
-         * Add this lane position to the start position and we have the actual
-         * robot position. 
+        /**
+         * In the Helicopter view, the camera (eye point) is located above the
+         * robots. Take the average time of the robots and use that to determine
+         * the position and direction.
          */
-        Vector startPosition = track.getPoint(robotPositions[robot]);
-        Vector lanePosition = new Vector(startPosition.x(), startPosition.y(), 0)
-                .normalized().scale(robot + 1);
-        Vector robotPosition = startPosition.add(lanePosition);
-        
-        // Set the up vector to equal the tangent of the robot
-        up = track.getTangent(robotPositions[robot]);
-        
-        // Set the center point to the actual robot position. 
-        center = robotPosition;
-        
-        // Set the eye point to the center point, then increased height
-        eye = new Vector(center.x(), center.y(), 10f);
+        double time_sum = 0;
+        for (Robot robot : robots) {
+            time_sum += robot.getTimePos();
+        }
+        double time_avg = time_sum / robots.length;
+
+        // center at the center lane
+        center = track.getPointForLane(time_avg, robots.length / 2 + .5);
+
+        // look in the direction where the robots walks, namely the tangent
+        up = track.getTangent(time_avg);
+
+        // "above" is 10 meters.
+        eye = center.add(new Vector(center.x(), center.y(), 10f));
     }
 
     /**
diff --git a/src/RaceTrack.java b/src/RaceTrack.java
index 012047a..630e58a 100644
--- a/src/RaceTrack.java
+++ b/src/RaceTrack.java
@@ -86,7 +86,7 @@ class RaceTrack extends BetterBase {
      */
     public Vector getPointForLane(double t, double laneNo) {
         Vector p = getPoint(t);
-        Vector lanes_len = new Vector(p.x(), p.y(), 0).normalized().scale(4);
+        Vector lanes_len = new Vector(p.x(), p.y(), 0).normalized().scale(laneNo);
         return p.add(lanes_len);
     }
 
diff --git a/src/Robot.java b/src/Robot.java
index 3fe4377..ba90dea 100644
--- a/src/Robot.java
+++ b/src/Robot.java
@@ -36,6 +36,16 @@ class Robot extends BetterBase {
     private boolean asStickFigure;
 
     /**
+     * Robot speed (on track) in meters per second.
+     */
+    private double speed;
+
+    /**
+     * Location of the robot on the track (in terms of GlobalState time).
+     */
+    private double robot_time_pos;
+
+    /**
      * Constructs the robot with initial parameters.
      */
     public Robot(Material material) {
@@ -308,8 +318,31 @@ class Robot extends BetterBase {
 
     /**
      * This function does nothing, its use is described at the caller in
-     * Rboot.draw().
+     * Robot.draw().
      */
     private void thisFunctionDoesAbsolutelyNothing() {
     }
+
+    /**
+     * Determine the location of the robot on the track.
+     * @return A positive number (time).
+     */
+    public double getTimePos() {
+        return robot_time_pos;
+    }
+
+    /**
+     * Set the speed (meters per second) for this robot.
+     */
+    public void setSpeed(double speed) {
+        this.speed = speed;
+    }
+
+    /**
+     * Move the robot with the number of seconds based on its current speed.
+     */
+    public void walkSome(double seconds) {
+        assert seconds >= 0 : "Robot cannot walk backwards!";
+        robot_time_pos += speed * seconds;
+    }
 }
diff --git a/src/RobotRace.java b/src/RobotRace.java
index fb4ab1e..aeddfba 100644
--- a/src/RobotRace.java
+++ b/src/RobotRace.java
@@ -114,6 +114,27 @@ public class RobotRace extends Base {
     private long last_frame_time;
 
     /**
+     * Last (Global State) time when the speed got calculated.
+     */
+    private double last_speed_update;
+
+    /**
+     * Last animation time when the position of robots were updated.
+     */
+    private double last_t;
+
+    /**
+     * Only recalculate the robot speed if at least this number of seconds has
+     * been elapsed.
+     */
+    private final static double SPEED_RECALC_INTERVAL = .500;
+
+    /**
+     * Desired average robot speed in meter per second.
+     */
+    private final static double TARGET_ROBOT_SPEED = .03;
+
+    /**
      * Constructs this robot race by initializing robots, camera, track, and
      * terrain.
      */
@@ -151,12 +172,12 @@ public class RobotRace extends Base {
         robots[3] = new Robot(Material.ORANGE
         /* add other parameters that characterize this robot */);
 
-        // Initialize the camera
-        camera = new Camera(gs);
-
         // Initialize the race track
         raceTrack = new RaceTrack();
 
+        // Initialize the camera
+        camera = new Camera(gs, raceTrack, robots);
+
         // Initialize the terrain
         terrain = new Terrain();
     }
@@ -350,30 +371,57 @@ public class RobotRace extends Base {
     }
 
     /**
+     * Periodically calculate the robot speed based and update robot position.
+     */
+    private void calculateRobotSpeedAndLocation() {
+        double current_t = gs.tAnim;
+        double last_speed_update_t_diff = current_t - last_speed_update;
+        if (last_speed_update_t_diff >= SPEED_RECALC_INTERVAL) {
+            for (Robot robot : robots) {
+                // smaller than 0 means that robot is faster than usual, equal
+                // to 0 means robot is matching expected and great means that
+                // we have a turtle.
+                double jitter = current_t - robot.getTimePos();
+                // TODO: do not use static speed
+                robot.setSpeed(TARGET_ROBOT_SPEED);
+            }
+            last_speed_update = current_t;
+        }
+        double t_diff = current_t - last_t;
+        // do not walk at every fart, that is expensive and inaccurate.
+        if (t_diff >= .010) {
+            for (Robot robot : robots) {
+                robot.walkSome(t_diff);
+            }
+            last_t = current_t;
+        }
+    }
+
+    /**
      * Draw all four robots in the robots array on the scene
      */
     private void drawRobots() {
-        // laziness, save current positions because of translations
-        gl.glPushMatrix();
-
-        // Place the robots on a line based on the current robot. Robots are
-        // drawn with a distance of 1 meter between the middle of each robot,
-        // from left to right (seen from the robot's POV). First, set the
-        // starting point in a way such that the robot is drawn left of the
-        // axis (for an even number of robots).
-        gl.glTranslatef(.5f - robots.length / 2, 0f, 0f);
+        // before repositioning the robots, change speed if needed
+        calculateRobotSpeedAndLocation();
 
         // Draw each robot on the X-axis
-        for (Robot robot: robots) {
+        for (int i = 0; i < robots.length; i++) {
+            gl.glPushMatrix();
+            Robot robot = robots[i];
+
+            // put robot on the lane, slightly rotated to look "forward"
+            Vector robotPos = raceTrack.getPointForLane(robot.getTimePos(), i);
+            gl.glTranslated(robotPos.x(), robotPos.y(), robotPos.z());
+            // FIXME: robot looks in wrong direction.
+            double angle = atan2(robotPos.y(), robotPos.x());
+            gl.glRotated(angle, 0, 0, 1);
+
             // Draw the current robot
             robot.draw(gs.showStick);
 
-            // Position the next robot 1 meter away from the current one.
-            gl.glTranslatef(1f, 0f, 0f);
+            // restore positions
+            gl.glPopMatrix();
         }
-
-        // restore positions
-        gl.glPopMatrix();
     }
 
     /**