From 270de098944a128c8dcf17fb4a6f9e45e72fee20 Mon Sep 17 00:00:00 2001
From: Stephen Seo <seo.disparate@gmail.com>
Date: Fri, 11 Aug 2023 12:14:48 +0900
Subject: [PATCH] Impl. walker bobbing head on idle

---
 src/walker.cc |  9 +++++++--
 src/walker.h  | 22 ++++++++++++++++++++++
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/walker.cc b/src/walker.cc
index eaa2922..57bba25 100644
--- a/src/walker.cc
+++ b/src/walker.cc
@@ -31,7 +31,8 @@ Walker::Walker(float body_height, float body_feet_radius, float feet_radius)
       feet_radius(feet_radius),
       lift_start_y(0.0F),
       rotation(0.0F),
-      target_rotation(0.0F) {
+      target_rotation(0.0F),
+      body_idle_move_timer(0.0F) {
   const Vector3 nw = Vector3Normalize(Vector3{-1.0F, 0.0F, -1.0F});
   const Vector3 ne = Vector3Normalize(Vector3{1.0F, 0.0F, -1.0F});
   const Vector3 sw = Vector3Normalize(Vector3{-1.0F, 0.0F, 1.0F});
@@ -71,7 +72,11 @@ void Walker::draw(const Model &model) {
                   .boneCount = model.boneCount,
                   .bones = model.bones,
                   .bindPose = model.bindPose},
-            body_pos, 1.0F, WHITE);
+            Vector3{body_pos.x,
+                    body_pos.y + BODY_IDLE_MOVE_AMOUNT *
+                                     std::sin(body_idle_move_timer + PI),
+                    body_pos.z},
+            1.0F, WHITE);
 
   // draw legs
   DrawModel(Model{.transform = model.transform * rotationMatrix,
diff --git a/src/walker.h b/src/walker.h
index 95b77dc..a0016c8 100644
--- a/src/walker.h
+++ b/src/walker.h
@@ -23,6 +23,8 @@ constexpr float FEET_LIFT_SPEED = 5.5F;
 constexpr float FEET_HORIZ_MOVE_SPEED = 8.0F;
 constexpr float FEET_INIT_POS_VARIANCE_DIV = 3.0F;
 constexpr float BODY_ROTATION_SPEED = 1.0F;
+constexpr float BODY_IDLE_TIMER_RATE = 1.0F;
+constexpr float BODY_IDLE_MOVE_AMOUNT = 0.2F;
 
 class Walker {
  public:
@@ -64,6 +66,7 @@ class Walker {
   float lift_start_y;
   float rotation;
   float target_rotation;
+  float body_idle_move_timer;
 };
 
 template <typename TBBS>
@@ -251,6 +254,25 @@ void Walker::update(float dt, const TBBS &bbs, unsigned int width,
   update_leg_fn(target_leg_sw, leg_sw, sw_flags,
                 ((nw_flags & 7) == 1 ? 1 : 0) + ((ne_flags & 7) == 1 ? 1 : 0) +
                     ((se_flags & 7) == 1 ? 1 : 0));
+
+  if ((flags & 3) == 0) {
+    body_idle_move_timer += dt * BODY_IDLE_TIMER_RATE;
+    if (body_idle_move_timer > PI * 2.0F) {
+      body_idle_move_timer -= PI * 2.0F;
+    }
+  } else if (!FloatEquals(body_idle_move_timer, 0.0F)) {
+    if (body_idle_move_timer < PI) {
+      body_idle_move_timer += dt * BODY_IDLE_TIMER_RATE;
+      if (body_idle_move_timer > PI) {
+        body_idle_move_timer = 0;
+      }
+    } else {
+      body_idle_move_timer += dt * BODY_IDLE_TIMER_RATE;
+      if (body_idle_move_timer > PI * 2.0F) {
+        body_idle_move_timer = 0.0F;
+      }
+    }
+  }
 }
 
 #endif