diff --git a/src/walker.cc b/src/walker.cc index 44bba1b..eaa2922 100644 --- a/src/walker.cc +++ b/src/walker.cc @@ -1,9 +1,13 @@ #include "walker.h" +// standard library includes +#include + // third party includes #include // 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, leg_ne, 1.0F, WHITE); - DrawModel(model, leg_sw, 1.0F, WHITE); - DrawModel(model, leg_se, 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_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; - target_body_pos.y += body_height; - flags &= ~1; + if (!Vector3Equals(target_body_pos, pos)) { + target_body_pos = pos; + 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; + } } diff --git a/src/walker.h b/src/walker.h index 9c38a2f..95b77dc 100644 --- a/src/walker.h +++ b/src/walker.h @@ -3,11 +3,17 @@ // standard library includes #include +#ifndef NDEBUG +#include +#endif // third party includes #include #include +// 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 @@ -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, - Vector3 &leg_pos, - unsigned int &flags, - unsigned int grounded_count) { + const Matrix rotationMatrix = get_rotation_matrix_about_y(rotation); + const auto update_leg_fn = [this, &bbs, dt, &rotationMatrix]( + Vector3 &leg_target, Vector3 &leg_pos, + 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