Impl. rotation for Walker

src/walker.cc

@@ -1,9 +1,13 @@
 #include "walker.h"
 
+// standard library includes
+#include <cmath>
+
 // third party includes
 #include <raymath.h>
 
 // local includes
+#include "3d_helpers.h"
 #include "ems.h"
 
 Walker::Walker(float body_height, float body_feet_radius, float feet_radius)
@@ -25,7 +29,9 @@ Walker::Walker(float body_height, float body_feet_radius, float feet_radius)
       body_height(body_height),
       body_feet_radius(body_feet_radius),
       feet_radius(feet_radius),
-      lift_start_y(0.0F) {
+      lift_start_y(0.0F),
+      rotation(0.0F),
+      target_rotation(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});
@@ -54,18 +60,77 @@ Walker::Walker(float body_height, float body_feet_radius, float feet_radius)
 }
 
 void Walker::draw(const Model &model) {
+  const Matrix rotationMatrix = get_rotation_matrix_about_y(rotation);
   // draw body
-  DrawModel(model, body_pos, 1.0F, WHITE);
+  DrawModel(Model{.transform = model.transform * rotationMatrix,
+                  .meshCount = model.meshCount,
+                  .materialCount = model.materialCount,
+                  .meshes = model.meshes,
+                  .materials = model.materials,
+                  .meshMaterial = model.meshMaterial,
+                  .boneCount = model.boneCount,
+                  .bones = model.bones,
+                  .bindPose = model.bindPose},
+            body_pos, 1.0F, WHITE);
 
   // draw legs
-  DrawModel(model, leg_nw, 1.0F, WHITE);
+  DrawModel(Model{.transform = model.transform * rotationMatrix,
-  DrawModel(model, leg_ne, 1.0F, WHITE);
+                  .meshCount = model.meshCount,
-  DrawModel(model, leg_sw, 1.0F, WHITE);
+                  .materialCount = model.materialCount,
-  DrawModel(model, leg_se, 1.0F, WHITE);
+                  .meshes = model.meshes,
+                  .materials = model.materials,
+                  .meshMaterial = model.meshMaterial,
+                  .boneCount = model.boneCount,
+                  .bones = model.bones,
+                  .bindPose = model.bindPose},
+            leg_nw, 1.0F, WHITE);
+  DrawModel(Model{.transform = model.transform * rotationMatrix,
+                  .meshCount = model.meshCount,
+                  .materialCount = model.materialCount,
+                  .meshes = model.meshes,
+                  .materials = model.materials,
+                  .meshMaterial = model.meshMaterial,
+                  .boneCount = model.boneCount,
+                  .bones = model.bones,
+                  .bindPose = model.bindPose},
+            leg_ne, 1.0F, WHITE);
+  DrawModel(Model{.transform = model.transform * rotationMatrix,
+                  .meshCount = model.meshCount,
+                  .materialCount = model.materialCount,
+                  .meshes = model.meshes,
+                  .materials = model.materials,
+                  .meshMaterial = model.meshMaterial,
+                  .boneCount = model.boneCount,
+                  .bones = model.bones,
+                  .bindPose = model.bindPose},
+            leg_sw, 1.0F, WHITE);
+  DrawModel(Model{.transform = model.transform * rotationMatrix,
+                  .meshCount = model.meshCount,
+                  .materialCount = model.materialCount,
+                  .meshes = model.meshes,
+                  .materials = model.materials,
+                  .meshMaterial = model.meshMaterial,
+                  .boneCount = model.boneCount,
+                  .bones = model.bones,
+                  .bindPose = model.bindPose},
+            leg_se, 1.0F, WHITE);
 }
 
 void Walker::set_body_pos(Vector3 pos) {
-  target_body_pos = pos;
+  if (!Vector3Equals(target_body_pos, pos)) {
-  target_body_pos.y += body_height;
+    target_body_pos = pos;
-  flags &= ~1;
+    target_body_pos.y += body_height;
+
+    Vector3 direction = Vector3Subtract(pos, body_pos);
+    target_rotation = std::atan2(-direction.z, direction.x);
+    while (target_rotation < 0.0F) {
+      target_rotation += PI * 2.0F;
+    }
+    while (target_rotation > PI * 2.0F) {
+      target_rotation -= PI * 2.0F;
+    }
+
+    flags &= ~3;
+    flags |= 1;
+  }
 }

src/walker.h

@@ -3,11 +3,17 @@
 
 // standard library includes
 #include <array>
+#ifndef NDEBUG
+#include <iostream>
+#endif
 
 // third party includes
 #include <raylib.h>
 #include <raymath.h>
 
+// local includes
+#include "3d_helpers.h"
+
 constexpr float FEET_RADIUS_PLACEMENT_CHECK_SCALE = 1.0F;
 constexpr float FEET_RADIUS_PLACEMENT_SCALE = 0.9F;
 constexpr float FEET_TARGET_RATE = 1.0F;
@@ -16,6 +22,7 @@ constexpr float FEET_LIFT_HEIGHT = 1.0F;
 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;
 
 class Walker {
  public:
@@ -46,13 +53,17 @@ class Walker {
   // ???1 0??? - is sw
   // ???1 1??? - is se
   unsigned int nw_flags, ne_flags, sw_flags, se_flags;
-  // ???? ???1 - body stopped
+  // ???? ??00 - body stopped
+  // ???? ??01 - rotating to move
+  // ???? ??10 - moving
   unsigned int flags;
 
   const float body_height;
   const float body_feet_radius;
   const float feet_radius;
   float lift_start_y;
+  float rotation;
+  float target_rotation;
 };
 
 template <typename TBBS>
@@ -85,24 +96,56 @@ void Walker::update(float dt, const TBBS &bbs, unsigned int width,
     se_flags |= 1;
   }
 
+  // body rotation
+  if ((flags & 3) == 1) {
+    float diff = target_rotation - rotation;
+    if (diff > PI) {
+      rotation -= dt * BODY_ROTATION_SPEED;
+      if (rotation < 0.0F) {
+        rotation += PI * 2.0F;
+      }
+    } else if (diff < -PI) {
+      rotation += dt * BODY_ROTATION_SPEED;
+      if (rotation > PI * 2.0F) {
+        rotation -= PI * 2.0F;
+      }
+    } else if (diff > 0.0F) {
+      rotation += dt * BODY_ROTATION_SPEED;
+      if (rotation > PI * 2.0F) {
+        rotation -= PI * 2.0F;
+      }
+    } else {
+      rotation -= dt * BODY_ROTATION_SPEED;
+      if (rotation < 0.0F) {
+        rotation += PI * 2.0F;
+      }
+    }
+
+    if (std::abs(target_rotation - rotation) < dt * BODY_ROTATION_SPEED) {
+      rotation = target_rotation;
+      flags &= ~3;
+      flags |= 2;
+    }
+  }
   // body to target pos
-  if ((flags & 1) == 0) {
+  if ((flags & 3) == 2) {
     float diff = Vector3Distance(target_body_pos, body_pos);
     body_pos =
         Vector3Add(body_pos, Vector3Scale(Vector3Normalize(Vector3Subtract(
                                               target_body_pos, body_pos)),
                                           dt * BODY_TARGET_SPEED));
     if (Vector3Distance(target_body_pos, body_pos) > diff) {
-      flags |= 1;
+      flags &= ~3;
       body_pos = target_body_pos;
     }
   }
 
   // moving legs
-  const auto update_leg_fn = [this, &bbs, dt](Vector3 &leg_target,
+  const Matrix rotationMatrix = get_rotation_matrix_about_y(rotation);
-                                              Vector3 &leg_pos,
+  const auto update_leg_fn = [this, &bbs, dt, &rotationMatrix](
-                                              unsigned int &flags,
+                                 Vector3 &leg_target, Vector3 &leg_pos,
-                                              unsigned int grounded_count) {
+                                 unsigned int &flags,
+                                 unsigned int grounded_count) {
     if ((flags & 7) == 1 && grounded_count > 1) {
       // Grounded.
       bool should_lift = false;
@@ -111,16 +154,20 @@ void Walker::update(float dt, const TBBS &bbs, unsigned int width,
       Vector3 ideal_foot_pos;
       if ((flags & 0x18) == 0) {
         // Is nw.
-        ideal_foot_pos = Vector3Normalize(Vector3{-1.0F, 0.0F, -1.0F});
+        ideal_foot_pos = Vector3Transform(
+            Vector3Normalize(Vector3{-1.0F, 0.0F, -1.0F}), rotationMatrix);
       } else if ((flags & 0x18) == 0x8) {
         // Is ne.
-        ideal_foot_pos = Vector3Normalize(Vector3{1.0F, 0.0F, -1.0F});
+        ideal_foot_pos = Vector3Transform(
+            Vector3Normalize(Vector3{1.0F, 0.0F, -1.0F}), rotationMatrix);
       } else if ((flags & 0x18) == 0x10) {
         // Is sw.
-        ideal_foot_pos = Vector3Normalize(Vector3{-1.0F, 0.0F, 1.0F});
+        ideal_foot_pos = Vector3Transform(
+            Vector3Normalize(Vector3{-1.0F, 0.0F, 1.0F}), rotationMatrix);
       } else if ((flags & 0x18) == 0x18) {
         // Is se.
-        ideal_foot_pos = Vector3Normalize(Vector3{1.0F, 0.0F, 1.0F});
+        ideal_foot_pos = Vector3Transform(
+            Vector3Normalize(Vector3{1.0F, 0.0F, 1.0F}), rotationMatrix);
       }
       ideal_foot_pos =
           Vector3Add(body_pos_same_y,
@@ -204,24 +251,6 @@ 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));
-
-  // legs to target pos
-  //  leg_nw =
-  //      Vector3Add(leg_nw, Vector3Scale(Vector3Subtract(target_leg_nw,
-  //      leg_nw),
-  //                                      dt * FEET_TARGET_RATE));
-  //  leg_ne =
-  //      Vector3Add(leg_ne, Vector3Scale(Vector3Subtract(target_leg_ne,
-  //      leg_ne),
-  //                                      dt * FEET_TARGET_RATE));
-  //  leg_sw =
-  //      Vector3Add(leg_sw, Vector3Scale(Vector3Subtract(target_leg_sw,
-  //      leg_sw),
-  //                                      dt * FEET_TARGET_RATE));
-  //  leg_se =
-  //      Vector3Add(leg_se, Vector3Scale(Vector3Subtract(target_leg_se,
-  //      leg_se),
-  //                                      dt * FEET_TARGET_RATE));
 }
 
 #endif