From 13134f99cbf816303fa05e9027a64b45f087d2ea Mon Sep 17 00:00:00 2001 From: "C. J. Howard" Date: Fri, 4 Aug 2023 10:52:47 +0800 Subject: [PATCH] Add functions to convert between Euler angles and quaternions. Improve Euler IK constraint. Add gait and step classes to animation module. Improve locomotion system --- CMakeLists.txt | 1 - src/engine/ai/steering/behavior/wander.cpp | 3 +- .../ik/constraints/euler-ik-constraint.cpp | 37 +- .../ik/constraints/euler-ik-constraint.hpp | 54 +- .../constraints/swing-twist-ik-constraint.cpp | 25 +- .../constraints/swing-twist-ik-constraint.hpp | 6 +- src/engine/animation/ik/ik-goal.hpp | 89 +++ .../animation/ik/solvers/ccd-ik-solver.cpp | 22 +- .../animation/ik/solvers/ccd-ik-solver.hpp | 38 +- src/engine/animation/locomotion/gait.cpp | 27 + src/engine/animation/locomotion/gait.hpp | 48 ++ src/engine/animation/locomotion/step.cpp | 39 ++ src/engine/animation/locomotion/step.hpp | 45 ++ src/engine/gl/shader-template.cpp | 91 +-- src/engine/gl/shader-template.hpp | 42 +- src/engine/math/euler-angles.hpp | 523 +++++++++++++++++ src/engine/math/matrix.hpp | 6 +- src/engine/math/quaternion.hpp | 28 +- src/engine/math/se3.hpp | 11 - src/engine/math/vector.hpp | 6 +- src/engine/scene/skeletal-mesh.hpp | 6 + src/game/ant/ant-skeleton.cpp | 529 ++++++++++++++++++ .../legged-locomotion-component.hpp | 20 + .../treadmill-experiment-state.cpp | 77 ++- .../treadmill-experiment-state.hpp | 4 +- src/game/states/nest-selection-state.cpp | 2 +- src/game/systems/locomotion-system.cpp | 76 ++- 27 files changed, 1682 insertions(+), 173 deletions(-) create mode 100644 src/engine/animation/ik/ik-goal.hpp create mode 100644 src/engine/animation/locomotion/gait.cpp create mode 100644 src/engine/animation/locomotion/gait.hpp create mode 100644 src/engine/animation/locomotion/step.cpp create mode 100644 src/engine/animation/locomotion/step.hpp create mode 100644 src/engine/math/euler-angles.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 73e1a79..21ca183 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,5 @@ cmake_minimum_required(VERSION 3.25) - option(APPLICATION_NAME "Application name" "Antkeeper") option(APPLICATION_VERSION "Application version string" "0.0.0") option(APPLICATION_AUTHOR "Application author" "C. J. Howard") diff --git a/src/engine/ai/steering/behavior/wander.cpp b/src/engine/ai/steering/behavior/wander.cpp index 05c5a9b..7d9bb40 100644 --- a/src/engine/ai/steering/behavior/wander.cpp +++ b/src/engine/ai/steering/behavior/wander.cpp @@ -35,8 +35,7 @@ math::fvec3 wander_2d(const agent& agent, float noise, float distance, float rad const math::fvec3 center = agent.position + agent.forward * distance; // Decompose orientation into swing and twist rotations - math::fquat swing, twist; - math::swing_twist(agent.orientation, agent.up, swing, twist); + auto [swing, twist] = math::swing_twist(agent.orientation, agent.up); // Calculate offset to point on wander circle const math::fvec3 offset = math::conjugate(twist) * (math::angle_axis(angle, agent.up) * agent.forward * radius); diff --git a/src/engine/animation/ik/constraints/euler-ik-constraint.cpp b/src/engine/animation/ik/constraints/euler-ik-constraint.cpp index 1a8c1fb..2a4ca9d 100644 --- a/src/engine/animation/ik/constraints/euler-ik-constraint.cpp +++ b/src/engine/animation/ik/constraints/euler-ik-constraint.cpp @@ -23,31 +23,18 @@ void euler_ik_constraint::solve(math::fquat& q) { - // Derive XYZ angles from quaternion - const auto x = std::atan2(2.0f * (q.w() * q.x() + q.y() * q.z()), 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y())); - const auto y = std::asin(2.0f * (q.w() * q.y() - q.z() * q.x())); - const auto z = std::atan2(2.0f * (q.w() * q.z() + q.x() * q.y()), 1.0f - 2.0f * (q.y() * q.y() + q.z() * q.z())); + // Store w-component of quaternion + float old_w = q.w(); - // Constrain and halve angles - const auto half_constrained_x = std::min(std::max(x, m_min.x()), m_max.x()) * 0.5f; - const auto half_constrained_y = std::min(std::max(y, m_min.y()), m_max.y()) * 0.5f; - const auto half_constrained_z = std::min(std::max(z, m_min.z()), m_max.z()) * 0.5f; + // Derive Euler angles from quaternion + auto angles = math::euler_from_quat(m_rotation_sequence, q); - // Reconstruct quaternion from constrained, halved angles - const auto cx = std::cos(half_constrained_x); - const auto sx = std::sin(half_constrained_x); - const auto cy = std::cos(half_constrained_y); - const auto sy = std::sin(half_constrained_y); - const auto cz = std::cos(half_constrained_z); - const auto sz = std::sin(half_constrained_z); - q = math::normalize - ( - math::fquat - { - cx * cy * cz + sx * sy * sz, - sx * cy * cz - cx * sy * sz, - cx * sy * cz + sx * cy * sz, - cx * cy * sz - sx * sy * cz - } - ); + // Constrain Euler angles + angles = math::clamp(angles, m_min_angles, m_max_angles); + + // Rebuild quaternion from constrained Euler angles + q = math::euler_to_quat(m_rotation_sequence, angles); + + // Restore quaternion sign + q.w() = std::copysign(q.w(), old_w); } diff --git a/src/engine/animation/ik/constraints/euler-ik-constraint.hpp b/src/engine/animation/ik/constraints/euler-ik-constraint.hpp index bda4aa2..67fe74c 100644 --- a/src/engine/animation/ik/constraints/euler-ik-constraint.hpp +++ b/src/engine/animation/ik/constraints/euler-ik-constraint.hpp @@ -21,6 +21,7 @@ #define ANTKEEPER_ANIMATION_EULER_IK_CONSTRAINT_HPP #include +#include #include /** @@ -32,20 +33,57 @@ public: void solve(math::fquat& q) override; /** - * Sets the angular limits. + * Sets the rotation sequence of the Euler angles of the constraint. * - * @param min Minimum angles of the x-, y-, and z-axes, in radians. - * @param max Maximum angles of the x-, y-, and z-axes, in radians. + * @param sequence Euler angle rotation sequence. */ - inline void set_limits(const math::fvec3& min, const math::fvec3& max) noexcept + inline void set_rotation_sequence(math::rotation_sequence sequence) noexcept { - m_min = min; - m_max = max; + m_rotation_sequence = sequence; + } + + /** + * Sets the minimum Euler angles of the constraint. + * + * @param angles Minimum angles of the first, second, and third Euler angles, in radians. + */ + inline void set_min_angles(const math::fvec3& angles) noexcept + { + m_min_angles = angles; + } + + /** + * Sets the maximum Euler angles of the constraint. + * + * @param angles Maximum angles of the first, second, and third Euler angles, in radians. + */ + inline void set_max_angles(const math::fvec3& angles) noexcept + { + m_max_angles = angles; + } + + /// Returns the rotation sequence of the Euler angles of the constraint. + [[nodiscard]] inline constexpr math::rotation_sequence get_rotation_sequence() const noexcept + { + return m_rotation_sequence; + } + + /// Returns the minimum angles of the first, second, and third Euler angles, in radians. + [[nodiscard]] inline constexpr const math::fvec3& get_min_angles() const noexcept + { + return m_min_angles; + } + + /// Returns the maximum angles of the first, second, and third Euler angles, in radians. + [[nodiscard]] inline constexpr const math::fvec3& get_max_angles() const noexcept + { + return m_max_angles; } private: - math::fvec3 m_min{-math::pi, -math::pi, -math::pi}; - math::fvec3 m_max{ math::pi, math::pi, math::pi}; + math::rotation_sequence m_rotation_sequence{math::rotation_sequence::xyz}; + math::fvec3 m_min_angles{-math::pi, -math::pi, -math::pi}; + math::fvec3 m_max_angles{ math::pi, math::pi, math::pi}; }; #endif // ANTKEEPER_ANIMATION_EULER_IK_CONSTRAINT_HPP diff --git a/src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp index 3004843..773c6f3 100644 --- a/src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp +++ b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp @@ -20,12 +20,12 @@ #include #include -void swing_twist_ik_constraint::set_twist_limit(float angle_min, float angle_max) +void swing_twist_ik_constraint::set_twist_limit(float min_angle, float max_angle) { - m_cos_half_twist_min = std::cos(angle_min * 0.5f); - m_sin_half_twist_min = std::sin(angle_min * 0.5f); - m_cos_half_twist_max = std::cos(angle_max * 0.5f); - m_sin_half_twist_max = std::sin(angle_max * 0.5f); + m_cos_half_twist_min = std::cos(min_angle * 0.5f); + m_sin_half_twist_min = std::sin(min_angle * 0.5f); + m_cos_half_twist_max = std::cos(max_angle * 0.5f); + m_sin_half_twist_max = std::sin(max_angle * 0.5f); } void swing_twist_ik_constraint::solve(math::fquat& q) @@ -33,20 +33,21 @@ void swing_twist_ik_constraint::solve(math::fquat& q) constexpr math::fvec3 twist_axis{0.0f, 0.0f, 1.0f}; // Decompose rotation into swing and twist components - math::fquat swing; - math::fquat twist; - math::swing_twist(q, twist_axis, swing, twist); + auto [swing, twist] = math::swing_twist(q, twist_axis); + + // Limit swing + // ... // Limit twist - if (twist.z() < m_sin_half_twist_min) + if (twist.w() < m_cos_half_twist_min) { - twist.z() = m_sin_half_twist_min; twist.w() = m_cos_half_twist_min; + twist.z() = m_sin_half_twist_min; } - else if (twist.z() > m_sin_half_twist_max) + else if (twist.w() > m_cos_half_twist_max) { - twist.z() = m_sin_half_twist_max; twist.w() = m_cos_half_twist_max; + twist.z() = m_sin_half_twist_max; } // Re-compose rotation from swing and twist components diff --git a/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp index 9513209..b917e46 100644 --- a/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp +++ b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp @@ -34,10 +34,10 @@ public: /** * Sets the twist rotation limit. * - * @param angle_min Minimum twist angle, in radians. - * @param angle_max Maximum twist angle, in radians. + * @param min_angle Minimum twist angle, in radians. + * @param max_angle Maximum twist angle, in radians. */ - void set_twist_limit(float angle_min, float angle_max); + void set_twist_limit(float min_angle, float max_angle); private: float m_cos_half_twist_min{ 0}; diff --git a/src/engine/animation/ik/ik-goal.hpp b/src/engine/animation/ik/ik-goal.hpp new file mode 100644 index 0000000..9beacc4 --- /dev/null +++ b/src/engine/animation/ik/ik-goal.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_ANIMATION_IK_GOAL_HPP +#define ANTKEEPER_ANIMATION_IK_GOAL_HPP + +#include +#include +#include + +/** + * IK goal. + */ +class ik_goal +{ +public: + /** + * Sets the solver of the IK goal. + * + * @param solver Goal solver. + */ + inline void set_solver(std::shared_ptr solver) noexcept + { + m_solver = solver; + } + + /** + * Sets the center of the IK goal. + * + * @param center Goal center, in world-space. + */ + inline void set_center(const math::fvec3& center) noexcept + { + m_center = center; + } + + /** + * Sets the radius of the IK goal. + * + * @param radius Goal radius. + */ + inline void set_radius(float radius) noexcept + { + m_radius = radius; + m_sqr_radius = m_sqr_radius * m_sqr_radius; + } + + /// Returns the solver of the IK goal. + [[nodiscard]] inline const std::shared_ptr& get_solver() const noexcept + { + return m_solver; + } + + /// Returns the center of the IK goal. + [[nodiscard]] inline const math::fvec3& get_center() const noexcept + { + return m_center; + } + + /// Returns the radius of the IK goal. + [[nodiscard]] inline float get_radius() const noexcept + { + return m_radius; + } + +private: + std::shared_ptr m_solver; + math::fvec3 m_center{}; + float m_radius{1e-3f}; + float m_sqr_radius{1e-6f}; +}; + +#endif // ANTKEEPER_ANIMATION_IK_GOAL_HPP diff --git a/src/engine/animation/ik/solvers/ccd-ik-solver.cpp b/src/engine/animation/ik/solvers/ccd-ik-solver.cpp index 2af12ef..1724483 100644 --- a/src/engine/animation/ik/solvers/ccd-ik-solver.cpp +++ b/src/engine/animation/ik/solvers/ccd-ik-solver.cpp @@ -60,36 +60,36 @@ void ccd_ik_solver::solve() const auto& ps_effector_bone_transform = pose.get_absolute_transform(m_bone_indices.front()); // Transform goal position into pose-space - const auto ps_goal_position = m_goal_position * skeletal_mesh.get_transform(); + const auto ps_goal_center = m_goal_center * skeletal_mesh.get_transform(); for (std::size_t i = 0; i < m_max_iterations; ++i) { for (std::size_t j = 0; j < m_bone_indices.size(); ++j) { // Transform end effector position into pose-space - auto ps_effector_position = ps_effector_bone_transform * m_effector_position; + const auto ps_effector_position = ps_effector_bone_transform * m_effector_position; - // Check if end effector is within tolerable distance to goal position - const auto sqr_distance = math::sqr_distance(ps_effector_position, ps_goal_position); - if (sqr_distance <= m_sqr_distance_tolerance) + // Check if end effector is within goal radius + const auto sqr_distance = math::sqr_distance(ps_effector_position, ps_goal_center); + if (sqr_distance <= m_sqr_goal_radius) { return; } // Get index of current bone - bone_index_type bone_index = m_bone_indices[j]; + const auto bone_index = m_bone_indices[j]; // Get pose-space and bone-space transforms of current bone const auto& ps_bone_transform = pose.get_absolute_transform(bone_index); const auto& bs_bone_transform = pose.get_relative_transform(bone_index); - // Find pose-space direction vector from current bone to end effector + // Find pose-space direction vector from current bone to effector const auto ps_effector_direction = math::normalize(ps_effector_position - ps_bone_transform.translation); - // Find pose-space direction vector from current bone to IK goal - const auto ps_goal_direction = math::normalize(ps_goal_position - ps_bone_transform.translation); + // Find pose-space direction vector from current bone to center of goal + const auto ps_goal_direction = math::normalize(ps_goal_center - ps_bone_transform.translation); - // Calculate rotation of current bone that brings effector closer to goal + // Find rotation for current bone that brings effector closer to goal auto bone_rotation = math::normalize(math::rotation(ps_effector_direction, ps_goal_direction, 1e-5f) * bs_bone_transform.rotation); // Apply current bone constraints to rotation @@ -100,6 +100,8 @@ void ccd_ik_solver::solve() // Rotate current bone pose.set_relative_rotation(bone_index, bone_rotation); + + // Update pose //pose.update(bone_index, j + 1); pose.update(); } diff --git a/src/engine/animation/ik/solvers/ccd-ik-solver.hpp b/src/engine/animation/ik/solvers/ccd-ik-solver.hpp index fcf0dfd..a63114b 100644 --- a/src/engine/animation/ik/solvers/ccd-ik-solver.hpp +++ b/src/engine/animation/ik/solvers/ccd-ik-solver.hpp @@ -57,16 +57,6 @@ public: m_max_iterations = iterations; } - /** - * Sets the maximum tolerable distance between the goal and end effector. - * - * @param tolerance Tolerable distance between the goal and end effector. - */ - inline void set_distance_tolerance(float distance) noexcept - { - m_sqr_distance_tolerance = distance * distance; - } - /// Returns the maximum number of solving iterations. [[nodiscard]] inline std::size_t get_max_iterations() const noexcept { @@ -100,19 +90,29 @@ public: /// @{ /** - * Thes the position of the IK goal. + * Sets the center of the IK goal. + * + * @param center IK goal center, in world-space. + */ + inline void set_goal_center(const math::fvec3& center) noexcept + { + m_goal_center = center; + } + + /** + * Sets the radius of the IK goal. * - * @param position IK goal position, in world-space. + * @param radius IK goal radius. */ - inline void set_goal_position(const math::fvec3& position) noexcept + inline void set_goal_radius(float radius) noexcept { - m_goal_position = position; + m_sqr_goal_radius = radius * radius; } - /// Returns the position of goal, in world-space. - [[nodiscard]] inline const math::fvec3& get_goal_position() const + /// Returns the center of the IK goal, in world-space. + [[nodiscard]] inline const math::fvec3& get_goal_center() const { - return m_goal_position; + return m_goal_center; } /// @} @@ -122,8 +122,8 @@ private: std::size_t m_max_iterations{10}; std::vector m_bone_indices; math::fvec3 m_effector_position{0.0f, 0.0f, 0.0f}; - math::fvec3 m_goal_position{0.0f, 0.0f, 0.0f}; - float m_sqr_distance_tolerance{1e-5f}; + math::fvec3 m_goal_center{0.0f, 0.0f, 0.0f}; + float m_sqr_goal_radius{1e-5f}; }; #endif // ANTKEEPER_ANIMATION_CCD_IK_SOLVER_HPP diff --git a/src/engine/animation/locomotion/gait.cpp b/src/engine/animation/locomotion/gait.cpp new file mode 100644 index 0000000..0106d8f --- /dev/null +++ b/src/engine/animation/locomotion/gait.cpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#include +#include + +float gait::phase(float t) const noexcept +{ + float i; + return std::modf(t * frequency, &i); +} diff --git a/src/engine/animation/locomotion/gait.hpp b/src/engine/animation/locomotion/gait.hpp new file mode 100644 index 0000000..fb6d019 --- /dev/null +++ b/src/engine/animation/locomotion/gait.hpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_ANIMATION_GAIT_HPP +#define ANTKEEPER_ANIMATION_GAIT_HPP + +#include +#include + +/** + * Describes the synchronized timing of limbs in a locomotion pattern. + */ +struct gait +{ +public: + /// Frequency of the gait cycle, in Hz. + float frequency{}; + + /// Array of steps for each limb. + std::vector steps; + + /** + * Returns the phase of the gait at the elapsed time. + * + * @param t Elapsed time, in seconds. + * + * @return Gait phase, on `[0, 1]`. + */ + [[nodiscard]] float phase(float t) const noexcept; +}; + +#endif // ANTKEEPER_ANIMATION_GAIT_HPP diff --git a/src/engine/animation/locomotion/step.cpp b/src/engine/animation/locomotion/step.cpp new file mode 100644 index 0000000..cc40946 --- /dev/null +++ b/src/engine/animation/locomotion/step.cpp @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#include +#include + +float step::phase(float t) const noexcept +{ + // Make phase relative to step stance + float i; + t = std::modf(1.0f + t + delay - duty_factor, &i); + + if (t < duty_factor) + { + // Stance phase, on `[-1, 0)` + return (t - duty_factor) / duty_factor; + } + else + { + // Swing phase, on `[0, 1]` + return (t - duty_factor) / (1.0f - duty_factor); + } +} diff --git a/src/engine/animation/locomotion/step.hpp b/src/engine/animation/locomotion/step.hpp new file mode 100644 index 0000000..a94dd53 --- /dev/null +++ b/src/engine/animation/locomotion/step.hpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_ANIMATION_STEP_HPP +#define ANTKEEPER_ANIMATION_STEP_HPP + +/** + * Describes the timing of a single step in a gait. + */ +struct step +{ +public: + /// Fraction of the gait cycle, on `[0, 1]`, in which the limb is in the stance phase. + float duty_factor{}; + + /// Fraction of the gait cycle, on `[0, 1]`, at which the limb enters the swing phase. + float delay{}; + + /** + * Returns the phase of the step at the given gait phase. + * + * @param t Gait phase, on `[0, 1]`. + * + * @return Step phase, on `[-1, 1]`. Values on `[-1, 0)` indicate a stance phase, while values on `[0, 1]` indicate a swing phase. + */ + [[nodiscard]] float phase(float t) const noexcept; +}; + +#endif // ANTKEEPER_ANIMATION_STEP_HPP diff --git a/src/engine/gl/shader-template.cpp b/src/engine/gl/shader-template.cpp index 3e9c217..f6c152d 100644 --- a/src/engine/gl/shader-template.cpp +++ b/src/engine/gl/shader-template.cpp @@ -31,14 +31,22 @@ namespace gl { shader_template::shader_template(const text_file& source_code): - template_source{source_code} + m_template_source{source_code} { find_directives(); rehash(); } shader_template::shader_template(text_file&& source_code): - template_source{source_code} + m_template_source{source_code} +{ + find_directives(); + rehash(); +} + +shader_template::shader_template(text_file&& source_code, std::vector>&& include_files): + m_template_source{source_code}, + m_include_files{include_files} { find_directives(); rehash(); @@ -46,14 +54,16 @@ shader_template::shader_template(text_file&& source_code): void shader_template::source(const text_file& source_code) { - template_source = source_code; + m_template_source = source_code; + m_include_files.clear(); find_directives(); rehash(); } void shader_template::source(text_file&& source_code) { - template_source = source_code; + m_template_source = source_code; + m_include_files.clear(); find_directives(); rehash(); } @@ -65,7 +75,7 @@ std::string shader_template::configure(gl::shader_stage stage, const dictionary_ // Join vector of source lines into single string std::string string; - for (const auto& line: template_source.lines) + for (const auto& line: m_template_source.lines) { string += line; string += '\n'; @@ -133,15 +143,15 @@ std::unique_ptr shader_template::build(const dictionary_type void shader_template::find_directives() { // Reset directives - vertex_directives.clear(); - fragment_directives.clear(); - geometry_directives.clear(); - define_directives.clear(); + m_vertex_directives.clear(); + m_fragment_directives.clear(); + m_geometry_directives.clear(); + m_define_directives.clear(); // Parse directives - for (std::size_t i = 0; i < template_source.lines.size(); ++i) + for (std::size_t i = 0; i < m_template_source.lines.size(); ++i) { - std::istringstream line_stream(template_source.lines[i]); + std::istringstream line_stream(m_template_source.lines[i]); std::string token; // Detect `#pragma` directives @@ -154,20 +164,20 @@ void shader_template::find_directives() { if (line_stream >> token) { - define_directives.insert({token, i}); + m_define_directives.insert({token, i}); } } else if (token == "vertex") { - vertex_directives.insert(i); + m_vertex_directives.insert(i); } else if (token == "fragment") { - fragment_directives.insert(i); + m_fragment_directives.insert(i); } else if (token == "geometry") { - geometry_directives.insert(i); + m_geometry_directives.insert(i); } } } @@ -177,7 +187,7 @@ void shader_template::find_directives() void shader_template::rehash() { m_hash = 0; - for (const auto& line: template_source.lines) + for (const auto& line: m_template_source.lines) { m_hash = hash::combine(m_hash, std::hash{}(line)); } @@ -191,27 +201,27 @@ void shader_template::replace_stage_directives(gl::shader_stage stage) const const char* geometry_directive = (stage == gl::shader_stage::geometry) ? "#define __GEOMETRY__" : "/* #undef __GEOMETRY__ */"; // Handle `#pragma ` directives - for (const auto directive_line: vertex_directives) + for (const auto directive_line: m_vertex_directives) { - template_source.lines[directive_line] = vertex_directive; + m_template_source.lines[directive_line] = vertex_directive; } - for (const auto directive_line: fragment_directives) + for (const auto directive_line: m_fragment_directives) { - template_source.lines[directive_line] = fragment_directive; + m_template_source.lines[directive_line] = fragment_directive; } - for (const auto directive_line: geometry_directives) + for (const auto directive_line: m_geometry_directives) { - template_source.lines[directive_line] = geometry_directive; + m_template_source.lines[directive_line] = geometry_directive; } } void shader_template::replace_define_directives(const dictionary_type& definitions) const { // For each `#pragma define ` directive - for (const auto& define_directive: define_directives) + for (const auto& define_directive: m_define_directives) { // Get a reference to the directive line - std::string& line = template_source.lines[define_directive.second]; + std::string& line = m_template_source.lines[define_directive.second]; // Check if the corresponding definition was given by the configuration auto definitions_it = definitions.find(define_directive.first); @@ -232,24 +242,9 @@ void shader_template::replace_define_directives(const dictionary_type& definitio } } -bool shader_template::has_vertex_directive() const noexcept -{ - return !vertex_directives.empty(); -} - -bool shader_template::has_fragment_directive() const noexcept -{ - return !fragment_directives.empty(); -} - -bool shader_template::has_geometry_directive() const noexcept -{ - return !geometry_directives.empty(); -} - bool shader_template::has_define_directive(const std::string& key) const { - return (define_directives.find(key) != define_directives.end()); + return (m_define_directives.find(key) != m_define_directives.end()); } } // namespace gl @@ -284,7 +279,7 @@ static bool has_pragma_once(const text_file& source) /** * Handles `#pragma include` directives by loading the specified text files and inserting them in place. */ -static void handle_includes(text_file& source, std::unordered_set& include_once, resource_manager& resource_manager) +static void handle_includes(std::vector>& include_files, text_file& source, std::unordered_set& include_once, resource_manager& resource_manager) { // For each line in the source for (std::size_t i = 0; i < source.lines.size(); ++i) @@ -312,12 +307,16 @@ static void handle_includes(text_file& source, std::unordered_set(path); + auto include_file = resource_manager.load(path); if (!include_file) { source.lines[i] = "#error file not found: " + path.string(); continue; } + else + { + include_files.emplace_back(include_file); + } // If file has `#pragma once` directive if (has_pragma_once(*include_file)) @@ -330,7 +329,7 @@ static void handle_includes(text_file& source, std::unordered_set resource_loader::load( // Make a copy of the shader template source file text_file source_file_copy = *source_file; + std::vector> include_files; + // Handle `#pragma include` directives std::unordered_set include_once; include_once.insert(ctx.path()); - handle_includes(source_file_copy, include_once, resource_manager); + handle_includes(include_files, source_file_copy, include_once, resource_manager); // Construct shader template - return std::make_unique(std::move(source_file_copy)); + return std::make_unique(std::move(source_file_copy), std::move(include_files)); } diff --git a/src/engine/gl/shader-template.hpp b/src/engine/gl/shader-template.hpp index b5c8814..9980cbe 100644 --- a/src/engine/gl/shader-template.hpp +++ b/src/engine/gl/shader-template.hpp @@ -50,7 +50,7 @@ class shader_template { public: /// Container of definitions used to generate `#pragma define ` directives. - typedef std::unordered_map dictionary_type; + using dictionary_type = std::unordered_map; /** * Constructs a shader template and sets its source code. @@ -62,10 +62,20 @@ public: explicit shader_template(text_file&& source_code); /// @} + /** + * Constructs a shader template and sets its source code. + * + * @param source_code Shader template source code. + * @param include_files Shader template include files. + * + * @note This constuctor is used to keep the loaded include files cached. + */ + shader_template(text_file&& source_code, std::vector>&& include_files); + /** * Constructs an empty shader template. */ - shader_template() = default; + constexpr shader_template() noexcept = default; /** * Replaces the source code of the shader template. @@ -115,13 +125,22 @@ public: [[nodiscard]] std::unique_ptr build(const dictionary_type& definitions = {}) const; /// Returns `true` if the template source contains one or more `#pragma vertex` directive. - [[nodiscard]] bool has_vertex_directive() const noexcept; + [[nodiscard]] inline bool has_vertex_directive() const noexcept + { + return !m_vertex_directives.empty(); + } /// Returns `true` if the template source contains one or more `#pragma fragment` directive. - [[nodiscard]] bool has_fragment_directive() const noexcept; + [[nodiscard]] inline bool has_fragment_directive() const noexcept + { + return !m_fragment_directives.empty(); + } /// Returns `true` if the template source contains one or more `#pragma geometry` directive. - [[nodiscard]] bool has_geometry_directive() const noexcept; + [[nodiscard]] inline bool has_geometry_directive() const noexcept + { + return !m_geometry_directives.empty(); + } /** * Returns `true` if the template source contains one or more instance of `#pragma define `. @@ -131,7 +150,7 @@ public: [[nodiscard]] bool has_define_directive(const std::string& key) const; /// Returns a hash of the template source code. - [[nodiscard]] inline std::size_t hash() const noexcept + [[nodiscard]] inline constexpr std::size_t hash() const noexcept { return m_hash; } @@ -142,12 +161,13 @@ private: void replace_stage_directives(gl::shader_stage stage) const; void replace_define_directives(const dictionary_type& definitions) const; - mutable text_file template_source; - std::unordered_set vertex_directives; - std::unordered_set fragment_directives; - std::unordered_set geometry_directives; - std::multimap define_directives; + mutable text_file m_template_source; + std::unordered_set m_vertex_directives; + std::unordered_set m_fragment_directives; + std::unordered_set m_geometry_directives; + std::multimap m_define_directives; std::size_t m_hash{0}; + std::vector> m_include_files; }; } // namespace gl diff --git a/src/engine/math/euler-angles.hpp b/src/engine/math/euler-angles.hpp new file mode 100644 index 0000000..808c6ed --- /dev/null +++ b/src/engine/math/euler-angles.hpp @@ -0,0 +1,523 @@ +/* + * Copyright (C) 2023 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Antkeeper source code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_MATH_EULER_ANGLES_HPP +#define ANTKEEPER_MATH_EULER_ANGLES_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace math { + +/** + * Rotation sequences of proper Euler and Tait-Bryan angles. + * + * Indices of the first, second, and third rotation axes are encoded in bits 0-1, 2-3, and 4-5, respectively. + */ +enum class rotation_sequence: std::uint8_t +{ + /// *z-x-z* rotation sequence (proper Euler angles). + zxz = 2 | (0 << 2) | (2 << 4), + + /// *x-y-x* rotation sequence (proper Euler angles). + xyx = 0 | (1 << 2) | (0 << 4), + + /// *y-z-y* rotation sequence (proper Euler angles). + yzy = 1 | (2 << 2) | (1 << 4), + + /// *z-y-z* rotation sequence (proper Euler angles). + zyz = 2 | (1 << 2) | (2 << 4), + + /// *x-z-x* rotation sequence (proper Euler angles). + xzx = 0 | (2 << 2) | (0 << 4), + + /// *y-x-y* rotation sequence (proper Euler angles). + yxy = 1 | (0 << 2) | (1 << 4), + + /// *x-y-z* rotation sequence (Tait-Bryan angles). + xyz = 0 | (1 << 2) | (2 << 4), + + /// *y-z-x* rotation sequence (Tait-Bryan angles). + yzx = 1 | (2 << 2) | (0 << 4), + + /// *z-x-y* rotation sequence (Tait-Bryan angles). + zxy = 2 | (0 << 2) | (1 << 4), + + /// *x-z-y* rotation sequence (Tait-Bryan angles). + xzy = 0 | (2 << 2) | (1 << 4), + + /// *z-y-x* rotation sequence (Tait-Bryan angles). + zyx = 2 | (1 << 2) | (0 << 4), + + /// *y-x-z* rotation sequence (Tait-Bryan angles). + yxz = 1 | (0 << 2) | (2 << 4) +}; + +/** + * Returns the indices of the axes of a rotation sequence. + * + * @tparam T Index type. + * + * @param sequence Rotation sequence. + * + * @return Vector containing the indices of the axes of @p sequence. + */ +template +[[nodiscard]] inline constexpr vec3 rotation_axes(rotation_sequence sequence) noexcept +{ + const auto x = static_cast(sequence); + return {x & 3, (x >> 2) & 3, x >> 4}; +} + +/** + * Derives Euler angles from a unit quaternion. + * + * @tparam Sequence Rotation sequence of the Euler angles. + * @tparam T Scalar type. + * + * @param sequence Rotation sequence of the Euler angles. + * @param q Quaternion to convert. + * @param tolerance Floating-point tolerance, in radians, for handling singularities. + * + * @return Euler angles representing the rotation described by the quaternion, in radians. + * + * @see Bernardes, Evandro & Viollet, Stéphane. (2022). Quaternion to Euler angles conversion: A direct, general and computationally efficient method. PLoS ONE. 17. 10.1371/journal.pone.0276302. + */ +/// @{ +template +[[nodiscard]] vec3 euler_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + constexpr auto axes = rotation_axes(Sequence); + constexpr auto proper = axes[0] == axes[2]; + constexpr auto i = axes[0]; + constexpr auto j = axes[1]; + constexpr auto k = proper ? 3 - i - j : axes[2]; + constexpr auto sign = static_cast(((i - j) * (j - k) * (k - i)) >> 1); + + auto a = q.r; + auto b = q.i[i]; + auto c = q.i[j]; + auto d = q.i[k] * sign; + if constexpr (!proper) + { + a -= q.i[j]; + b += d; + c += q.r; + d -= q.i[i]; + } + const auto aa_bb = a * a + b * b; + + vec3 angles; + angles[1] = std::acos(T{2} * aa_bb / (aa_bb + c * c + d * d) - T{1}); + + if (std::abs(angles[1]) <= tolerance) + { + angles[0] = T{0}; + angles[2] = std::atan2(b, a) * T{2}; + } + else if (std::abs(angles[1] - pi) <= tolerance) + { + angles[0] = T{0}; + angles[2] = std::atan2(-d, c) * T{-2}; + } + else + { + const auto half_sum = std::atan2(b, a); + const auto half_diff = std::atan2(-d, c); + + angles[0] = half_sum + half_diff; + angles[2] = half_sum - half_diff; + } + + if constexpr (!proper) + { + angles[2] *= sign; + angles[1] -= half_pi; + } + + return angles; +} + +template +[[nodiscard]] inline vec3 euler_zxz_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_xyx_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_yzy_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_zyz_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_xzx_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_yxy_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_xyz_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_yzx_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_zxy_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_xzy_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_zyx_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_yxz_from_quat(const quat& q, T tolerance = T{1e-6}) +{ + return euler_from_quat(q, tolerance); +} + +template +[[nodiscard]] inline vec3 euler_from_quat(rotation_sequence sequence, const quat& q, T tolerance = T{1e-6}) +{ + switch (sequence) + { + case rotation_sequence::zxz: + return euler_zxz_from_quat(q, tolerance); + case rotation_sequence::xyx: + return euler_xyx_from_quat(q, tolerance); + case rotation_sequence::yzy: + return euler_yzy_from_quat(q, tolerance); + case rotation_sequence::zyz: + return euler_zyz_from_quat(q, tolerance); + case rotation_sequence::xzx: + return euler_xzx_from_quat(q, tolerance); + case rotation_sequence::yxy: + return euler_yxy_from_quat(q, tolerance); + case rotation_sequence::xyz: + return euler_xyz_from_quat(q, tolerance); + case rotation_sequence::yzx: + return euler_yzx_from_quat(q, tolerance); + case rotation_sequence::zxy: + return euler_zxy_from_quat(q, tolerance); + case rotation_sequence::xzy: + return euler_xzy_from_quat(q, tolerance); + case rotation_sequence::zyx: + return euler_zyx_from_quat(q, tolerance); + case rotation_sequence::yxz: + default: + return euler_yxz_from_quat(q, tolerance); + } +} + +/// @} + +/** + * Constructs a quaternion from Euler angles. + * + * @tparam Sequence Rotation sequence of the Euler angles. + * @tparam T Scalar type. + * + * @param angles Vector of Euler angles. + * + * @return Quaternion. + * + * @see Diebel, J. (2006). Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix, 58(15-16), 1-35. + */ +/// @{ +template +[[nodiscard]] quat euler_to_quat(const vec3& angles) +{ + const auto half_angles = angles * T{0.5}; + const T cx = std::cos(half_angles.x()); + const T sx = std::sin(half_angles.x()); + const T cy = std::cos(half_angles.y()); + const T sy = std::sin(half_angles.y()); + const T cz = std::cos(half_angles.z()); + const T sz = std::sin(half_angles.z()); + + if constexpr (Sequence == rotation_sequence::zxz) + { + return + { + cx * cy * cz - sx * cy * sz, + cx * cz * sy + sx * sy * sz, + cx * sy * sz - sx * cz * sy, + cx * cy * sz + cy * cz * sx + }; + } + else if constexpr (Sequence == rotation_sequence::xyx) + { + return + { + cx * cy * cz - sx * cy * sz, + cx * cy * sz + cy * cz * sx, + cx * cz * sy + sx * sy * sz, + cx * sy * sz - sx * cz * sy + }; + } + else if constexpr (Sequence == rotation_sequence::yzy) + { + return + { + cx * cy * cz - sx * cy * sz, + cx * sy * sz - sx * cz * sy, + cx * cy * sz + cy * cz * sx, + cx * cz * sy + sx * sy * sz + }; + } + else if constexpr (Sequence == rotation_sequence::zyz) + { + return + { + cx * cy * cz - sx * cy * sz, + -cx * sy * sz + sx * cz * sy, + cx * cz * sy + sx * sy * sz, + cx * cy * sz + cy * cz * sx + }; + } + else if constexpr (Sequence == rotation_sequence::xzx) + { + return + { + cx * cy * cz - sx * cy * sz, + cx * cy * sz + cy * cz * sx, + -cx * sy * sz + sx * cz * sy, + cx * cz * sy + sx * sy * sz + }; + } + else if constexpr (Sequence == rotation_sequence::yxy) + { + return + { + cx * cy * cz - sx * cy * sz, + cx * cz * sy + sx * sy * sz, + cx * cy * sz + cy * cz * sx, + -cx * sy * sz + sx * cz * sy + }; + } + else if constexpr (Sequence == rotation_sequence::xyz) + { + return + { + cx * cy * cz + sx * sy * sz, + -cx * sy * sz + cy * cz * sx, + cx * cz * sy + sx * cy * sz, + cx * cy * sz - sx * cz * sy + }; + } + else if constexpr (Sequence == rotation_sequence::yzx) + { + return + { + cx * cy * cz + sx * sy * sz, + cx * cy * sz - sx * cz * sy, + -cx * sy * sz + cy * cz * sx, + cx * cz * sy + sx * cy * sz + }; + } + else if constexpr (Sequence == rotation_sequence::zxy) + { + return + { + cx * cy * cz + sx * sy * sz, + cx * cz * sy + sx * cy * sz, + cx * cy * sz - sx * cz * sy, + -cx * sy * sz + cy * cz * sx + }; + } + else if constexpr (Sequence == rotation_sequence::xzy) + { + return + { + cx * cy * cz - sx * sy * sz, + cx * sy * sz + cy * cz * sx, + cx * cy * sz + sx * cz * sy, + cx * cz * sy - sx * cy * sz + }; + } + else if constexpr (Sequence == rotation_sequence::zyx) + { + return + { + cx * cy * cz - sx * sy * sz, + cx * cy * sz + sx * cz * sy, + cx * cz * sy - sx * cy * sz, + cx * sy * sz + cy * cz * sx + }; + } + else //if constexpr (Sequence == rotation_sequence::yxz) + { + return + { + cx * cy * cz - sx * sy * sz, + cx * cz * sy - sx * cy * sz, + cx * sy * sz + cy * cz * sx, + cx * cy * sz + sx * cz * sy + }; + } +} + +template +[[nodiscard]] inline quat euler_zxz_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_xyx_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_yzy_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_zyz_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_xzx_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_yxy_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_xyz_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_yzx_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_zxy_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_xzy_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_zyx_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_yxz_to_quat(const vec3& angles) +{ + return euler_to_quat(angles); +} + +template +[[nodiscard]] inline quat euler_to_quat(rotation_sequence sequence, const vec3& angles) +{ + switch (sequence) + { + case rotation_sequence::zxz: + return euler_zxz_to_quat(angles); + case rotation_sequence::xyx: + return euler_xyx_to_quat(angles); + case rotation_sequence::yzy: + return euler_yzy_to_quat(angles); + case rotation_sequence::zyz: + return euler_zyz_to_quat(angles); + case rotation_sequence::xzx: + return euler_xzx_to_quat(angles); + case rotation_sequence::yxy: + return euler_yxy_to_quat(angles); + case rotation_sequence::xyz: + return euler_xyz_to_quat(angles); + case rotation_sequence::yzx: + return euler_yzx_to_quat(angles); + case rotation_sequence::zxy: + return euler_zxy_to_quat(angles); + case rotation_sequence::xzy: + return euler_xzy_to_quat(angles); + case rotation_sequence::zyx: + return euler_zyx_to_quat(angles); + case rotation_sequence::yxz: + default: + return euler_yxz_to_quat(angles); + } +} +/// @} + +} // namespace math + +#endif // ANTKEEPER_MATH_EULER_ANGLES_HPP diff --git a/src/engine/math/matrix.hpp b/src/engine/math/matrix.hpp index 6a637e0..52c165a 100644 --- a/src/engine/math/matrix.hpp +++ b/src/engine/math/matrix.hpp @@ -28,9 +28,6 @@ namespace math { -/// Matrix types. -namespace matrix_types { - /** * *n* by *m* column-major matrix. * @@ -359,6 +356,9 @@ struct matrix /// @} }; +/// Matrix types. +namespace matrix_types { + /** * *n* by *m* matrix. * diff --git a/src/engine/math/quaternion.hpp b/src/engine/math/quaternion.hpp index c494388..9342c91 100644 --- a/src/engine/math/quaternion.hpp +++ b/src/engine/math/quaternion.hpp @@ -23,13 +23,12 @@ #include #include #include +#include #include +#include namespace math { -/// Quaternion types. -namespace quaternion_types { - /** * Quaternion composed of a real scalar part and imaginary vector part. * @@ -150,7 +149,7 @@ struct quaternion } /** - * Constructs a matrix representing the rotation described the quaternion. + * Constructs a matrix representing the rotation described by the quaternion. * * @return Rotation matrix. */ @@ -204,7 +203,10 @@ struct quaternion } }; -/// @copydoc math::quaternion_types::quaternion +/// Quaternion types. +namespace quaternion_types { + +/// @copydoc math::quaternion template using quat = quaternion; @@ -514,13 +516,15 @@ template * @param[out] twist Twist rotation component. * @param[in] tolerance Floating-point tolerance. * + * @return Array containing the swing rotation component, followed by the twist rotation component. + * * @warning @p q must be a unit quaternion. * @warning @p twist_axis must be a unit vector. * * @see https://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/ */ template -void swing_twist(const quaternion& q, const vec3& twist_axis, quaternion& swing, quaternion& twist, T tolerance = T{1e-6}); +[[nodiscard]] std::array, 2> swing_twist(const quaternion& q, const vec3& twist_axis, T tolerance = T{1e-6}); /** * Converts a 3x3 rotation matrix to a quaternion. @@ -653,7 +657,7 @@ inline constexpr quaternion negate(const quaternion& q) noexcept template quaternion nlerp(const quaternion& a, const quaternion& b, T t) { - return normalize(add(mul(a, T{1} - t), mul(b, t * std::copysign(T{1}, dot(a, b))))); + return normalize(add(mul(a, T{1} - t), mul(b, std::copysign(t, dot(a, b))))); } template @@ -738,16 +742,18 @@ inline constexpr quaternion sub(T a, const quaternion& b) noexcept } template -void swing_twist(const quaternion& q, const vec3& twist_axis, quaternion& swing, quaternion& twist, T tolerance) +std::array, 2> swing_twist(const quaternion& q, const vec3& twist_axis, T tolerance) { + quaternion swing; + quaternion twist; + if (sqr_length(q.i) <= tolerance) { // Singularity, rotate 180 degrees about twist axis twist = angle_axis(pi, twist_axis); const auto rotated_twist_axis = mul(q, twist_axis); - - auto swing_axis = cross(twist_axis, rotated_twist_axis); + const auto swing_axis = cross(twist_axis, rotated_twist_axis); const auto swing_axis_sqr_length = sqr_length(swing_axis); if (swing_axis_sqr_length <= tolerance) @@ -766,6 +772,8 @@ void swing_twist(const quaternion& q, const vec3& twist_axis, quaternion{q.r, twist_axis * dot(twist_axis, q.i)}); swing = mul(q, conjugate(twist)); } + + return {std::move(swing), std::move(twist)}; } template diff --git a/src/engine/math/se3.hpp b/src/engine/math/se3.hpp index 72986cc..de79434 100644 --- a/src/engine/math/se3.hpp +++ b/src/engine/math/se3.hpp @@ -25,9 +25,6 @@ namespace math { -/// Transformation types -namespace transformation_types { - /** * SE(3) proper rigid transformation (rototranslation). * @@ -140,14 +137,6 @@ public: } }; -} // namespace transformation_types - -// Bring transformation types into math namespace -using namespace transformation_types; - -// Bring transformation types into math::types namespace -namespace types { using namespace math::transformation_types; } - } // namespace math #endif // ANTKEEPER_MATH_TRANSFORMATION_SE3_HPP diff --git a/src/engine/math/vector.hpp b/src/engine/math/vector.hpp index b7dd0cd..d49d187 100644 --- a/src/engine/math/vector.hpp +++ b/src/engine/math/vector.hpp @@ -31,9 +31,6 @@ namespace math { -/// Vector types. -namespace vector_types { - /** * *n*-dimensional vector. * @@ -348,6 +345,9 @@ struct vector /// @} }; +/// Vector types. +namespace vector_types { + /** * 2-dimensional vector. * diff --git a/src/engine/scene/skeletal-mesh.hpp b/src/engine/scene/skeletal-mesh.hpp index 56309b1..981f490 100644 --- a/src/engine/scene/skeletal-mesh.hpp +++ b/src/engine/scene/skeletal-mesh.hpp @@ -81,6 +81,12 @@ public: void render(render::context& ctx) const override; + /// Returns the skeleton of the skeletal mesh. + [[nodiscard]] inline const skeleton* get_skeleton() const noexcept + { + return m_pose.get_skeleton(); + } + /// Returns the animation pose of the skeletal mesh. /// @{ [[nodiscard]] inline const animation_pose& get_pose() const noexcept diff --git a/src/game/ant/ant-skeleton.cpp b/src/game/ant/ant-skeleton.cpp index 3922489..8f13e1a 100644 --- a/src/game/ant/ant-skeleton.cpp +++ b/src/game/ant/ant-skeleton.cpp @@ -20,6 +20,7 @@ #include "game/ant/ant-skeleton.hpp" #include "game/ant/ant-bone-set.hpp" #include +#include namespace { @@ -121,6 +122,530 @@ void generate_ant_rest_pose(::skeleton& skeleton, const ant_bone_set& bones, con skeleton.update_rest_pose(); } +/** + * Generates a standing pose (named "midstance") for an ant skeleton. + * + * @param[in,out] skeleton Ant skeleton. + * @parma bones Set of ant bone indices. + */ +void generate_ant_midstance_pose(skeleton& skeleton, const ant_bone_set& bones) +{ + auto& pose = skeleton.add_pose("midstance"); + const auto& rest_pose = skeleton.get_rest_pose(); + + // Pose forelegs + { + const auto procoxa_l_angles = math::fvec3{0.0f, 40.0f, 0.0f} * math::deg2rad; + const auto procoxa_r_angles = procoxa_l_angles * math::fvec3{1, -1, 1}; + const auto profemur_l_angles = math::fvec3{31.0f, 0.0f, 0.0f} * math::deg2rad; + const auto profemur_r_angles = profemur_l_angles * math::fvec3{1, 1, 1}; + const auto protibia_l_angles = math::fvec3{-90.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protibia_r_angles = protibia_l_angles * math::fvec3{1, 1, 1}; + const auto protarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protarsomere1_r_angles = protarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto procoxa_l_xf = math::transform::identity(); + auto procoxa_r_xf = math::transform::identity(); + auto profemur_l_xf = math::transform::identity(); + auto profemur_r_xf = math::transform::identity(); + auto protibia_l_xf = math::transform::identity(); + auto protibia_r_xf = math::transform::identity(); + auto protarsomere1_l_xf = math::transform::identity(); + auto protarsomere1_r_xf = math::transform::identity(); + + procoxa_l_xf.rotation = math::euler_xyz_to_quat(procoxa_l_angles); + procoxa_r_xf.rotation = math::euler_xyz_to_quat(procoxa_r_angles); + profemur_l_xf.rotation = math::euler_xyz_to_quat(profemur_l_angles); + profemur_r_xf.rotation = math::euler_xyz_to_quat(profemur_r_angles); + protibia_l_xf.rotation = math::euler_xyz_to_quat(protibia_l_angles); + protibia_r_xf.rotation = math::euler_xyz_to_quat(protibia_r_angles); + protarsomere1_l_xf.rotation = math::euler_xyz_to_quat(protarsomere1_l_angles); + protarsomere1_r_xf.rotation = math::euler_xyz_to_quat(protarsomere1_r_angles); + + pose.set_relative_transform(bones.procoxa_l, rest_pose.get_relative_transform(bones.procoxa_l) * procoxa_l_xf); + pose.set_relative_transform(bones.procoxa_r, rest_pose.get_relative_transform(bones.procoxa_r) * procoxa_r_xf); + pose.set_relative_transform(bones.profemur_l, rest_pose.get_relative_transform(bones.profemur_l) * profemur_l_xf); + pose.set_relative_transform(bones.profemur_r, rest_pose.get_relative_transform(bones.profemur_r) * profemur_r_xf); + pose.set_relative_transform(bones.protibia_l, rest_pose.get_relative_transform(bones.protibia_l) * protibia_l_xf); + pose.set_relative_transform(bones.protibia_r, rest_pose.get_relative_transform(bones.protibia_r) * protibia_r_xf); + pose.set_relative_transform(bones.protarsomere1_l, rest_pose.get_relative_transform(bones.protarsomere1_l) * protarsomere1_l_xf); + pose.set_relative_transform(bones.protarsomere1_r, rest_pose.get_relative_transform(bones.protarsomere1_r) * protarsomere1_r_xf); + } + + // Pose midlegs + { + const auto mesocoxa_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesocoxa_r_angles = mesocoxa_l_angles * math::fvec3{1, 1, 1}; + const auto mesofemur_l_angles = math::fvec3{38.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesofemur_r_angles = mesofemur_l_angles * math::fvec3{1, 1, 1}; + const auto mesotibia_l_angles = math::fvec3{-100.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotibia_r_angles = mesotibia_l_angles * math::fvec3{1, 1, 1}; + const auto mesotarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotarsomere1_r_angles = mesotarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto mesocoxa_l_xf = math::transform::identity(); + auto mesocoxa_r_xf = math::transform::identity(); + auto mesofemur_l_xf = math::transform::identity(); + auto mesofemur_r_xf = math::transform::identity(); + auto mesotibia_l_xf = math::transform::identity(); + auto mesotibia_r_xf = math::transform::identity(); + auto mesotarsomere1_l_xf = math::transform::identity(); + auto mesotarsomere1_r_xf = math::transform::identity(); + + mesocoxa_l_xf.rotation = math::euler_xyz_to_quat(mesocoxa_l_angles); + mesocoxa_r_xf.rotation = math::euler_xyz_to_quat(mesocoxa_r_angles); + mesofemur_l_xf.rotation = math::euler_xyz_to_quat(mesofemur_l_angles); + mesofemur_r_xf.rotation = math::euler_xyz_to_quat(mesofemur_r_angles); + mesotibia_l_xf.rotation = math::euler_xyz_to_quat(mesotibia_l_angles); + mesotibia_r_xf.rotation = math::euler_xyz_to_quat(mesotibia_r_angles); + mesotarsomere1_l_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_l_angles); + mesotarsomere1_r_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_r_angles); + + pose.set_relative_transform(bones.mesocoxa_l, rest_pose.get_relative_transform(bones.mesocoxa_l) * mesocoxa_l_xf); + pose.set_relative_transform(bones.mesocoxa_r, rest_pose.get_relative_transform(bones.mesocoxa_r) * mesocoxa_r_xf); + pose.set_relative_transform(bones.mesofemur_l, rest_pose.get_relative_transform(bones.mesofemur_l) * mesofemur_l_xf); + pose.set_relative_transform(bones.mesofemur_r, rest_pose.get_relative_transform(bones.mesofemur_r) * mesofemur_r_xf); + pose.set_relative_transform(bones.mesotibia_l, rest_pose.get_relative_transform(bones.mesotibia_l) * mesotibia_l_xf); + pose.set_relative_transform(bones.mesotibia_r, rest_pose.get_relative_transform(bones.mesotibia_r) * mesotibia_r_xf); + pose.set_relative_transform(bones.mesotarsomere1_l, rest_pose.get_relative_transform(bones.mesotarsomere1_l) * mesotarsomere1_l_xf); + pose.set_relative_transform(bones.mesotarsomere1_r, rest_pose.get_relative_transform(bones.mesotarsomere1_r) * mesotarsomere1_r_xf); + } + + // Pose hindlegs + { + const auto metacoxa_l_angles = math::fvec3{0.0f, -34.0f, 0.0f} * math::deg2rad; + const auto metacoxa_r_angles = metacoxa_l_angles * math::fvec3{1, -1, 1}; + const auto metafemur_l_angles = math::fvec3{48.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metafemur_r_angles = metafemur_l_angles * math::fvec3{1, 1, 1}; + const auto metatibia_l_angles = math::fvec3{-100.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatibia_r_angles = metatibia_l_angles * math::fvec3{1, 1, 1}; + const auto metatarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatarsomere1_r_angles = metatarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto metacoxa_l_xf = math::transform::identity(); + auto metacoxa_r_xf = math::transform::identity(); + auto metafemur_l_xf = math::transform::identity(); + auto metafemur_r_xf = math::transform::identity(); + auto metatibia_l_xf = math::transform::identity(); + auto metatibia_r_xf = math::transform::identity(); + auto metatarsomere1_l_xf = math::transform::identity(); + auto metatarsomere1_r_xf = math::transform::identity(); + + metacoxa_l_xf.rotation = math::euler_xyz_to_quat(metacoxa_l_angles); + metacoxa_r_xf.rotation = math::euler_xyz_to_quat(metacoxa_r_angles); + metafemur_l_xf.rotation = math::euler_xyz_to_quat(metafemur_l_angles); + metafemur_r_xf.rotation = math::euler_xyz_to_quat(metafemur_r_angles); + metatibia_l_xf.rotation = math::euler_xyz_to_quat(metatibia_l_angles); + metatibia_r_xf.rotation = math::euler_xyz_to_quat(metatibia_r_angles); + metatarsomere1_l_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_l_angles); + metatarsomere1_r_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_r_angles); + + pose.set_relative_transform(bones.metacoxa_l, rest_pose.get_relative_transform(bones.metacoxa_l) * metacoxa_l_xf); + pose.set_relative_transform(bones.metacoxa_r, rest_pose.get_relative_transform(bones.metacoxa_r) * metacoxa_r_xf); + pose.set_relative_transform(bones.metafemur_l, rest_pose.get_relative_transform(bones.metafemur_l) * metafemur_l_xf); + pose.set_relative_transform(bones.metafemur_r, rest_pose.get_relative_transform(bones.metafemur_r) * metafemur_r_xf); + pose.set_relative_transform(bones.metatibia_l, rest_pose.get_relative_transform(bones.metatibia_l) * metatibia_l_xf); + pose.set_relative_transform(bones.metatibia_r, rest_pose.get_relative_transform(bones.metatibia_r) * metatibia_r_xf); + pose.set_relative_transform(bones.metatarsomere1_l, rest_pose.get_relative_transform(bones.metatarsomere1_l) * metatarsomere1_l_xf); + pose.set_relative_transform(bones.metatarsomere1_r, rest_pose.get_relative_transform(bones.metatarsomere1_r) * metatarsomere1_r_xf); + } + + pose.update(); +} + +/** + * Generates a lift-off pose (named "liftoff") for an ant skeleton. + * + * @param[in,out] skeleton Ant skeleton. + * @parma bones Set of ant bone indices. + */ +void generate_ant_liftoff_pose(skeleton& skeleton, const ant_bone_set& bones) +{ + auto& pose = skeleton.add_pose("liftoff"); + const auto& rest_pose = skeleton.get_rest_pose(); + + // Pose forelegs + { + const auto procoxa_l_angles = math::fvec3{0.0f, 50.0f, 0.0f} * math::deg2rad; + const auto procoxa_r_angles = procoxa_l_angles * math::fvec3{1, -1, 1}; + const auto profemur_l_angles = math::fvec3{34.0f, 0.0f, 0.0f} * math::deg2rad; + const auto profemur_r_angles = profemur_l_angles * math::fvec3{1, 1, 1}; + const auto protibia_l_angles = math::fvec3{-118.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protibia_r_angles = protibia_l_angles * math::fvec3{1, 1, 1}; + const auto protarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protarsomere1_r_angles = protarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto procoxa_l_xf = math::transform::identity(); + auto procoxa_r_xf = math::transform::identity(); + auto profemur_l_xf = math::transform::identity(); + auto profemur_r_xf = math::transform::identity(); + auto protibia_l_xf = math::transform::identity(); + auto protibia_r_xf = math::transform::identity(); + auto protarsomere1_l_xf = math::transform::identity(); + auto protarsomere1_r_xf = math::transform::identity(); + + procoxa_l_xf.rotation = math::euler_xyz_to_quat(procoxa_l_angles); + procoxa_r_xf.rotation = math::euler_xyz_to_quat(procoxa_r_angles); + profemur_l_xf.rotation = math::euler_xyz_to_quat(profemur_l_angles); + profemur_r_xf.rotation = math::euler_xyz_to_quat(profemur_r_angles); + protibia_l_xf.rotation = math::euler_xyz_to_quat(protibia_l_angles); + protibia_r_xf.rotation = math::euler_xyz_to_quat(protibia_r_angles); + protarsomere1_l_xf.rotation = math::euler_xyz_to_quat(protarsomere1_l_angles); + protarsomere1_r_xf.rotation = math::euler_xyz_to_quat(protarsomere1_r_angles); + + pose.set_relative_transform(bones.procoxa_l, rest_pose.get_relative_transform(bones.procoxa_l) * procoxa_l_xf); + pose.set_relative_transform(bones.procoxa_r, rest_pose.get_relative_transform(bones.procoxa_r) * procoxa_r_xf); + pose.set_relative_transform(bones.profemur_l, rest_pose.get_relative_transform(bones.profemur_l) * profemur_l_xf); + pose.set_relative_transform(bones.profemur_r, rest_pose.get_relative_transform(bones.profemur_r) * profemur_r_xf); + pose.set_relative_transform(bones.protibia_l, rest_pose.get_relative_transform(bones.protibia_l) * protibia_l_xf); + pose.set_relative_transform(bones.protibia_r, rest_pose.get_relative_transform(bones.protibia_r) * protibia_r_xf); + pose.set_relative_transform(bones.protarsomere1_l, rest_pose.get_relative_transform(bones.protarsomere1_l) * protarsomere1_l_xf); + pose.set_relative_transform(bones.protarsomere1_r, rest_pose.get_relative_transform(bones.protarsomere1_r) * protarsomere1_r_xf); + } + + // Pose midlegs + { + const auto mesocoxa_l_angles = math::fvec3{0.0f, 30.0f, 0.0f} * math::deg2rad; + const auto mesocoxa_r_angles = mesocoxa_l_angles * math::fvec3{1, -1, 1}; + const auto mesofemur_l_angles = math::fvec3{36.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesofemur_r_angles = mesofemur_l_angles * math::fvec3{1, 1, 1}; + const auto mesotibia_l_angles = math::fvec3{-110.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotibia_r_angles = mesotibia_l_angles * math::fvec3{1, 1, 1}; + const auto mesotarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotarsomere1_r_angles = mesotarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto mesocoxa_l_xf = math::transform::identity(); + auto mesocoxa_r_xf = math::transform::identity(); + auto mesofemur_l_xf = math::transform::identity(); + auto mesofemur_r_xf = math::transform::identity(); + auto mesotibia_l_xf = math::transform::identity(); + auto mesotibia_r_xf = math::transform::identity(); + auto mesotarsomere1_l_xf = math::transform::identity(); + auto mesotarsomere1_r_xf = math::transform::identity(); + + mesocoxa_l_xf.rotation = math::euler_xyz_to_quat(mesocoxa_l_angles); + mesocoxa_r_xf.rotation = math::euler_xyz_to_quat(mesocoxa_r_angles); + mesofemur_l_xf.rotation = math::euler_xyz_to_quat(mesofemur_l_angles); + mesofemur_r_xf.rotation = math::euler_xyz_to_quat(mesofemur_r_angles); + mesotibia_l_xf.rotation = math::euler_xyz_to_quat(mesotibia_l_angles); + mesotibia_r_xf.rotation = math::euler_xyz_to_quat(mesotibia_r_angles); + mesotarsomere1_l_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_l_angles); + mesotarsomere1_r_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_r_angles); + + pose.set_relative_transform(bones.mesocoxa_l, rest_pose.get_relative_transform(bones.mesocoxa_l) * mesocoxa_l_xf); + pose.set_relative_transform(bones.mesocoxa_r, rest_pose.get_relative_transform(bones.mesocoxa_r) * mesocoxa_r_xf); + pose.set_relative_transform(bones.mesofemur_l, rest_pose.get_relative_transform(bones.mesofemur_l) * mesofemur_l_xf); + pose.set_relative_transform(bones.mesofemur_r, rest_pose.get_relative_transform(bones.mesofemur_r) * mesofemur_r_xf); + pose.set_relative_transform(bones.mesotibia_l, rest_pose.get_relative_transform(bones.mesotibia_l) * mesotibia_l_xf); + pose.set_relative_transform(bones.mesotibia_r, rest_pose.get_relative_transform(bones.mesotibia_r) * mesotibia_r_xf); + pose.set_relative_transform(bones.mesotarsomere1_l, rest_pose.get_relative_transform(bones.mesotarsomere1_l) * mesotarsomere1_l_xf); + pose.set_relative_transform(bones.mesotarsomere1_r, rest_pose.get_relative_transform(bones.mesotarsomere1_r) * mesotarsomere1_r_xf); + } + + // Pose hindlegs + { + const auto metacoxa_l_angles = math::fvec3{0.0f, -27.5f, 0.0f} * math::deg2rad; + const auto metacoxa_r_angles = metacoxa_l_angles * math::fvec3{1, -1, 1}; + const auto metafemur_l_angles = math::fvec3{6.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metafemur_r_angles = metafemur_l_angles * math::fvec3{1, 1, 1}; + const auto metatibia_l_angles = math::fvec3{-39.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatibia_r_angles = metatibia_l_angles * math::fvec3{1, 1, 1}; + const auto metatarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatarsomere1_r_angles = metatarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto metacoxa_l_xf = math::transform::identity(); + auto metacoxa_r_xf = math::transform::identity(); + auto metafemur_l_xf = math::transform::identity(); + auto metafemur_r_xf = math::transform::identity(); + auto metatibia_l_xf = math::transform::identity(); + auto metatibia_r_xf = math::transform::identity(); + auto metatarsomere1_l_xf = math::transform::identity(); + auto metatarsomere1_r_xf = math::transform::identity(); + + metacoxa_l_xf.rotation = math::euler_xyz_to_quat(metacoxa_l_angles); + metacoxa_r_xf.rotation = math::euler_xyz_to_quat(metacoxa_r_angles); + metafemur_l_xf.rotation = math::euler_xyz_to_quat(metafemur_l_angles); + metafemur_r_xf.rotation = math::euler_xyz_to_quat(metafemur_r_angles); + metatibia_l_xf.rotation = math::euler_xyz_to_quat(metatibia_l_angles); + metatibia_r_xf.rotation = math::euler_xyz_to_quat(metatibia_r_angles); + metatarsomere1_l_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_l_angles); + metatarsomere1_r_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_r_angles); + + pose.set_relative_transform(bones.metacoxa_l, rest_pose.get_relative_transform(bones.metacoxa_l) * metacoxa_l_xf); + pose.set_relative_transform(bones.metacoxa_r, rest_pose.get_relative_transform(bones.metacoxa_r) * metacoxa_r_xf); + pose.set_relative_transform(bones.metafemur_l, rest_pose.get_relative_transform(bones.metafemur_l) * metafemur_l_xf); + pose.set_relative_transform(bones.metafemur_r, rest_pose.get_relative_transform(bones.metafemur_r) * metafemur_r_xf); + pose.set_relative_transform(bones.metatibia_l, rest_pose.get_relative_transform(bones.metatibia_l) * metatibia_l_xf); + pose.set_relative_transform(bones.metatibia_r, rest_pose.get_relative_transform(bones.metatibia_r) * metatibia_r_xf); + pose.set_relative_transform(bones.metatarsomere1_l, rest_pose.get_relative_transform(bones.metatarsomere1_l) * metatarsomere1_l_xf); + pose.set_relative_transform(bones.metatarsomere1_r, rest_pose.get_relative_transform(bones.metatarsomere1_r) * metatarsomere1_r_xf); + } + + pose.update(); +} + +/** + * Generates a touchdown pose (named "touchdown") for an ant skeleton. + * + * @param[in,out] skeleton Ant skeleton. + * @parma bones Set of ant bone indices. + */ +void generate_ant_touchdown_pose(skeleton& skeleton, const ant_bone_set& bones) +{ + auto& pose = skeleton.add_pose("touchdown"); + const auto& rest_pose = skeleton.get_rest_pose(); + + // Pose forelegs + { + const auto procoxa_l_angles = math::fvec3{0.0f, 25.0f, 0.0f} * math::deg2rad; + const auto procoxa_r_angles = procoxa_l_angles * math::fvec3{1, -1, 1}; + const auto profemur_l_angles = math::fvec3{10.0f, 0.0f, 0.0f} * math::deg2rad; + const auto profemur_r_angles = profemur_l_angles * math::fvec3{1, 1, 1}; + const auto protibia_l_angles = math::fvec3{-60.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protibia_r_angles = protibia_l_angles * math::fvec3{1, 1, 1}; + const auto protarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protarsomere1_r_angles = protarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto procoxa_l_xf = math::transform::identity(); + auto procoxa_r_xf = math::transform::identity(); + auto profemur_l_xf = math::transform::identity(); + auto profemur_r_xf = math::transform::identity(); + auto protibia_l_xf = math::transform::identity(); + auto protibia_r_xf = math::transform::identity(); + auto protarsomere1_l_xf = math::transform::identity(); + auto protarsomere1_r_xf = math::transform::identity(); + + procoxa_l_xf.rotation = math::euler_xyz_to_quat(procoxa_l_angles); + procoxa_r_xf.rotation = math::euler_xyz_to_quat(procoxa_r_angles); + profemur_l_xf.rotation = math::euler_xyz_to_quat(profemur_l_angles); + profemur_r_xf.rotation = math::euler_xyz_to_quat(profemur_r_angles); + protibia_l_xf.rotation = math::euler_xyz_to_quat(protibia_l_angles); + protibia_r_xf.rotation = math::euler_xyz_to_quat(protibia_r_angles); + protarsomere1_l_xf.rotation = math::euler_xyz_to_quat(protarsomere1_l_angles); + protarsomere1_r_xf.rotation = math::euler_xyz_to_quat(protarsomere1_r_angles); + + pose.set_relative_transform(bones.procoxa_l, rest_pose.get_relative_transform(bones.procoxa_l) * procoxa_l_xf); + pose.set_relative_transform(bones.procoxa_r, rest_pose.get_relative_transform(bones.procoxa_r) * procoxa_r_xf); + pose.set_relative_transform(bones.profemur_l, rest_pose.get_relative_transform(bones.profemur_l) * profemur_l_xf); + pose.set_relative_transform(bones.profemur_r, rest_pose.get_relative_transform(bones.profemur_r) * profemur_r_xf); + pose.set_relative_transform(bones.protibia_l, rest_pose.get_relative_transform(bones.protibia_l) * protibia_l_xf); + pose.set_relative_transform(bones.protibia_r, rest_pose.get_relative_transform(bones.protibia_r) * protibia_r_xf); + pose.set_relative_transform(bones.protarsomere1_l, rest_pose.get_relative_transform(bones.protarsomere1_l) * protarsomere1_l_xf); + pose.set_relative_transform(bones.protarsomere1_r, rest_pose.get_relative_transform(bones.protarsomere1_r) * protarsomere1_r_xf); + } + + // Pose midlegs + { + const auto mesocoxa_l_angles = math::fvec3{0.0f, -22.0f, 0.0f} * math::deg2rad; + const auto mesocoxa_r_angles = mesocoxa_l_angles * math::fvec3{1, -1, 1}; + const auto mesofemur_l_angles = math::fvec3{21.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesofemur_r_angles = mesofemur_l_angles * math::fvec3{1, 1, 1}; + const auto mesotibia_l_angles = math::fvec3{-80.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotibia_r_angles = mesotibia_l_angles * math::fvec3{1, 1, 1}; + const auto mesotarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotarsomere1_r_angles = mesotarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto mesocoxa_l_xf = math::transform::identity(); + auto mesocoxa_r_xf = math::transform::identity(); + auto mesofemur_l_xf = math::transform::identity(); + auto mesofemur_r_xf = math::transform::identity(); + auto mesotibia_l_xf = math::transform::identity(); + auto mesotibia_r_xf = math::transform::identity(); + auto mesotarsomere1_l_xf = math::transform::identity(); + auto mesotarsomere1_r_xf = math::transform::identity(); + + mesocoxa_l_xf.rotation = math::euler_xyz_to_quat(mesocoxa_l_angles); + mesocoxa_r_xf.rotation = math::euler_xyz_to_quat(mesocoxa_r_angles); + mesofemur_l_xf.rotation = math::euler_xyz_to_quat(mesofemur_l_angles); + mesofemur_r_xf.rotation = math::euler_xyz_to_quat(mesofemur_r_angles); + mesotibia_l_xf.rotation = math::euler_xyz_to_quat(mesotibia_l_angles); + mesotibia_r_xf.rotation = math::euler_xyz_to_quat(mesotibia_r_angles); + mesotarsomere1_l_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_l_angles); + mesotarsomere1_r_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_r_angles); + + pose.set_relative_transform(bones.mesocoxa_l, rest_pose.get_relative_transform(bones.mesocoxa_l) * mesocoxa_l_xf); + pose.set_relative_transform(bones.mesocoxa_r, rest_pose.get_relative_transform(bones.mesocoxa_r) * mesocoxa_r_xf); + pose.set_relative_transform(bones.mesofemur_l, rest_pose.get_relative_transform(bones.mesofemur_l) * mesofemur_l_xf); + pose.set_relative_transform(bones.mesofemur_r, rest_pose.get_relative_transform(bones.mesofemur_r) * mesofemur_r_xf); + pose.set_relative_transform(bones.mesotibia_l, rest_pose.get_relative_transform(bones.mesotibia_l) * mesotibia_l_xf); + pose.set_relative_transform(bones.mesotibia_r, rest_pose.get_relative_transform(bones.mesotibia_r) * mesotibia_r_xf); + pose.set_relative_transform(bones.mesotarsomere1_l, rest_pose.get_relative_transform(bones.mesotarsomere1_l) * mesotarsomere1_l_xf); + pose.set_relative_transform(bones.mesotarsomere1_r, rest_pose.get_relative_transform(bones.mesotarsomere1_r) * mesotarsomere1_r_xf); + } + + // Pose hindlegs + { + const auto metacoxa_l_angles = math::fvec3{0.0f, -42.0f, 0.0f} * math::deg2rad; + const auto metacoxa_r_angles = metacoxa_l_angles * math::fvec3{1, -1, 1}; + const auto metafemur_l_angles = math::fvec3{60.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metafemur_r_angles = metafemur_l_angles * math::fvec3{1, 1, 1}; + const auto metatibia_l_angles = math::fvec3{-125.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatibia_r_angles = metatibia_l_angles * math::fvec3{1, 1, 1}; + const auto metatarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatarsomere1_r_angles = metatarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto metacoxa_l_xf = math::transform::identity(); + auto metacoxa_r_xf = math::transform::identity(); + auto metafemur_l_xf = math::transform::identity(); + auto metafemur_r_xf = math::transform::identity(); + auto metatibia_l_xf = math::transform::identity(); + auto metatibia_r_xf = math::transform::identity(); + auto metatarsomere1_l_xf = math::transform::identity(); + auto metatarsomere1_r_xf = math::transform::identity(); + + metacoxa_l_xf.rotation = math::euler_xyz_to_quat(metacoxa_l_angles); + metacoxa_r_xf.rotation = math::euler_xyz_to_quat(metacoxa_r_angles); + metafemur_l_xf.rotation = math::euler_xyz_to_quat(metafemur_l_angles); + metafemur_r_xf.rotation = math::euler_xyz_to_quat(metafemur_r_angles); + metatibia_l_xf.rotation = math::euler_xyz_to_quat(metatibia_l_angles); + metatibia_r_xf.rotation = math::euler_xyz_to_quat(metatibia_r_angles); + metatarsomere1_l_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_l_angles); + metatarsomere1_r_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_r_angles); + + pose.set_relative_transform(bones.metacoxa_l, rest_pose.get_relative_transform(bones.metacoxa_l) * metacoxa_l_xf); + pose.set_relative_transform(bones.metacoxa_r, rest_pose.get_relative_transform(bones.metacoxa_r) * metacoxa_r_xf); + pose.set_relative_transform(bones.metafemur_l, rest_pose.get_relative_transform(bones.metafemur_l) * metafemur_l_xf); + pose.set_relative_transform(bones.metafemur_r, rest_pose.get_relative_transform(bones.metafemur_r) * metafemur_r_xf); + pose.set_relative_transform(bones.metatibia_l, rest_pose.get_relative_transform(bones.metatibia_l) * metatibia_l_xf); + pose.set_relative_transform(bones.metatibia_r, rest_pose.get_relative_transform(bones.metatibia_r) * metatibia_r_xf); + pose.set_relative_transform(bones.metatarsomere1_l, rest_pose.get_relative_transform(bones.metatarsomere1_l) * metatarsomere1_l_xf); + pose.set_relative_transform(bones.metatarsomere1_r, rest_pose.get_relative_transform(bones.metatarsomere1_r) * metatarsomere1_r_xf); + } + + pose.update(); +} + +/** + * Generates a pose in which each leg is lifted to its step height (named "midswing") for an ant skeleton. + * + * @param[in,out] skeleton Ant skeleton. + * @parma bones Set of ant bone indices. + */ +void generate_ant_midswing_pose(skeleton& skeleton, const ant_bone_set& bones) +{ + auto& pose = skeleton.add_pose("midswing"); + const auto& rest_pose = skeleton.get_rest_pose(); + + // Pose forelegs + { + const auto procoxa_l_angles = math::fvec3{0.0f, 37.5f, 0.0f} * math::deg2rad; + const auto procoxa_r_angles = procoxa_l_angles * math::fvec3{1, -1, 1}; + const auto profemur_l_angles = math::fvec3{60.0f, 0.0f, 0.0f} * math::deg2rad; + const auto profemur_r_angles = profemur_l_angles * math::fvec3{1, 1, 1}; + const auto protibia_l_angles = math::fvec3{-100.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protibia_r_angles = protibia_l_angles * math::fvec3{1, 1, 1}; + const auto protarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto protarsomere1_r_angles = protarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto procoxa_l_xf = math::transform::identity(); + auto procoxa_r_xf = math::transform::identity(); + auto profemur_l_xf = math::transform::identity(); + auto profemur_r_xf = math::transform::identity(); + auto protibia_l_xf = math::transform::identity(); + auto protibia_r_xf = math::transform::identity(); + auto protarsomere1_l_xf = math::transform::identity(); + auto protarsomere1_r_xf = math::transform::identity(); + + procoxa_l_xf.rotation = math::euler_xyz_to_quat(procoxa_l_angles); + procoxa_r_xf.rotation = math::euler_xyz_to_quat(procoxa_r_angles); + profemur_l_xf.rotation = math::euler_xyz_to_quat(profemur_l_angles); + profemur_r_xf.rotation = math::euler_xyz_to_quat(profemur_r_angles); + protibia_l_xf.rotation = math::euler_xyz_to_quat(protibia_l_angles); + protibia_r_xf.rotation = math::euler_xyz_to_quat(protibia_r_angles); + protarsomere1_l_xf.rotation = math::euler_xyz_to_quat(protarsomere1_l_angles); + protarsomere1_r_xf.rotation = math::euler_xyz_to_quat(protarsomere1_r_angles); + + pose.set_relative_transform(bones.procoxa_l, rest_pose.get_relative_transform(bones.procoxa_l) * procoxa_l_xf); + pose.set_relative_transform(bones.procoxa_r, rest_pose.get_relative_transform(bones.procoxa_r) * procoxa_r_xf); + pose.set_relative_transform(bones.profemur_l, rest_pose.get_relative_transform(bones.profemur_l) * profemur_l_xf); + pose.set_relative_transform(bones.profemur_r, rest_pose.get_relative_transform(bones.profemur_r) * profemur_r_xf); + pose.set_relative_transform(bones.protibia_l, rest_pose.get_relative_transform(bones.protibia_l) * protibia_l_xf); + pose.set_relative_transform(bones.protibia_r, rest_pose.get_relative_transform(bones.protibia_r) * protibia_r_xf); + pose.set_relative_transform(bones.protarsomere1_l, rest_pose.get_relative_transform(bones.protarsomere1_l) * protarsomere1_l_xf); + pose.set_relative_transform(bones.protarsomere1_r, rest_pose.get_relative_transform(bones.protarsomere1_r) * protarsomere1_r_xf); + } + + // Pose midlegs + { + const auto mesocoxa_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesocoxa_r_angles = mesocoxa_l_angles * math::fvec3{1, 1, 1}; + const auto mesofemur_l_angles = math::fvec3{60.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesofemur_r_angles = mesofemur_l_angles * math::fvec3{1, 1, 1}; + const auto mesotibia_l_angles = math::fvec3{-100.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotibia_r_angles = mesotibia_l_angles * math::fvec3{1, 1, 1}; + const auto mesotarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto mesotarsomere1_r_angles = mesotarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto mesocoxa_l_xf = math::transform::identity(); + auto mesocoxa_r_xf = math::transform::identity(); + auto mesofemur_l_xf = math::transform::identity(); + auto mesofemur_r_xf = math::transform::identity(); + auto mesotibia_l_xf = math::transform::identity(); + auto mesotibia_r_xf = math::transform::identity(); + auto mesotarsomere1_l_xf = math::transform::identity(); + auto mesotarsomere1_r_xf = math::transform::identity(); + + mesocoxa_l_xf.rotation = math::euler_xyz_to_quat(mesocoxa_l_angles); + mesocoxa_r_xf.rotation = math::euler_xyz_to_quat(mesocoxa_r_angles); + mesofemur_l_xf.rotation = math::euler_xyz_to_quat(mesofemur_l_angles); + mesofemur_r_xf.rotation = math::euler_xyz_to_quat(mesofemur_r_angles); + mesotibia_l_xf.rotation = math::euler_xyz_to_quat(mesotibia_l_angles); + mesotibia_r_xf.rotation = math::euler_xyz_to_quat(mesotibia_r_angles); + mesotarsomere1_l_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_l_angles); + mesotarsomere1_r_xf.rotation = math::euler_xyz_to_quat(mesotarsomere1_r_angles); + + pose.set_relative_transform(bones.mesocoxa_l, rest_pose.get_relative_transform(bones.mesocoxa_l) * mesocoxa_l_xf); + pose.set_relative_transform(bones.mesocoxa_r, rest_pose.get_relative_transform(bones.mesocoxa_r) * mesocoxa_r_xf); + pose.set_relative_transform(bones.mesofemur_l, rest_pose.get_relative_transform(bones.mesofemur_l) * mesofemur_l_xf); + pose.set_relative_transform(bones.mesofemur_r, rest_pose.get_relative_transform(bones.mesofemur_r) * mesofemur_r_xf); + pose.set_relative_transform(bones.mesotibia_l, rest_pose.get_relative_transform(bones.mesotibia_l) * mesotibia_l_xf); + pose.set_relative_transform(bones.mesotibia_r, rest_pose.get_relative_transform(bones.mesotibia_r) * mesotibia_r_xf); + pose.set_relative_transform(bones.mesotarsomere1_l, rest_pose.get_relative_transform(bones.mesotarsomere1_l) * mesotarsomere1_l_xf); + pose.set_relative_transform(bones.mesotarsomere1_r, rest_pose.get_relative_transform(bones.mesotarsomere1_r) * mesotarsomere1_r_xf); + } + + // Pose hindlegs + { + const auto metacoxa_l_angles = math::fvec3{0.0f, -37.5f, 0.0f} * math::deg2rad; + const auto metacoxa_r_angles = metacoxa_l_angles * math::fvec3{1, -1, 1}; + const auto metafemur_l_angles = math::fvec3{60.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metafemur_r_angles = metafemur_l_angles * math::fvec3{1, 1, 1}; + const auto metatibia_l_angles = math::fvec3{-100.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatibia_r_angles = metatibia_l_angles * math::fvec3{1, 1, 1}; + const auto metatarsomere1_l_angles = math::fvec3{0.0f, 0.0f, 0.0f} * math::deg2rad; + const auto metatarsomere1_r_angles = metatarsomere1_l_angles * math::fvec3{1, 1, 1}; + + auto metacoxa_l_xf = math::transform::identity(); + auto metacoxa_r_xf = math::transform::identity(); + auto metafemur_l_xf = math::transform::identity(); + auto metafemur_r_xf = math::transform::identity(); + auto metatibia_l_xf = math::transform::identity(); + auto metatibia_r_xf = math::transform::identity(); + auto metatarsomere1_l_xf = math::transform::identity(); + auto metatarsomere1_r_xf = math::transform::identity(); + + metacoxa_l_xf.rotation = math::euler_xyz_to_quat(metacoxa_l_angles); + metacoxa_r_xf.rotation = math::euler_xyz_to_quat(metacoxa_r_angles); + metafemur_l_xf.rotation = math::euler_xyz_to_quat(metafemur_l_angles); + metafemur_r_xf.rotation = math::euler_xyz_to_quat(metafemur_r_angles); + metatibia_l_xf.rotation = math::euler_xyz_to_quat(metatibia_l_angles); + metatibia_r_xf.rotation = math::euler_xyz_to_quat(metatibia_r_angles); + metatarsomere1_l_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_l_angles); + metatarsomere1_r_xf.rotation = math::euler_xyz_to_quat(metatarsomere1_r_angles); + + pose.set_relative_transform(bones.metacoxa_l, rest_pose.get_relative_transform(bones.metacoxa_l) * metacoxa_l_xf); + pose.set_relative_transform(bones.metacoxa_r, rest_pose.get_relative_transform(bones.metacoxa_r) * metacoxa_r_xf); + pose.set_relative_transform(bones.metafemur_l, rest_pose.get_relative_transform(bones.metafemur_l) * metafemur_l_xf); + pose.set_relative_transform(bones.metafemur_r, rest_pose.get_relative_transform(bones.metafemur_r) * metafemur_r_xf); + pose.set_relative_transform(bones.metatibia_l, rest_pose.get_relative_transform(bones.metatibia_l) * metatibia_l_xf); + pose.set_relative_transform(bones.metatibia_r, rest_pose.get_relative_transform(bones.metatibia_r) * metatibia_r_xf); + pose.set_relative_transform(bones.metatarsomere1_l, rest_pose.get_relative_transform(bones.metatarsomere1_l) * metatarsomere1_l_xf); + pose.set_relative_transform(bones.metatarsomere1_r, rest_pose.get_relative_transform(bones.metatarsomere1_r) * metatarsomere1_r_xf); + } + + pose.update(); +} + /** * Generates a pupa pose (named "pupa") for an ant skeleton. * @@ -512,5 +1037,9 @@ void generate_ant_skeleton(skeleton& skeleton, ant_bone_set& bones, const ant_ph // Generate poses generate_ant_rest_pose(skeleton, bones, phenome); + generate_ant_midstance_pose(skeleton, bones); + generate_ant_midswing_pose(skeleton, bones); + generate_ant_liftoff_pose(skeleton, bones); + generate_ant_touchdown_pose(skeleton, bones); generate_ant_pupa_pose(skeleton, bones); } diff --git a/src/game/components/legged-locomotion-component.hpp b/src/game/components/legged-locomotion-component.hpp index 83416d1..2797434 100644 --- a/src/game/components/legged-locomotion-component.hpp +++ b/src/game/components/legged-locomotion-component.hpp @@ -21,6 +21,10 @@ #define ANTKEEPER_GAME_LEGGED_LOCOMOTION_COMPONENT_HPP #include +#include +#include +#include +#include /** * Legged terrestrial locomotion. @@ -29,6 +33,22 @@ struct legged_locomotion_component { /// Force vector. math::fvec3 force{0.0f, 0.0f, 0.0f}; + + pose* current_pose{}; + const pose* midstance_pose{}; + const pose* midswing_pose{}; + const pose* liftoff_pose{}; + const pose* touchdown_pose{}; + + /// Indices of the the final bones in the legs. + std::vector tip_bones; + + /// Number of bones per leg. + std::uint8_t leg_bone_count{}; + + std::shared_ptr<::gait> gait; + + bool moving{}; }; #endif // ANTKEEPER_GAME_LEGGED_LOCOMOTION_COMPONENT_HPP diff --git a/src/game/states/experiments/treadmill-experiment-state.cpp b/src/game/states/experiments/treadmill-experiment-state.cpp index 90fdc07..ef6fbc6 100644 --- a/src/game/states/experiments/treadmill-experiment-state.cpp +++ b/src/game/states/experiments/treadmill-experiment-state.cpp @@ -84,6 +84,7 @@ #include #include #include +#include treadmill_experiment_state::treadmill_experiment_state(::game& ctx): game_state(ctx) @@ -136,7 +137,7 @@ treadmill_experiment_state::treadmill_experiment_state(::game& ctx): //const float color_temperature = 5000.0f; //const math::fvec3 light_color = color::aces::ap1.from_xyz * color::cat::matrix(color::illuminant::deg2::d50, color::aces::white_point) * color::cct::to_xyz(color_temperature); - const math::fvec3 light_color{1.0f, 1.0f, 1.0f}; + // const math::fvec3 light_color{1.0f, 1.0f, 1.0f}; // Create rectangle light // ctx.underground_rectangle_light = std::make_unique(); @@ -169,20 +170,61 @@ treadmill_experiment_state::treadmill_experiment_state(::game& ctx): // Create treadmill auto treadmill_eid = ctx.entity_registry->create(); scene_component treadmill_scene_component; - treadmill_scene_component.object = std::make_shared(ctx.resource_manager->load("cube-15cm.mdl")); + treadmill_scene_component.object = std::make_shared(ctx.resource_manager->load("cube-500mm.mdl")); treadmill_scene_component.layer_mask = 1; ctx.entity_registry->emplace(treadmill_eid, std::move(treadmill_scene_component)); // Create worker auto worker_skeletal_mesh = std::make_unique(worker_model); + + // Create worker IK rig + const auto& worker_skeleton = worker_model->get_skeleton(); + worker_ik_rig = std::make_shared(*worker_skeletal_mesh); + auto mesocoxa_ik_constraint = std::make_shared(); + mesocoxa_ik_constraint->set_min_angles({-math::pi, -math::pi, -math::pi}); + mesocoxa_ik_constraint->set_max_angles({ math::pi, math::pi, math::pi}); + worker_ik_rig->set_constraint(*worker_skeleton.get_bone_index("mesocoxa_l"), std::move(mesocoxa_ik_constraint)); + + // Pose worker + worker_skeletal_mesh->get_pose() = *worker_model->get_skeleton().get_pose("midswing"); + worker_eid = ctx.entity_registry->create(); transform_component worker_transform_component; worker_transform_component.local = math::transform::identity(); worker_transform_component.local.translation = {0, 0.1f, 0}; worker_transform_component.local.scale = math::fvec3::one() * worker_phenome.body_size->mean_mesosoma_length; worker_transform_component.world = worker_transform_component.local; + + legged_locomotion_component worker_locomotion_component; + worker_locomotion_component.current_pose = &worker_skeletal_mesh->get_pose(); + worker_locomotion_component.midstance_pose = worker_model->get_skeleton().get_pose("midstance"); + worker_locomotion_component.midswing_pose = worker_model->get_skeleton().get_pose("midswing"); + worker_locomotion_component.liftoff_pose = worker_model->get_skeleton().get_pose("liftoff"); + worker_locomotion_component.touchdown_pose = worker_model->get_skeleton().get_pose("touchdown"); + + worker_locomotion_component.tip_bones = + { + *worker_skeleton.get_bone_index("protarsomere1_l"), + *worker_skeleton.get_bone_index("mesotarsomere1_l"), + *worker_skeleton.get_bone_index("metatarsomere1_l"), + *worker_skeleton.get_bone_index("protarsomere1_r"), + *worker_skeleton.get_bone_index("mesotarsomere1_r"), + *worker_skeleton.get_bone_index("metatarsomere1_r") + }; + worker_locomotion_component.leg_bone_count = 4; + worker_locomotion_component.gait = std::make_shared<::gait>(); + worker_locomotion_component.gait->frequency = 4.0f; + worker_locomotion_component.gait->steps.resize(6); + for (std::size_t i = 0; i < 6; ++i) + { + auto& step = worker_locomotion_component.gait->steps[i]; + step.duty_factor = 0.5f; + step.delay = (i % 2) ? 0.5f : 0.0f; + } + ctx.entity_registry->emplace(worker_eid, worker_transform_component); ctx.entity_registry->emplace(worker_eid, std::move(worker_skeletal_mesh), std::uint8_t{1}); + ctx.entity_registry->emplace(worker_eid, std::move(worker_locomotion_component)); // Create color checker auto color_checker_eid = ctx.entity_registry->create(); @@ -244,16 +286,16 @@ treadmill_experiment_state::treadmill_experiment_state(::game& ctx): ctx.frame_scheduler.refresh(); // Load navmesh - navmesh = ctx.resource_manager->load("cube-15cm.msh"); + navmesh = ctx.resource_manager->load("cube-500mm.msh"); // Generate navmesh attributes geom::generate_face_normals(*navmesh); geom::generate_vertex_normals(*navmesh); // Build navmesh BVH - debug::log::info("building bvh"); + debug::log::info("Building BVH..."); navmesh_bvh = std::make_unique(*navmesh); - debug::log::info("building bvh done"); + debug::log::info("Built BVH"); debug::log::trace("Entered nest view state"); } @@ -715,10 +757,35 @@ void treadmill_experiment_state::setup_controls() update_third_person_camera(); } ); + ctx.entity_registry->patch<::legged_locomotion_component> + ( + worker_eid, + [&](auto& component) + { + component.moving = true; + } + ); } } ) ); + action_subscriptions.emplace_back + ( + ctx.move_forward_action.get_deactivated_channel().subscribe + ( + [&](const auto& event) + { + ctx.entity_registry->patch<::legged_locomotion_component> + ( + worker_eid, + [&](auto& component) + { + component.moving = false; + } + ); + } + ) + ); // Move back action_subscriptions.emplace_back diff --git a/src/game/states/experiments/treadmill-experiment-state.hpp b/src/game/states/experiments/treadmill-experiment-state.hpp index 39a2879..a293d37 100644 --- a/src/game/states/experiments/treadmill-experiment-state.hpp +++ b/src/game/states/experiments/treadmill-experiment-state.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include "game/ant/ant-phenome.hpp" class treadmill_experiment_state: public game_state @@ -80,7 +81,7 @@ private: double third_person_camera_zoom{0.25}; std::uint32_t third_person_camera_zoom_step_count{6}; - double third_person_camera_near_focal_plane_height{2.0f}; + double third_person_camera_near_focal_plane_height{1.0f}; double third_person_camera_far_focal_plane_height{50.0f}; double third_person_camera_near_hfov{math::radians(90.0)}; @@ -120,6 +121,7 @@ private: geom::brep_face* navmesh_agent_face{}; math::fvec3 navmesh_agent_position{}; math::fvec3 navmesh_agent_normal{}; + std::shared_ptr worker_ik_rig; std::shared_ptr sky_probe; }; diff --git a/src/game/states/nest-selection-state.cpp b/src/game/states/nest-selection-state.cpp index ba78b10..92e1626 100644 --- a/src/game/states/nest-selection-state.cpp +++ b/src/game/states/nest-selection-state.cpp @@ -190,7 +190,7 @@ nest_selection_state::nest_selection_state(::game& ctx): larva_ik_solver = std::make_shared(*larva_ik_rig, larva_ik_root_bone_index, larva_ik_effector_bone_index); larva_ik_solver->set_effector_position(larva_head_relative_transform * math::fvec3{0.0f, 0.0f, -0.0f}); - larva_ik_solver->set_goal_position(larva_head_absolute_transform.translation + math::fvec3{0.2f, 0.2f, 0.5f}); + larva_ik_solver->set_goal_center(larva_head_absolute_transform.translation + math::fvec3{0.2f, 0.2f, 0.5f}); diff --git a/src/game/systems/locomotion-system.cpp b/src/game/systems/locomotion-system.cpp index 8b174b9..cc7f3c5 100644 --- a/src/game/systems/locomotion-system.cpp +++ b/src/game/systems/locomotion-system.cpp @@ -21,7 +21,10 @@ #include "game/components/legged-locomotion-component.hpp" #include "game/components/winged-locomotion-component.hpp" #include "game/components/rigid-body-component.hpp" +#include "game/components/scene-component.hpp" #include +#include +#include #include #include @@ -32,7 +35,8 @@ locomotion_system::locomotion_system(entity::registry& registry): void locomotion_system::update(float t, float dt) { // Legged locomotion - auto legged_group = registry.group(entt::get); + // auto legged_group = registry.group(entt::get); + auto legged_group = registry.group(entt::get); std::for_each ( std::execution::par_unseq, @@ -41,10 +45,76 @@ void locomotion_system::update(float t, float dt) [&](auto entity_id) { const auto& locomotion = legged_group.get(entity_id); - auto& body = *(legged_group.get(entity_id).body); + if (!locomotion.moving) + { + return; + } + + //auto& body = *(legged_group.get(entity_id).body); // Apply locomotive force - body.apply_central_force(locomotion.force); + //body.apply_central_force(locomotion.force); + + // Animate legs + if (locomotion.current_pose) + { + // Get gait phase + float gait_phase = locomotion.gait->phase(t); + + // For each leg + for (std::size_t i = 0; i < locomotion.tip_bones.size(); ++i) + { + // Get step phase + float step_phase = locomotion.gait->steps[i].phase(gait_phase); + + // Determine leg pose + const pose* pose_a; + const pose* pose_b; + float t; + if (step_phase < 0.0f) + { + pose_b = locomotion.touchdown_pose; + pose_a = locomotion.liftoff_pose; + t = std::abs(step_phase); + } + else + { + if (step_phase < 0.5f) + { + pose_a = locomotion.liftoff_pose; + pose_b = locomotion.midswing_pose; + t = step_phase * 2.0f; + } + else + { + pose_a = locomotion.midswing_pose; + pose_b = locomotion.touchdown_pose; + t = (step_phase - 0.5f) * 2.0f; + } + } + + // Update leg bones + bone_index_type bone_index = locomotion.tip_bones[i]; + for (std::uint8_t j = 0; j < locomotion.leg_bone_count; ++j) + { + if (j) + { + bone_index = locomotion.current_pose->get_skeleton()->get_bone_parent(bone_index); + } + + const auto rotation_a = pose_a->get_relative_transform(bone_index).rotation; + const auto rotation_b = pose_b->get_relative_transform(bone_index).rotation; + + auto transform = pose_a->get_relative_transform(bone_index); + transform.rotation = math::nlerp(rotation_a, rotation_b, t); + + locomotion.current_pose->set_relative_transform(bone_index, transform); + } + + // Update subset of pose containing the leg bones + locomotion.current_pose->update(bone_index, locomotion.leg_bone_count); + } + } } );