From 77ff725962a64645b791881078cd42cb2504e8db Mon Sep 17 00:00:00 2001 From: "C. J. Howard" Date: Mon, 3 Apr 2023 18:10:55 +0800 Subject: [PATCH] Add IK system. Optimize some quaternion functions. Make active actions publish active events each frame --- CMakeLists.txt | 1 + src/engine/animation/animation-pose.cpp | 2 +- .../ik/constraints/euler-ik-constraint.cpp | 53 +++++ .../ik/constraints/euler-ik-constraint.hpp | 51 +++++ .../constraints/swing-twist-ik-constraint.cpp | 54 +++++ .../constraints/swing-twist-ik-constraint.hpp | 49 +++++ src/engine/animation/ik/ik-constraint.hpp | 39 ++++ src/engine/animation/ik/ik-rig.cpp | 56 +++++ src/engine/animation/ik/ik-rig.hpp | 116 ++++++++++ src/engine/animation/ik/ik-solver.hpp | 35 +++ .../animation/ik/solvers/ccd-ik-solver.cpp | 107 ++++++++++ .../animation/ik/solvers/ccd-ik-solver.hpp | 129 +++++++++++ src/engine/animation/pose.cpp | 2 +- src/engine/animation/pose.hpp | 33 +++ src/engine/animation/rest-pose.cpp | 2 +- src/engine/app/sdl/sdl-input-manager.cpp | 4 + src/engine/input/action-map.cpp | 200 ++++++++++++------ src/engine/input/action-map.hpp | 43 ++-- src/engine/input/action.cpp | 50 +++-- src/engine/input/action.hpp | 46 ++-- src/engine/input/input-update-event.hpp | 32 +++ src/engine/math/matrix.hpp | 158 ++++++-------- src/engine/math/polynomial.hpp | 13 +- src/engine/math/quaternion.hpp | 166 +++++++++------ src/engine/math/vector.hpp | 52 +---- src/engine/scene/directional-light.hpp | 2 +- src/game/components/ik-component.hpp | 31 +++ src/game/game.cpp | 6 + src/game/game.hpp | 3 + src/game/states/nest-selection-state.cpp | 45 +++- src/game/states/nest-selection-state.hpp | 4 +- src/game/systems/ik-system.cpp | 45 ++++ src/game/systems/ik-system.hpp | 36 ++++ 33 files changed, 1330 insertions(+), 335 deletions(-) create mode 100644 src/engine/animation/ik/constraints/euler-ik-constraint.cpp create mode 100644 src/engine/animation/ik/constraints/euler-ik-constraint.hpp create mode 100644 src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp create mode 100644 src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp create mode 100644 src/engine/animation/ik/ik-constraint.hpp create mode 100644 src/engine/animation/ik/ik-rig.cpp create mode 100644 src/engine/animation/ik/ik-rig.hpp create mode 100644 src/engine/animation/ik/ik-solver.hpp create mode 100644 src/engine/animation/ik/solvers/ccd-ik-solver.cpp create mode 100644 src/engine/animation/ik/solvers/ccd-ik-solver.hpp create mode 100644 src/engine/input/input-update-event.hpp create mode 100644 src/game/components/ik-component.hpp create mode 100644 src/game/systems/ik-system.cpp create mode 100644 src/game/systems/ik-system.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 21ca183..73e1a79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ 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/animation/animation-pose.cpp b/src/engine/animation/animation-pose.cpp index 59aea50..b516c50 100644 --- a/src/engine/animation/animation-pose.cpp +++ b/src/engine/animation/animation-pose.cpp @@ -42,7 +42,7 @@ void animation_pose::update(bone_index_type first_index, std::size_t bone_count) ( std::execution::par_unseq, m_matrix_palette.begin() + first_index, - m_matrix_palette.begin() + bone_count, + m_matrix_palette.begin() + (first_index + bone_count), [&](auto& skinning_matrix) { const bone_index_type bone_index = static_cast(&skinning_matrix - m_matrix_palette.data()); diff --git a/src/engine/animation/ik/constraints/euler-ik-constraint.cpp b/src/engine/animation/ik/constraints/euler-ik-constraint.cpp new file mode 100644 index 0000000..4fe8088 --- /dev/null +++ b/src/engine/animation/ik/constraints/euler-ik-constraint.cpp @@ -0,0 +1,53 @@ +/* + * 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 +#include + +void euler_ik_constraint::solve(math::quaternion& 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())); + + // 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; + + // 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::quaternion + { + cx * cy * cz + sx * sy * sz, + sx * cy * cz - cx * sy * sz, + cx * sy * cz + sx * cy * sz, + cx * cy * sz - sx * sy * cz + } + ); +} diff --git a/src/engine/animation/ik/constraints/euler-ik-constraint.hpp b/src/engine/animation/ik/constraints/euler-ik-constraint.hpp new file mode 100644 index 0000000..1901d3a --- /dev/null +++ b/src/engine/animation/ik/constraints/euler-ik-constraint.hpp @@ -0,0 +1,51 @@ +/* + * 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_EULER_IK_CONSTRAINT_HPP +#define ANTKEEPER_ANIMATION_EULER_IK_CONSTRAINT_HPP + +#include +#include + +/** + * Euler angle IK constraint. + */ +class euler_ik_constraint: public ik_constraint +{ +public: + void solve(math::quaternion& q) override; + + /** + * Sets the angular limits. + * + * @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. + */ + inline void set_limits(const math::vector& min, const math::vector& max) noexcept + { + m_min = min; + m_max = max; + } + +private: + math::vector m_min{-math::pi, -math::pi, -math::pi}; + math::vector m_max{ 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 new file mode 100644 index 0000000..87c2aab --- /dev/null +++ b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.cpp @@ -0,0 +1,54 @@ +/* + * 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 + +void swing_twist_ik_constraint::set_twist_limit(float angle_min, float angle_max) +{ + 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); +} + +void swing_twist_ik_constraint::solve(math::quaternion& q) +{ + constexpr math::vector twist_axis{0.0f, 0.0f, 1.0f}; + + // Decompose rotation into swing and twist components + math::quaternion swing; + math::quaternion twist; + math::swing_twist(q, twist_axis, swing, twist); + + // Limit twist + if (twist.z() < m_sin_half_twist_min) + { + twist.z() = m_sin_half_twist_min; + twist.w() = m_cos_half_twist_min; + } + else if (twist.z() > m_sin_half_twist_max) + { + twist.z() = m_sin_half_twist_max; + twist.w() = m_cos_half_twist_max; + } + + // Re-compose rotation from swing and twist components + q = math::normalize(swing * twist); +} diff --git a/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp new file mode 100644 index 0000000..16a1068 --- /dev/null +++ b/src/engine/animation/ik/constraints/swing-twist-ik-constraint.hpp @@ -0,0 +1,49 @@ +/* + * 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_SWING_TWIST_IK_CONSTRAINT_HPP +#define ANTKEEPER_ANIMATION_SWING_TWIST_IK_CONSTRAINT_HPP + +#include +#include + +/** + * IK constraint with cone-limited swing and angle-limited twist. + */ +class swing_twist_ik_constraint: public ik_constraint +{ +public: + void solve(math::quaternion& q) override; + + /** + * Sets the twist rotation limit. + * + * @param angle_min Minimum twist angle, in radians. + * @param angle_max Maximum twist angle, in radians. + */ + void set_twist_limit(float angle_min, float angle_max); + +private: + float m_cos_half_twist_min{ 0}; + float m_sin_half_twist_min{-1}; + float m_cos_half_twist_max{ 0}; + float m_sin_half_twist_max{ 1}; +}; + +#endif // ANTKEEPER_ANIMATION_SWING_TWIST_IK_CONSTRAINT_HPP diff --git a/src/engine/animation/ik/ik-constraint.hpp b/src/engine/animation/ik/ik-constraint.hpp new file mode 100644 index 0000000..555eeaf --- /dev/null +++ b/src/engine/animation/ik/ik-constraint.hpp @@ -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 . + */ + +#ifndef ANTKEEPER_ANIMATION_IK_CONSTRAINT_HPP +#define ANTKEEPER_ANIMATION_IK_CONSTRAINT_HPP + +#include + +/** + * Abstract base class for IK joint constraints. + */ +class ik_constraint +{ +public: + /** + * Solves the constraint. + * + * @param[in,out] q Unit quaternion representing the rotation of a joint. + */ + virtual void solve(math::quaternion& q) = 0; +}; + +#endif // ANTKEEPER_ANIMATION_IK_CONSTRAINT_HPP diff --git a/src/engine/animation/ik/ik-rig.cpp b/src/engine/animation/ik/ik-rig.cpp new file mode 100644 index 0000000..002991f --- /dev/null +++ b/src/engine/animation/ik/ik-rig.cpp @@ -0,0 +1,56 @@ +/* + * 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 + +ik_rig::ik_rig(scene::skeletal_mesh& skeletal_mesh): + m_skeletal_mesh(&skeletal_mesh), + m_constraints(skeletal_mesh.get_pose().get_skeleton()->get_bone_count()) +{} + +void ik_rig::set_constraint(bone_index_type index, std::shared_ptr constraint) +{ + m_constraints[index] = std::move(constraint); +} + +void ik_rig::clear_constraints() +{ + for (auto& constraint: m_constraints) + { + constraint.reset(); + } +} + +void ik_rig::solve() +{ + for (const auto& solver: m_solvers) + { + solver->solve(); + } +} + +void ik_rig::add_solver(std::shared_ptr solver) +{ + m_solvers.emplace_back(std::move(solver)); +} + +void ik_rig::remove_solvers() +{ + m_solvers.clear(); +} diff --git a/src/engine/animation/ik/ik-rig.hpp b/src/engine/animation/ik/ik-rig.hpp new file mode 100644 index 0000000..aa09022 --- /dev/null +++ b/src/engine/animation/ik/ik-rig.hpp @@ -0,0 +1,116 @@ +/* + * 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_RIG_HPP +#define ANTKEEPER_ANIMATION_IK_RIG_HPP + +#include +#include +#include +#include +#include + +/** + * + */ +class ik_rig +{ +public: + /** + * Constructs an IK rig. + * + * @param skeletal_mesh Skeletal mesh with which to associate the IK rig. + */ + explicit ik_rig(scene::skeletal_mesh& skeletal_mesh); + + /// Returns the skeleton with which the IK rig is associated. + /// @{ + [[nodiscard]] inline const scene::skeletal_mesh& get_skeletal_mesh() const noexcept + { + return *m_skeletal_mesh; + } + [[nodiscard]] inline scene::skeletal_mesh& get_skeletal_mesh() noexcept + { + return *m_skeletal_mesh; + } + /// @} + + /// @name Constraints + /// @{ + + /** + * Sets the IK constraint of a bone. + * + * @param index Index of a bone. + * @param constraint IK constraint of the bone. + */ + void set_constraint(bone_index_type index, std::shared_ptr constraint); + + /// Removes all constraints from the IK rig. + void clear_constraints(); + + /** + * Returns the IK constraint of a bone. + * + * @param index Index of a bone. + * + * @return Pointer to the IK constraint of the bone, or `nullptr` if the bone is unconstrained. + */ + /// @{ + [[nodiscard]] inline const ik_constraint* get_constraint(bone_index_type index) const + { + return m_constraints[index].get(); + } + [[nodiscard]] inline ik_constraint* get_constraint(bone_index_type index) + { + return m_constraints[index].get(); + } + /// @} + + /// @} + + /// @name Solvers + /// @{ + + /** + * Solves each solver in the IK rig. + */ + void solve(); + + /** + * Adds a solver to the IK rig. + * + * @param solver IK solver to add. + */ + void add_solver(std::shared_ptr solver); + + /** + * Removes all solvers from the IK rig. + */ + void remove_solvers(); + + /// @} + +private: + scene::skeletal_mesh* m_skeletal_mesh{nullptr}; + std::vector> m_constraints; + std::vector> m_solvers; +}; + +#endif // ANTKEEPER_ANIMATION_IK_RIG_HPP diff --git a/src/engine/animation/ik/ik-solver.hpp b/src/engine/animation/ik/ik-solver.hpp new file mode 100644 index 0000000..d56aea7 --- /dev/null +++ b/src/engine/animation/ik/ik-solver.hpp @@ -0,0 +1,35 @@ +/* + * 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_SOLVER_HPP +#define ANTKEEPER_ANIMATION_IK_SOLVER_HPP + +/** + * Abstract base class for IK solvers. + */ +class ik_solver +{ +public: + /** + * Transforms bones to find an inverse kinematic solution for an end effector. + */ + virtual void solve() = 0; +}; + +#endif // ANTKEEPER_ANIMATION_IK_SOLVER_HPP diff --git a/src/engine/animation/ik/solvers/ccd-ik-solver.cpp b/src/engine/animation/ik/solvers/ccd-ik-solver.cpp new file mode 100644 index 0000000..2af12ef --- /dev/null +++ b/src/engine/animation/ik/solvers/ccd-ik-solver.cpp @@ -0,0 +1,107 @@ +/* + * 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 +#include +#include + +ccd_ik_solver::ccd_ik_solver(ik_rig& ik_rig, bone_index_type root_bone_index, bone_index_type effector_bone_index): + m_ik_rig{&ik_rig} +{ + // Get reference to skeleton + const auto& skeleton = *m_ik_rig->get_skeletal_mesh().get_pose().get_skeleton(); + + // Validate and count number of bones in bone chain + std::size_t bone_count = 1; + for (bone_index_type bone_index = effector_bone_index; bone_index != root_bone_index; ++bone_count) + { + const auto parent_bone = skeleton.get_bone_parent(bone_index); + if (parent_bone == bone_index) + { + throw std::invalid_argument("Invalid bone chain"); + } + + bone_index = parent_bone; + } + + // Allocate and store bone indices + m_bone_indices.resize(bone_count); + m_bone_indices.front() = effector_bone_index; + for (std::size_t i = 1; i < bone_count; ++i) + { + m_bone_indices[i] = skeleton.get_bone_parent(m_bone_indices[i - 1]); + } +} + +void ccd_ik_solver::solve() +{ + // Get reference to skeletal mesh and its pose from the parent IK rig + auto& skeletal_mesh = m_ik_rig->get_skeletal_mesh(); + auto& pose = skeletal_mesh.get_pose(); + + // Get pose-space transform of end effector bone + 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(); + + 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; + + // 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) + { + return; + } + + // Get index of current bone + bone_index_type 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 + 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); + + // Calculate rotation of 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 + if (auto* constraint = m_ik_rig->get_constraint(bone_index)) + { + constraint->solve(bone_rotation); + } + + // Rotate current bone + pose.set_relative_rotation(bone_index, bone_rotation); + //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 new file mode 100644 index 0000000..930e071 --- /dev/null +++ b/src/engine/animation/ik/solvers/ccd-ik-solver.hpp @@ -0,0 +1,129 @@ +/* + * 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_CCD_IK_SOLVER_HPP +#define ANTKEEPER_ANIMATION_CCD_IK_SOLVER_HPP + +#include +#include +#include + +class ik_rig; + +/** + * Cyclic Coordinate Descent (CCD) IK solver. + */ +class ccd_ik_solver: public ik_solver +{ +public: + /** + * Constructs a CCD IK solver. + * + * @param ik_rig IK rig with which to associate this IK solver. + * @param root_bone_index Index of the first bone in the bone chain. + * @param effector_bone_index Index of the last bone in the bone chain. + * @param chain_length Number of bones in the IK chain. + */ + ccd_ik_solver(ik_rig& ik_rig, bone_index_type root_bone_index, bone_index_type effector_bone_index); + + /// @name Solving + /// @{ + + void solve() override; + + /** + * Sets the maximum number of solving iterations. + * + * @param iterations Maximum number of solving iterations. + */ + inline void set_max_iterations(std::size_t iterations) noexcept + { + 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 + { + return m_max_iterations; + } + + /// @} + + /// @name Effector + /// @{ + + /** + * Sets the position of the end effector. + * + * @param position Position of the end effector, relative to the tip bone. + */ + inline void set_effector_position(const math::vector& position) noexcept + { + m_effector_position = position; + } + + /// Returns the position of the end effector, relative to the tip bone. + [[nodiscard]] inline const math::vector& get_effector_position() const + { + return m_effector_position; + } + + /// @} + + /// @name Goal + /// @{ + + /** + * Thes the position of the IK goal. + * + * @param position IK goal position, in world-space. + */ + inline void set_goal_position(const math::vector& position) noexcept + { + m_goal_position = position; + } + + /// Returns the position of goal, in world-space. + [[nodiscard]] inline const math::vector& get_goal_position() const + { + return m_goal_position; + } + + /// @} + +private: + ik_rig* m_ik_rig{nullptr}; + std::size_t m_max_iterations{10}; + std::vector m_bone_indices; + math::vector m_effector_position{0.0f, 0.0f, 0.0f}; + math::vector m_goal_position{0.0f, 0.0f, 0.0f}; + float m_sqr_distance_tolerance{1e-5f}; +}; + +#endif // ANTKEEPER_ANIMATION_CCD_IK_SOLVER_HPP diff --git a/src/engine/animation/pose.cpp b/src/engine/animation/pose.cpp index ec52432..4931b72 100644 --- a/src/engine/animation/pose.cpp +++ b/src/engine/animation/pose.cpp @@ -41,7 +41,7 @@ void pose::update(bone_index_type first_index, std::size_t bone_count) ( std::execution::seq, m_absolute_transforms.begin() + first_index, - m_absolute_transforms.begin() + bone_count, + m_absolute_transforms.begin() + (first_index + bone_count), [&](auto& child_absolute_transform) { const bone_index_type parent_index = m_skeleton->get_bone_parent(child_index); diff --git a/src/engine/animation/pose.hpp b/src/engine/animation/pose.hpp index 4c73a24..852250a 100644 --- a/src/engine/animation/pose.hpp +++ b/src/engine/animation/pose.hpp @@ -67,6 +67,39 @@ public: m_relative_transforms[index] = transform; } + /** + * Sets the relative translation of a bone pose. + * + * @param index Index of a bone. + * @param translation Relative translation of the bone pose. + */ + inline void set_relative_translation(bone_index_type index, const bone_transform_type::vector_type& translation) + { + m_relative_transforms[index].translation = translation; + } + + /** + * Sets the relative rotation of a bone pose. + * + * @param index Index of a bone. + * @param translation Relative rotation of the bone pose. + */ + inline void set_relative_rotation(bone_index_type index, const bone_transform_type::quaternion_type& rotation) + { + m_relative_transforms[index].rotation = rotation; + } + + /** + * Sets the relative scale of a bone pose. + * + * @param index Index of a bone. + * @param scale Relative scale of the bone pose. + */ + inline void set_relative_scale(bone_index_type index, const bone_transform_type::vector_type& scale) + { + m_relative_transforms[index].scale = scale; + } + /// Returns the skeleton with which the pose is associated. [[nodiscard]] inline const skeleton* get_skeleton() const noexcept { diff --git a/src/engine/animation/rest-pose.cpp b/src/engine/animation/rest-pose.cpp index c381012..ae6a94e 100644 --- a/src/engine/animation/rest-pose.cpp +++ b/src/engine/animation/rest-pose.cpp @@ -37,7 +37,7 @@ void rest_pose::update(bone_index_type first_index, std::size_t bone_count) ( std::execution::par_unseq, m_inverse_absolute_transforms.begin() + first_index, - m_inverse_absolute_transforms.begin() + bone_count, + m_inverse_absolute_transforms.begin() + (first_index + bone_count), [&](auto& inverse_absolute_transform) { bone_index_type bone_index = static_cast(&inverse_absolute_transform - m_inverse_absolute_transforms.data()); diff --git a/src/engine/app/sdl/sdl-input-manager.cpp b/src/engine/app/sdl/sdl-input-manager.cpp index 0c58630..34b57a1 100644 --- a/src/engine/app/sdl/sdl-input-manager.cpp +++ b/src/engine/app/sdl/sdl-input-manager.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -312,6 +313,9 @@ void sdl_input_manager::update() break; } } + + // Dispatch input update event + this->m_event_dispatcher.dispatch({}); } void sdl_input_manager::set_cursor_visible(bool visible) diff --git a/src/engine/input/action-map.cpp b/src/engine/input/action-map.cpp index 3526808..a795802 100644 --- a/src/engine/input/action-map.cpp +++ b/src/engine/input/action-map.cpp @@ -27,51 +27,59 @@ namespace input { void action_map::enable() { - if (!enabled) + if (!m_enabled) { - if (event_dispatcher) + if (m_event_dispatcher) { subscribe(); } - enabled = true; + m_enabled = true; } } void action_map::disable() { - if (enabled) + if (m_enabled) { - if (event_dispatcher) + if (m_event_dispatcher) { unsubscribe(); } - enabled = false; + m_enabled = false; + } +} + +void action_map::reset() +{ + for (auto action: m_actions) + { + action->reset(); } } void action_map::set_event_dispatcher(event::dispatcher* dispatcher) { - if (event_dispatcher != dispatcher) + if (m_event_dispatcher != dispatcher) { - if (enabled) + if (m_enabled) { - if (event_dispatcher) + if (m_event_dispatcher) { unsubscribe(); } - event_dispatcher = dispatcher; + m_event_dispatcher = dispatcher; - if (event_dispatcher) + if (m_event_dispatcher) { subscribe(); } } else { - event_dispatcher = dispatcher; + m_event_dispatcher = dispatcher; } } } @@ -112,35 +120,41 @@ void action_map::add_mapping(action& action, const mapping& mapping) void action_map::add_gamepad_axis_mapping(action& action, gamepad_axis_mapping mapping) { - gamepad_axis_mappings.emplace_back(&action, std::move(mapping)); + m_gamepad_axis_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } void action_map::add_gamepad_button_mapping(action& action, gamepad_button_mapping mapping) { - gamepad_button_mappings.emplace_back(&action, std::move(mapping)); + m_gamepad_button_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } void action_map::add_key_mapping(action& action, key_mapping mapping) { - key_mappings.emplace_back(&action, std::move(mapping)); + m_key_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } void action_map::add_mouse_button_mapping(action& action, mouse_button_mapping mapping) { - mouse_button_mappings.emplace_back(&action, std::move(mapping)); + m_mouse_button_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } void action_map::add_mouse_motion_mapping(action& action, mouse_motion_mapping mapping) { - mouse_motion_mappings.emplace_back(&action, std::move(mapping)); + m_mouse_motion_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } void action_map::add_mouse_scroll_mapping(action& action, mouse_scroll_mapping mapping) { - mouse_scroll_mappings.emplace_back(&action, std::move(mapping)); + m_mouse_scroll_mappings.emplace_back(&action, std::move(mapping)); + m_actions.emplace(&action); } -void action_map::remove_mappings(const action& action, mapping_type type) +void action_map::remove_mappings(action& action, mapping_type type) { auto predicate = [&](const auto& tuple) -> bool { @@ -150,63 +164,112 @@ void action_map::remove_mappings(const action& action, mapping_type type) switch (type) { case mapping_type::gamepad_axis: - std::erase_if(gamepad_axis_mappings, predicate); + std::erase_if(m_gamepad_axis_mappings, predicate); break; case mapping_type::gamepad_button: - std::erase_if(gamepad_button_mappings, predicate); + std::erase_if(m_gamepad_button_mappings, predicate); break; case mapping_type::key: - std::erase_if(key_mappings, predicate); + std::erase_if(m_key_mappings, predicate); break; case mapping_type::mouse_button: - std::erase_if(mouse_button_mappings, predicate); + std::erase_if(m_mouse_button_mappings, predicate); break; case mapping_type::mouse_motion: - std::erase_if(mouse_motion_mappings, predicate); + std::erase_if(m_mouse_motion_mappings, predicate); break; case mapping_type::mouse_scroll: - std::erase_if(mouse_scroll_mappings, predicate); + std::erase_if(m_mouse_scroll_mappings, predicate); break; default: //std::unreachable(); break; } + + for (const auto& entry: m_gamepad_axis_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + for (const auto& entry: m_gamepad_button_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + for (const auto& entry: m_key_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + for (const auto& entry: m_mouse_button_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + for (const auto& entry: m_mouse_motion_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + for (const auto& entry: m_mouse_scroll_mappings) + { + if (std::get<0>(entry) == &action) + { + return; + } + } + + m_actions.erase(&action); } -void action_map::remove_mappings(const action& action) +void action_map::remove_mappings(action& action) { auto predicate = [&](const auto& tuple) -> bool { return std::get<0>(tuple) == &action; }; - std::erase_if(gamepad_axis_mappings, predicate); - std::erase_if(gamepad_button_mappings, predicate); - std::erase_if(key_mappings, predicate); - std::erase_if(mouse_button_mappings, predicate); - std::erase_if(mouse_motion_mappings, predicate); - std::erase_if(mouse_scroll_mappings, predicate); + std::erase_if(m_gamepad_axis_mappings, predicate); + std::erase_if(m_gamepad_button_mappings, predicate); + std::erase_if(m_key_mappings, predicate); + std::erase_if(m_mouse_button_mappings, predicate); + std::erase_if(m_mouse_motion_mappings, predicate); + std::erase_if(m_mouse_scroll_mappings, predicate); + + m_actions.erase(&action); } void action_map::remove_mappings() { - gamepad_axis_mappings.clear(); - gamepad_button_mappings.clear(); - key_mappings.clear(); - mouse_button_mappings.clear(); - mouse_motion_mappings.clear(); - mouse_scroll_mappings.clear(); + m_gamepad_axis_mappings.clear(); + m_gamepad_button_mappings.clear(); + m_key_mappings.clear(); + m_mouse_button_mappings.clear(); + m_mouse_motion_mappings.clear(); + m_mouse_scroll_mappings.clear(); + + m_actions.clear(); } void action_map::handle_gamepad_axis_moved(const gamepad_axis_moved_event& event) { - for (const auto& [action, mapping]: gamepad_axis_mappings) + for (const auto& [action, mapping]: m_gamepad_axis_mappings) { if (mapping.axis == event.axis && (!mapping.gamepad || mapping.gamepad == event.gamepad)) @@ -225,7 +288,7 @@ void action_map::handle_gamepad_axis_moved(const gamepad_axis_moved_event& event void action_map::handle_gamepad_button_pressed(const gamepad_button_pressed_event& event) { - for (const auto& [action, mapping]: gamepad_button_mappings) + for (const auto& [action, mapping]: m_gamepad_button_mappings) { if (mapping.button == event.button && (!mapping.gamepad || mapping.gamepad == event.gamepad)) @@ -237,7 +300,7 @@ void action_map::handle_gamepad_button_pressed(const gamepad_button_pressed_even void action_map::handle_gamepad_button_released(const gamepad_button_released_event& event) { - for (const auto& [action, mapping]: gamepad_button_mappings) + for (const auto& [action, mapping]: m_gamepad_button_mappings) { if (mapping.button == event.button && (!mapping.gamepad || mapping.gamepad == event.gamepad)) @@ -249,7 +312,7 @@ void action_map::handle_gamepad_button_released(const gamepad_button_released_ev void action_map::handle_key_pressed(const key_pressed_event& event) { - for (const auto& [action, mapping]: key_mappings) + for (const auto& [action, mapping]: m_key_mappings) { if (mapping.scancode == event.scancode && (!mapping.keyboard || mapping.keyboard == event.keyboard) && @@ -270,7 +333,7 @@ void action_map::handle_key_pressed(const key_pressed_event& event) void action_map::handle_key_released(const key_released_event& event) { - for (const auto& [action, mapping]: key_mappings) + for (const auto& [action, mapping]: m_key_mappings) { if (mapping.scancode == event.scancode && (!mapping.keyboard || mapping.keyboard == event.keyboard)) @@ -282,7 +345,7 @@ void action_map::handle_key_released(const key_released_event& event) void action_map::handle_mouse_moved(const mouse_moved_event& event) { - for (const auto& [action, mapping]: mouse_motion_mappings) + for (const auto& [action, mapping]: m_mouse_motion_mappings) { if (!mapping.mouse || mapping.mouse == event.mouse) { @@ -299,7 +362,7 @@ void action_map::handle_mouse_moved(const mouse_moved_event& event) void action_map::handle_mouse_scrolled(const mouse_scrolled_event& event) { - for (const auto& [action, mapping]: mouse_scroll_mappings) + for (const auto& [action, mapping]: m_mouse_scroll_mappings) { if (!mapping.mouse || mapping.mouse == event.mouse) { @@ -316,7 +379,7 @@ void action_map::handle_mouse_scrolled(const mouse_scrolled_event& event) void action_map::handle_mouse_button_pressed(const mouse_button_pressed_event& event) { - for (const auto& [action, mapping]: mouse_button_mappings) + for (const auto& [action, mapping]: m_mouse_button_mappings) { if (mapping.button == event.button && (!mapping.mouse || mapping.mouse == event.mouse)) @@ -328,7 +391,7 @@ void action_map::handle_mouse_button_pressed(const mouse_button_pressed_event& e void action_map::handle_mouse_button_released(const mouse_button_released_event& event) { - for (const auto& [action, mapping]: mouse_button_mappings) + for (const auto& [action, mapping]: m_mouse_button_mappings) { if (mapping.button == event.button && (!mapping.mouse || mapping.mouse == event.mouse)) @@ -338,11 +401,19 @@ void action_map::handle_mouse_button_released(const mouse_button_released_event& } } +void action_map::handle_update(const update_event& event) +{ + for (const auto* action: m_actions) + { + action->update(); + } +} + std::vector action_map::get_gamepad_axis_mappings(const action& action) const { std::vector mappings; - for (const auto& [mapped_action, mapping]: gamepad_axis_mappings) + for (const auto& [mapped_action, mapping]: m_gamepad_axis_mappings) { if (mapped_action == &action) { @@ -352,12 +423,12 @@ std::vector action_map::get_gamepad_axis_mappings(const ac return mappings; } - + std::vector action_map::get_gamepad_button_mappings(const action& action) const { std::vector mappings; - for (const auto& [mapped_action, mapping]: gamepad_button_mappings) + for (const auto& [mapped_action, mapping]: m_gamepad_button_mappings) { if (mapped_action == &action) { @@ -372,7 +443,7 @@ std::vector action_map::get_key_mappings(const action& action) cons { std::vector mappings; - for (const auto& [mapped_action, mapping]: key_mappings) + for (const auto& [mapped_action, mapping]: m_key_mappings) { if (mapped_action == &action) { @@ -387,7 +458,7 @@ std::vector action_map::get_mouse_button_mappings(const ac { std::vector mappings; - for (const auto& [mapped_action, mapping]: mouse_button_mappings) + for (const auto& [mapped_action, mapping]: m_mouse_button_mappings) { if (mapped_action == &action) { @@ -402,7 +473,7 @@ std::vector action_map::get_mouse_motion_mappings(const ac { std::vector mappings; - for (const auto& [mapped_action, mapping]: mouse_motion_mappings) + for (const auto& [mapped_action, mapping]: m_mouse_motion_mappings) { if (mapped_action == &action) { @@ -417,7 +488,7 @@ std::vector action_map::get_mouse_scroll_mappings(const ac { std::vector mappings; - for (const auto& [mapped_action, mapping]: mouse_scroll_mappings) + for (const auto& [mapped_action, mapping]: m_mouse_scroll_mappings) { if (mapped_action == &action) { @@ -430,20 +501,21 @@ std::vector action_map::get_mouse_scroll_mappings(const ac void action_map::subscribe() { - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_axis_moved, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_button_pressed, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_button_released, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_key_pressed, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_key_released, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_button_pressed, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_button_released, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_moved, this))); - subscriptions.emplace_back(event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_scrolled, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_axis_moved, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_button_pressed, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_gamepad_button_released, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_key_pressed, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_key_released, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_button_pressed, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_button_released, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_moved, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_mouse_scrolled, this))); + m_subscriptions.emplace_back(m_event_dispatcher->subscribe(std::bind_front(&action_map::handle_update, this))); } void action_map::unsubscribe() { - subscriptions.clear(); + m_subscriptions.clear(); } } // namespace input diff --git a/src/engine/input/action-map.hpp b/src/engine/input/action-map.hpp index efc3a5f..541c59b 100644 --- a/src/engine/input/action-map.hpp +++ b/src/engine/input/action-map.hpp @@ -27,9 +27,11 @@ #include #include #include +#include #include #include #include +#include #include namespace input { @@ -50,6 +52,11 @@ public: */ void disable(); + /** + * Resets the activation states of each action in the action map. + */ + void reset(); + /** * Sets the event dispatcher from which this action map will receive input events. * @@ -79,14 +86,14 @@ public: * @param action Action from which input will be unmapped. * @param type Type of input mapping to remove. */ - void remove_mappings(const action& action, mapping_type type); + void remove_mappings(action& action, mapping_type type); /** * Unmaps all input from an action. * * @param action Action from which input will be unmapped. */ - void remove_mappings(const action& action); + void remove_mappings(action& action); /** * Unmaps all input from all actions in the action map. @@ -98,42 +105,42 @@ public: * * @param action Action with which associated mappings will be returned. */ - std::vector get_gamepad_axis_mappings(const action& action) const; + [[nodiscard]] std::vector get_gamepad_axis_mappings(const action& action) const; /** * Returns all of the gamepad button mappings associated with an action. * * @param action Action with which associated mappings will be returned. */ - std::vector get_gamepad_button_mappings(const action& action) const; + [[nodiscard]] std::vector get_gamepad_button_mappings(const action& action) const; /** * Returns all of the key mappings associated with an action. * * @param action Action with which associated mappings will be returned. */ - std::vector get_key_mappings(const action& action) const; + [[nodiscard]] std::vector get_key_mappings(const action& action) const; /** * Returns all of the mouse button mappings associated with an action. * * @param action Action with which associated mappings will be returned. */ - std::vector get_mouse_button_mappings(const action& action) const; + [[nodiscard]] std::vector get_mouse_button_mappings(const action& action) const; /** * Returns all of the mouse motion mappings associated with an action. * * @param action Action with which associated mappings will be returned. */ - std::vector get_mouse_motion_mappings(const action& action) const; + [[nodiscard]] std::vector get_mouse_motion_mappings(const action& action) const; /** * Returns all of the mouse scroll associated with an action. * * @param action Action with which associated mappings will be returned. */ - std::vector get_mouse_scroll_mappings(const action& action) const; + [[nodiscard]] std::vector get_mouse_scroll_mappings(const action& action) const; private: void subscribe(); @@ -148,16 +155,18 @@ private: void handle_mouse_button_released(const mouse_button_released_event& event); void handle_mouse_moved(const mouse_moved_event& event); void handle_mouse_scrolled(const mouse_scrolled_event& event); + void handle_update(const update_event& event); - event::dispatcher* event_dispatcher{nullptr}; - bool enabled{false}; - std::vector> subscriptions; - std::vector> gamepad_axis_mappings; - std::vector> gamepad_button_mappings; - std::vector> key_mappings; - std::vector> mouse_button_mappings; - std::vector> mouse_motion_mappings; - std::vector> mouse_scroll_mappings; + event::dispatcher* m_event_dispatcher{nullptr}; + bool m_enabled{false}; + std::unordered_set m_actions; + std::vector> m_subscriptions; + std::vector> m_gamepad_axis_mappings; + std::vector> m_gamepad_button_mappings; + std::vector> m_key_mappings; + std::vector> m_mouse_button_mappings; + std::vector> m_mouse_motion_mappings; + std::vector> m_mouse_scroll_mappings; }; } // namespace input diff --git a/src/engine/input/action.cpp b/src/engine/input/action.cpp index cd11c21..8cb5368 100644 --- a/src/engine/input/action.cpp +++ b/src/engine/input/action.cpp @@ -21,55 +21,61 @@ namespace input { -static bool default_threshold_function(float x) noexcept +namespace { + +inline bool default_threshold_function(float x) noexcept { return x > 0.0f; } +} // namespace + action::action(): - threshold_function(default_threshold_function), - active(false), - activated_event{this}, - active_event{this, 0.0f}, - deactivated_event{this} + m_threshold_function(default_threshold_function) {} -void action::set_threshold_function(const threshold_function_type& function) -{ - threshold_function = function; -} - void action::evaluate(float value) { + // Update input value + m_active_event.input_value = value; + // Store activation state - const bool was_active = active; + const bool was_active = m_active; // Re-evaluate activation state - active = threshold_function(value); + m_active = m_threshold_function(value); - // Emit events - if (active) + if (m_active) { if (!was_active) { - activated_publisher.publish(activated_event); + // Publish activated event + m_activated_publisher.publish(m_activated_event); } - - active_event.input_value = value; - active_publisher.publish(active_event); } else { if (was_active) { - deactivated_publisher.publish(deactivated_event); + // Publish deactivated event + m_deactivated_publisher.publish(m_deactivated_event); } } } -void action::reset() +void action::update() const +{ + if (m_active) + { + // Publish active event + m_active_publisher.publish(m_active_event); + } +} + +void action::reset() noexcept { - active = false; + m_active = false; + m_active_event.input_value = 0.0f; } } // namespace input diff --git a/src/engine/input/action.hpp b/src/engine/input/action.hpp index 83174f1..b7846f5 100644 --- a/src/engine/input/action.hpp +++ b/src/engine/input/action.hpp @@ -37,7 +37,7 @@ public: * * Given an input value, returns `true` if the action should be considered active, and `false` otherwise. */ - typedef std::function threshold_function_type; + using threshold_function_type = std::function; /// Constructs an action. action(); @@ -47,7 +47,10 @@ public: * * @param function Threshold function. */ - void set_threshold_function(const threshold_function_type& function); + inline void set_threshold_function(const threshold_function_type& function) noexcept + { + m_threshold_function = function; + } /** * Evaluates the activation state of the action, according to its threshold function and an input value. @@ -56,52 +59,63 @@ public: */ void evaluate(float value); + /** + * Publishes an action active event if the action is active. + */ + void update() const; + /** * Resets the activation state of the action without publishing any events. */ - void reset(); + void reset() noexcept; /// Returns the threshold function. [[nodiscard]] inline const threshold_function_type& get_threshold_function() const noexcept { - return threshold_function; + return m_threshold_function; } /// Returns `true` if the action is active, `false` otherwise. [[nodiscard]] inline bool is_active() const noexcept { - return active; + return m_active; + } + + /// Returns the msot recently evaluated input value. + [[nodiscard]] inline float get_input_value() const noexcept + { + return m_active_event.input_value; } /// Returns the channel through which action activated events are published. [[nodiscard]] inline ::event::channel& get_activated_channel() noexcept { - return activated_publisher.channel(); + return m_activated_publisher.channel(); } /// Returns the channel through which action active events are published. [[nodiscard]] inline ::event::channel& get_active_channel() noexcept { - return active_publisher.channel(); + return m_active_publisher.channel(); } /// Returns the channel through which action deactivated events are published. [[nodiscard]] inline ::event::channel& get_deactivated_channel() noexcept { - return deactivated_publisher.channel(); + return m_deactivated_publisher.channel(); } private: - threshold_function_type threshold_function; - bool active{false}; + threshold_function_type m_threshold_function; + bool m_active{false}; - action_activated_event activated_event; - action_active_event active_event; - action_deactivated_event deactivated_event; + action_activated_event m_activated_event{this}; + action_active_event m_active_event{this, 0.0f}; + action_deactivated_event m_deactivated_event{this}; - ::event::publisher activated_publisher; - ::event::publisher active_publisher; - ::event::publisher deactivated_publisher; + ::event::publisher m_activated_publisher; + ::event::publisher m_active_publisher; + ::event::publisher m_deactivated_publisher; }; } // namespace input diff --git a/src/engine/input/input-update-event.hpp b/src/engine/input/input-update-event.hpp new file mode 100644 index 0000000..7ae2381 --- /dev/null +++ b/src/engine/input/input-update-event.hpp @@ -0,0 +1,32 @@ +/* + * 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_INPUT_UPDATE_EVENT_HPP +#define ANTKEEPER_INPUT_UPDATE_EVENT_HPP + +namespace input { + +/** + * Event generated after input events are polled. + */ +struct update_event {}; + +} // namespace input + +#endif // ANTKEEPER_INPUT_UPDATE_EVENT_HPP diff --git a/src/engine/math/matrix.hpp b/src/engine/math/matrix.hpp index 5b4d261..a7b52c5 100644 --- a/src/engine/math/matrix.hpp +++ b/src/engine/math/matrix.hpp @@ -22,9 +22,7 @@ #include #include -#include #include -#include #include #include @@ -39,11 +37,11 @@ namespace math { * * @see https://en.wikipedia.org/wiki/Row-_and_column-major_order */ -template +template struct matrix { /// Element type. - typedef T element_type; + using element_type = T; /// Number of columns. static constexpr std::size_t column_count = N; @@ -55,10 +53,10 @@ struct matrix static constexpr std::size_t element_count = column_count * row_count; /// Matrix column vector data type. - typedef vector column_vector_type; + using column_vector_type = vector; /// Matrix row vector data type. - typedef vector row_vector_type; + using row_vector_type = vector; /// Array of matrix column vectors. column_vector_type columns[column_count]; @@ -359,27 +357,27 @@ struct matrix }; /// 2x2 matrix. -template +template using matrix2 = matrix; /// 2x2 matrix. -template +template using matrix2x2 = matrix; /// 3x3 matrix. -template +template using matrix3 = matrix; /// 3x3 matrix. -template +template using matrix3x3 = matrix; /// 4x4 matrix. -template +template using matrix4 = matrix; /// 4x4 matrix. -template +template using matrix4x4 = matrix; /** @@ -500,7 +498,7 @@ template * * @param position Position of the view point. * @param target Position of the target. - * @param up Normalized direction of the up vector. + * @param up Unit vector pointing in the up direction. * * @return Viewing transformation matrix. */ @@ -520,7 +518,7 @@ template * * @return Product of `a * b`. */ -template +template [[nodiscard]] constexpr matrix mul(const matrix& a, const matrix& b) noexcept; /** @@ -542,7 +540,7 @@ template * * @return Product of the matrix and the row vector. */ -template +template [[nodiscard]] constexpr typename matrix::column_vector_type mul(const matrix& a, const typename matrix::row_vector_type& b) noexcept; /** @@ -553,7 +551,7 @@ template * * @return Product of the column vector and the matrix. */ -template +template [[nodiscard]] constexpr typename matrix::row_vector_type mul(const typename matrix::column_vector_type& a, const matrix& b) noexcept; /** @@ -669,7 +667,7 @@ template * * @return Transposed matrix. */ -template +template [[nodiscard]] constexpr matrix transpose(const matrix& m) noexcept; /// @private @@ -893,18 +891,18 @@ constexpr matrix look_at(const vector& position, const vector right = normalize(cross(forward, up)); up = cross(right, forward); - matrix m = - {{ - {right[0], up[0], -forward[0], T(0)}, - {right[1], up[1], -forward[1], T(0)}, - {right[2], up[2], -forward[2], T(0)}, - {T(0), T(0), T(0), T(1)} - }}; + matrix m + {{ + {right[0], up[0], -forward[0], T{0}}, + {right[1], up[1], -forward[1], T{0}}, + {right[2], up[2], -forward[2], T{0}}, + {T{0}, T{0}, T{0}, T{1}} + }}; return translate(m, negate(position)); } -template +template constexpr matrix mul(const matrix& a, const matrix& b) noexcept { matrix c = matrix::zero(); @@ -937,26 +935,26 @@ constexpr matrix mul(const matrix& a, T b) noexcept } /// @private -template +template inline constexpr typename matrix::column_vector_type mul(const matrix& a, const typename matrix::row_vector_type& b, std::index_sequence) noexcept { return ((a[I] * b[I]) + ...); } -template +template constexpr typename matrix::column_vector_type mul(const matrix& a, const typename matrix::row_vector_type& b) noexcept { return mul(a, b, std::make_index_sequence{}); } /// @private -template +template inline constexpr typename matrix::row_vector_type mul(const typename matrix::column_vector_type& a, const matrix& b, std::index_sequence) noexcept { return {dot(a, b[I]) ...}; } -template +template constexpr typename matrix::row_vector_type mul(const typename matrix::column_vector_type& a, const matrix& b) noexcept { return mul(a, b, std::make_index_sequence{}); @@ -967,8 +965,8 @@ matrix rotate(T angle, const vector& axis) { const T c = std::cos(angle); const T s = std::sin(angle); - const vector temp = mul(axis, T(1) - c); - + const vector temp = mul(axis, T{1} - c); + matrix rotation; rotation[0][0] = axis[0] * temp[0] + c; rotation[0][1] = axis[1] * temp[0] + axis[2] * s; @@ -979,7 +977,7 @@ matrix rotate(T angle, const vector& axis) rotation[2][0] = axis[0] * temp[2] + axis[1] * s; rotation[2][1] = axis[1] * temp[2] - axis[0] * s; rotation[2][2] = axis[2] * temp[2] + c; - + return rotation; } @@ -991,9 +989,9 @@ matrix3 rotate_x(T angle) return matrix3 { - T(1), T(0), T(0), - T(0), c, s, - T(0), -s, c + T{1}, T{0}, T{0}, + T{0}, c, s, + T{0}, -s, c }; } @@ -1005,9 +1003,9 @@ matrix3 rotate_y(T angle) return matrix3 { - c, T(0), -s, - T(0), T(1), T(0), - s, T(0), c + c, T{0}, -s, + T{0}, T{1}, T{0}, + s, T{0}, c }; } @@ -1019,22 +1017,26 @@ matrix3 rotate_z(T angle) return matrix3 { - c, s, T(0), - -s, c, T(0), - T(0), T(0), T(1) + c, s, T{0}, + -s, c, T{0}, + T{0}, T{0}, T{1} }; } template constexpr matrix scale(const matrix& m, const vector& v) { - return mul(m, matrix + return mul + ( + m, + matrix {{ - {v[0], T(0), T(0), T(0)}, - {T(0), v[1], T(0), T(0)}, - {T(0), T(0), v[2], T(0)}, - {T(0), T(0), T(0), T(1)} - }}); + {v[0], T{0}, T{0}, T{0}}, + {T{0}, v[1], T{0}, T{0}}, + {T{0}, T{0}, v[2], T{0}}, + {T{0}, T{0}, T{0}, T{1}} + }} + ); } /// @private @@ -1093,29 +1095,29 @@ template constexpr matrix translate(const matrix& m, const vector& v) { return mul(m, matrix - {{ - {T(1), T(0), T(0), T(0)}, - {T(0), T(1), T(0), T(0)}, - {T(0), T(0), T(1), T(0)}, - {v[0], v[1], v[2], T(1)} - }}); + {{ + {T{1}, T{0}, T{0}, T{0}}, + {T{0}, T{1}, T{0}, T{0}}, + {T{0}, T{0}, T{1}, T{0}}, + {v[0], v[1], v[2], T{1}} + }}); } /// @private -template +template inline constexpr typename matrix::column_vector_type transpose_column(const matrix& m, std::size_t i, std::index_sequence) noexcept { return {m[I][i] ...}; } /// @private -template +template inline constexpr matrix transpose(const matrix& m, std::index_sequence) noexcept { return {transpose_column(m, I, std::make_index_sequence{}) ...}; } -template +template constexpr matrix transpose(const matrix& m) noexcept { return transpose(m, std::make_index_sequence{}); @@ -1166,7 +1168,7 @@ inline constexpr matrix operator/(T a, const matrix& b) noexce } /// @copydoc mul(const matrix&, const matrix&) -template +template inline constexpr matrix operator*(const matrix& a, const matrix& b) noexcept { return mul(a, b); @@ -1187,14 +1189,14 @@ inline constexpr matrix operator*(T a, const matrix& b) noexce /// @} /// @copydoc mul(const matrix&, const typename matrix::row_vector_type&) -template +template inline constexpr typename matrix::column_vector_type operator*(const matrix& a, const typename matrix::row_vector_type& b) noexcept { return mul(a, b); } /// @copydoc mul(const typename matrix::column_vector_type&, const matrix&) -template +template inline constexpr typename matrix::row_vector_type operator*(const typename matrix::column_vector_type& a, const matrix& b) noexcept { return mul(a, b); @@ -1305,44 +1307,6 @@ inline constexpr matrix& operator/=(matrix& a, T b) noexcept } /// @} -/** - * Writes the elements of a matrix to an output stream, with each element delimeted by a space. - * - * @param os Output stream. - * @param m Matrix. - * - * @return Output stream. - */ -template -std::ostream& operator<<(std::ostream& os, const matrix& m) -{ - for (std::size_t i = 0; i < m.size(); ++i) - { - if (i) - os << ' '; - os << m.element(i); - } - - return os; -} - -/** - * Reads the elements of a matrix from an input stream, with each element delimeted by a space. - * - * @param is Input stream. - * @param m Matrix. - * - * @return Input stream. - */ -template -std::istream& operator>>(std::istream& is, matrix& m) -{ - for (std::size_t i = 0; i < m.size(); ++i) - is >> m.element(i); - - return is; -} - } // namespace operators } // namespace math diff --git a/src/engine/math/polynomial.hpp b/src/engine/math/polynomial.hpp index 564c0cc..2df2f52 100644 --- a/src/engine/math/polynomial.hpp +++ b/src/engine/math/polynomial.hpp @@ -42,7 +42,9 @@ template { T y = *first; for (++first; first != last; ++first) + { y = y * x + *first; + } return y; } @@ -66,8 +68,11 @@ namespace chebyshev { T y = *(first++); y += *(first++) * x; - T n2 = T(1), n1 = x, n0; - x *= T(2); + T n2 = T{1}; + T n1 = x; + T n0; + + x += x; for (; first != last; n2 = n1, n1 = n0) { @@ -77,7 +82,7 @@ namespace chebyshev { return y; } - + /** * Evaluates a Chebyshev polynomial. * @@ -90,7 +95,7 @@ namespace chebyshev { template [[nodiscard]] T evaluate(InputIt first, InputIt last, T min, T max, T x) { - return evaluate(first, last, math::map(x, min, max, T(-1), T(1))); + return evaluate(first, last, math::map(x, min, max, T{-1}, T{1})); } } // namespace chebyshev diff --git a/src/engine/math/quaternion.hpp b/src/engine/math/quaternion.hpp index 0478159..1a2a1a3 100644 --- a/src/engine/math/quaternion.hpp +++ b/src/engine/math/quaternion.hpp @@ -163,7 +163,7 @@ struct quaternion const T yw = y() * w(); const T zz = z() * z(); const T zw = z() * w(); - + return { T{1} - (yy + zz) * T{2}, (xy + zw) * T{2}, (xz - yw) * T{2}, @@ -185,7 +185,7 @@ struct quaternion */ [[nodiscard]] inline constexpr explicit operator vector() const noexcept { - return {r, i[0], i[1], i[2]}; + return {r, i.x(), i.y(), i.z()}; } /// Returns a zero quaternion, where every scalar is equal to zero. @@ -343,19 +343,32 @@ template [[nodiscard]] constexpr quaternion mul(const quaternion& a, T b) noexcept; /** - * Calculates the product of a quaternion and a vector. + * Rotates a vector by a unit quaternion. * - * @param a First value. - * @param b second value. + * @param q Unit quaternion. + * @param v Vector to rotate. * - * @return Product of the quaternion and vector. + * @return Rotated vector. + * + * @warning @p q must be a unit quaternion. + * + * @see https://fgiesen.wordpress.com/2019/02/09/rotating-a-single-vector-using-a-quaternion/ */ -/// @{ template -[[nodiscard]] constexpr vector mul(const quaternion& a, const vector& b) noexcept; +[[nodiscard]] constexpr vector mul(const quaternion& q, const vector& v) noexcept; + +/** + * Rotates a vector by the inverse of a unit quaternion. + * + * @param v Vector to rotate. + * @param q Unit quaternion. + * + * @return Rotated vector. + * + * @warning @p q must be a unit quaternion. + */ template -[[nodiscard]] constexpr vector mul(const vector& a, const quaternion& b) noexcept; -/// @} +[[nodiscard]] constexpr vector mul(const vector& v, const quaternion& q) noexcept; /** * Negates a quaternion. @@ -384,7 +397,7 @@ template * * @param q Quaternion to normalize. * - * @return Normalized quaternion. + * @return Unit quaternion. */ template [[nodiscard]] quaternion normalize(const quaternion& q); @@ -401,15 +414,18 @@ template [[nodiscard]] quaternion angle_axis(T angle, const vector& axis); /** - * Calculates the minimum rotation between two normalized direction vectors. + * Calculates a quaternion representing the minimum rotation from one direction to another directions. + * + * @param from Unit vector pointing in the source direction. + * @param to Unit vector pointing in the target direction. + * @param tolerance Floating-point tolerance. * - * @param source Normalized source direction. - * @param destination Normalized destination direction. + * @return Unit quaternion representing the minimum rotation from direction @p from to direction @p to. * - * @return Quaternion representing the minimum rotation between the source and destination vectors. + * @warning @p from and @p to must be unit vectors. */ template -[[nodiscard]] quaternion rotation(const vector& source, const vector& destination); +[[nodiscard]] quaternion rotation(const vector& from, const vector& to, T tolerance = T{1e-6}); /** * Performs spherical linear interpolation between two quaternions. @@ -417,11 +433,12 @@ template * @param a First quaternion. * @param b Second quaternion. * @param t Interpolation factor. + * @param tolerance Floating-point tolerance. * * @return Interpolated quaternion. */ template -[[nodiscard]] quaternion slerp(const quaternion& a, const quaternion& b, T t, T error = T{1e-6}); +[[nodiscard]] quaternion slerp(const quaternion& a, const quaternion& b, T t, T tolerance = T{1e-6}); /** * Calculates the square length of a quaternion. The square length can be calculated faster than the length because a call to `std::sqrt` is saved. @@ -462,16 +479,19 @@ template /** * Decomposes a quaternion into swing and twist rotation components. * - * @param[in] q Quaternion to decompose. - * @param[in] a Axis of twist rotation. + * @param[in] q Unit quaternion to decompose. + * @param[in] twist_axis Axis of twist rotation. * @param[out] swing Swing rotation component. * @param[out] twist Twist rotation component. - * @param[in] error Threshold at which a number is considered zero. + * @param[in] tolerance Floating-point tolerance. + * + * @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 vector& a, quaternion& qs, quaternion& qt, T error = T{1e-6}); +void swing_twist(const quaternion& q, const vector& twist_axis, quaternion& swing, quaternion& twist, T tolerance = T{1e-6}); /** * Converts a 3x3 rotation matrix to a quaternion. @@ -550,16 +570,16 @@ inline constexpr quaternion lerp(const quaternion& a, const quaternion& template quaternion look_rotation(const vector& forward, vector up) { - vector right = normalize(cross(forward, up)); + const vector right = normalize(cross(forward, up)); up = cross(right, forward); - - matrix m = + + const matrix m = { right, up, -forward }; - + // Convert to quaternion return normalize(quaternion_cast(m)); } @@ -568,12 +588,12 @@ template constexpr quaternion mul(const quaternion& a, const quaternion& b) noexcept { return - { - -a.x() * b.x() - a.y() * b.y() - a.z() * b.z() + a.w() * b.w(), - a.x() * b.w() + a.y() * b.z() - a.z() * b.y() + a.w() * b.x(), - -a.x() * b.z() + a.y() * b.w() + a.z() * b.x() + a.w() * b.y(), - a.x() * b.y() - a.y() * b.x() + a.z() * b.w() + a.w() * b.z() - }; + { + a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), + a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), + a.w() * b.y() - a.x() * b.z() + a.y() * b.w() + a.z() * b.x(), + a.w() * b.z() + a.x() * b.y() - a.y() * b.x() + a.z() * b.w() + }; } template @@ -583,15 +603,16 @@ inline constexpr quaternion mul(const quaternion& a, T b) noexcept } template -constexpr vector mul(const quaternion& a, const vector& b) noexcept +constexpr vector mul(const quaternion& q, const vector& v) noexcept { - return a.i * dot(a.i, b) * T{2} + b * (a.r * a.r - sqr_length(a.i)) + cross(a.i, b) * a.r * T{2}; + const auto t = cross(q.i, v) * T{2}; + return v + q.r * t + cross(q.i, t); } template -inline constexpr vector mul(const vector& a, const quaternion& b) noexcept +inline constexpr vector mul(const vector& v, const quaternion& q) noexcept { - return mul(conjugate(b), a); + return mul(conjugate(q), v); } template @@ -619,23 +640,42 @@ quaternion angle_axis(T angle, const vector& axis) } template -quaternion rotation(const vector& source, const vector& destination) +quaternion rotation(const vector& from, const vector& to, T tolerance) { - quaternion q = {dot(source, destination), cross(source, destination)}; - q.w() += length(q); - return normalize(q); + const auto cos_theta = dot(from, to); + + if (cos_theta <= T{-1} + tolerance) + { + // Direction vectors are opposing, return 180 degree rotation about arbitrary axis + return quaternion{T{0}, {T{1}, T{0}, T{0}}}; + } + else if (cos_theta >= T{1} - tolerance) + { + // Direction vectors are parallel, return identity quaternion + return quaternion::identity(); + } + else + { + const auto r = cos_theta + T{1}; + const auto i = cross(from, to); + const auto inv_length = T{1.0} / std::sqrt(r + r); + + return quaternion{r * inv_length, i * inv_length}; + } } template -quaternion slerp(const quaternion& a, const quaternion& b, T t, T error) +quaternion slerp(const quaternion& a, const quaternion& b, T t, T tolerance) { T cos_theta = dot(a, b); - if (cos_theta > T{1} - error) + if (cos_theta >= T{1} - tolerance) { + // Use linear interpolation if quaternions are nearly aligned return normalize(lerp(a, b, t)); } - - cos_theta = std::max(T{-1}, std::min(T{1}, cos_theta)); + + // Clamp to acos domain + cos_theta = std::min(std::max(cos_theta, T{-1}), T{1}); const T theta = std::acos(cos_theta) * t; @@ -669,28 +709,34 @@ inline constexpr quaternion sub(T a, const quaternion& b) noexcept } template -void swing_twist(const quaternion& q, const vector& a, quaternion& qs, quaternion& qt, T error) +void swing_twist(const quaternion& q, const vector& twist_axis, quaternion& swing, quaternion& twist, T tolerance) { - if (sqr_length(q.i) > error) - { - qt = normalize(quaternion{q.w(), a * dot(a, q.i)}); - qs = mul(q, conjugate(qt)); - } - else + if (sqr_length(q.i) <= tolerance) { - qt = angle_axis(pi, a); + // 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_sqr_length = sqr_length(swing_axis); - const vector qa = mul(q, a); - const vector sa = cross(a, qa); - if (sqr_length(sa) > error) + if (swing_axis_sqr_length <= tolerance) { - qs = angle_axis(std::acos(dot(a, qa)), sa); + // Swing axis and twist axis are parallel, no swing + swing = quaternion::identity(); } else { - qs = quaternion::identity(); + const auto cos_swing_angle = std::min(std::max(dot(twist_axis, rotated_twist_axis), T{-1}), T{1}); + swing = angle_axis(std::acos(cos_swing_angle), swing_axis * (T{1} / std::sqrt(swing_axis_sqr_length))); } } + else + { + twist = normalize(quaternion{q.r, twist_axis * dot(twist_axis, q.i)}); + swing = mul(q, conjugate(twist)); + } } template @@ -815,16 +861,16 @@ inline constexpr quaternion operator*(T a, const quaternion& b) noexcept /// @copydoc mul(const quaternion&, const vector&) template -inline constexpr vector operator*(const quaternion& a, const vector& b) noexcept +inline constexpr vector operator*(const quaternion& q, const vector& v) noexcept { - return mul(a, b); + return mul(q, v); } /// @copydoc mul(const vector&, const quaternion&) template -inline constexpr vector operator*(const vector& a, const quaternion& b) noexcept +inline constexpr vector operator*(const vector& v, const quaternion& q) noexcept { - return mul(a, b); + return mul(v, q); } /// @copydoc sub(const quaternion&, const quaternion&) diff --git a/src/engine/math/vector.hpp b/src/engine/math/vector.hpp index f1e8e73..233c432 100644 --- a/src/engine/math/vector.hpp +++ b/src/engine/math/vector.hpp @@ -24,9 +24,7 @@ #include #include #include -#include #include -#include #include #include @@ -38,11 +36,11 @@ namespace math { * @tparam T Element type. * @tparam N Number of elements. */ -template +template struct vector { /// Element type. - typedef T element_type; + using element_type = T; /// Number of elements. static constexpr std::size_t element_count = N; @@ -329,15 +327,15 @@ struct vector }; /// Vector with two elements. -template +template using vector2 = vector; /// Vector with three elements. -template +template using vector3 = vector; /// Vector with four elements. -template +template using vector4 = vector; /** @@ -692,7 +690,7 @@ template * * @param x Vector to normalize. * - * @return Normalized vector. + * @return Unit vector. */ template [[nodiscard]] vector normalize(const vector& x); @@ -1629,44 +1627,6 @@ inline constexpr vector& operator/=(vector& x, T y) noexcept } /// @} -/** - * Writes the elements of a vector to an output stream, with each element delimeted by a space. - * - * @param os Output stream. - * @param x Vector. - * - * @return Output stream. - */ -template -std::ostream& operator<<(std::ostream& os, const vector& x) -{ - for (std::size_t i = 0; i < N; ++i) - { - if (i) - os << ' '; - os << x[i]; - } - - return os; -} - -/** - * Reads the elements of a vector from an input stream, with each element delimeted by a space. - * - * @param is Input stream. - * @param x Vector. - * - * @return Input stream. - */ -template -std::istream& operator>>(std::istream& is, vector& x) -{ - for (std::size_t i = 0; i < N; ++i) - is >> x[i]; - - return is; -} - } // namespace operators } // namespace math diff --git a/src/engine/scene/directional-light.hpp b/src/engine/scene/directional-light.hpp index 57390bf..a0ed259 100644 --- a/src/engine/scene/directional-light.hpp +++ b/src/engine/scene/directional-light.hpp @@ -43,7 +43,7 @@ public: return light_type::directional; } - /// Returns the normalized direction vector of the light. + /// Returns a unit vector pointing in the light direction. [[nodiscard]] inline const math::vector& get_direction() const noexcept { return m_direction; diff --git a/src/game/components/ik-component.hpp b/src/game/components/ik-component.hpp new file mode 100644 index 0000000..f11a660 --- /dev/null +++ b/src/game/components/ik-component.hpp @@ -0,0 +1,31 @@ +/* + * 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_GAME_IK_COMPONENT_HPP +#define ANTKEEPER_GAME_IK_COMPONENT_HPP + +#include +#include + +struct ik_component +{ + std::shared_ptr rig; +}; + +#endif // ANTKEEPER_GAME_IK_COMPONENT_HPP diff --git a/src/game/game.cpp b/src/game/game.cpp index 7d7a08b..70d65b7 100644 --- a/src/game/game.cpp +++ b/src/game/game.cpp @@ -36,6 +36,7 @@ #include "game/systems/collision-system.hpp" #include "game/systems/constraint-system.hpp" #include "game/systems/locomotion-system.hpp" +#include "game/systems/ik-system.hpp" #include "game/systems/orbit-system.hpp" #include "game/systems/render-system.hpp" #include "game/systems/spatial-system.hpp" @@ -1054,6 +1055,9 @@ void game::setup_systems() // Setup locomotion system locomotion_system = std::make_unique<::locomotion_system>(*entity_registry); + // Setup IK system + ik_system = std::make_unique<::ik_system>(*entity_registry); + // Setup physics system physics_system = std::make_unique<::physics_system>(*entity_registry); physics_system->set_gravity({0.0f, -9.80665f * 100.0f, 0.0f}); @@ -1120,6 +1124,7 @@ void game::setup_controls() menu_action_map.set_event_dispatcher(input_event_dispatcher); movement_action_map.set_event_dispatcher(input_event_dispatcher); keeper_action_map.set_event_dispatcher(input_event_dispatcher); + ant_action_map.set_event_dispatcher(input_event_dispatcher); // Default control profile settings control_profile_filename = "controls.cfg"; @@ -1247,6 +1252,7 @@ void game::fixed_update(::frame_scheduler::duration_type fixed_update_time, ::fr behavior_system->update(t, dt); steering_system->update(t, dt); locomotion_system->update(t, dt); + ik_system->update(t, dt); physics_system->update(t, dt); camera_system->update(t, dt); orbit_system->update(t, dt); diff --git a/src/game/game.hpp b/src/game/game.hpp index 7a769ea..c4dcc9c 100644 --- a/src/game/game.hpp +++ b/src/game/game.hpp @@ -115,6 +115,7 @@ class steering_system; class physics_system; class subterrain_system; class terrain_system; +class ik_system; struct control_profile; @@ -202,6 +203,7 @@ public: input::action_map menu_action_map; input::action_map movement_action_map; input::action_map keeper_action_map; + input::action_map ant_action_map; input::mapper input_mapper; input::action fullscreen_action; @@ -360,6 +362,7 @@ public: std::unique_ptr<::constraint_system> constraint_system; std::unique_ptr<::steering_system> steering_system; std::unique_ptr<::locomotion_system> locomotion_system; + std::unique_ptr<::ik_system> ik_system; std::unique_ptr<::physics_system> physics_system; std::unique_ptr<::render_system> render_system; std::unique_ptr<::subterrain_system> subterrain_system; diff --git a/src/game/states/nest-selection-state.cpp b/src/game/states/nest-selection-state.cpp index a6640bd..2d35a29 100644 --- a/src/game/states/nest-selection-state.cpp +++ b/src/game/states/nest-selection-state.cpp @@ -32,6 +32,7 @@ #include "game/components/terrain-component.hpp" #include "game/components/legged-locomotion-component.hpp" #include "game/components/winged-locomotion-component.hpp" +#include "game/components/ik-component.hpp" #include "game/components/transform-component.hpp" #include "game/constraints/child-of-constraint.hpp" #include "game/constraints/copy-rotation-constraint.hpp" @@ -73,11 +74,13 @@ #include #include #include +#include +#include nest_selection_state::nest_selection_state(::game& ctx): game_state(ctx) { - debug::log::trace("Entering nest selection state..."); + debug::log::trace("Entering nest selection state..."); // Create world if not yet created if (ctx.entities.find("earth") == ctx.entities.end()) @@ -157,8 +160,42 @@ nest_selection_state::nest_selection_state(::game& ctx): larva_eid = ctx.entity_registry->create(); - auto larva_skeletal_mesh = std::make_unique(worker_phenome.larva->model); + auto larva_skeletal_mesh = std::make_shared(worker_phenome.larva->model); + //auto larva_skeletal_mesh = std::make_shared(ctx.resource_manager->load("snake.mdl")); + const auto& larva_skeleton = larva_skeletal_mesh->get_model()->get_skeleton(); + + auto larva_ik_rig = std::make_shared(*larva_skeletal_mesh); + + auto no_twist_constraint = std::make_shared(); + no_twist_constraint->set_twist_limit(0.0f, 0.0f); + + //auto euler_constraint = std::make_shared(); + //euler_constraint->set_limits({0.0f, -math::radians(90.0f), 0.0f}, {math::radians(90.0f), math::radians(90.0f), 0.0f}); + + for (std::size_t i = 0; i < larva_skeleton.get_bone_count(); ++i) + { + larva_ik_rig->set_constraint(static_cast(i), no_twist_constraint); + } + + const auto larva_ik_root_bone_index = *larva_skeleton.get_bone_index("abdomen3"); + const auto larva_ik_effector_bone_index = *larva_skeleton.get_bone_index("head"); + + const auto& larva_head_absolute_transform = larva_skeletal_mesh->get_pose().get_absolute_transform(larva_ik_effector_bone_index); + const auto& larva_head_relative_transform = larva_skeletal_mesh->get_pose().get_relative_transform(larva_ik_effector_bone_index); + + 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 * float3{0.0f, 0.0f, -0.0f}); + larva_ik_solver->set_goal_position(larva_head_absolute_transform.translation + float3{0.2f, 0.2f, 0.5f}); + + + + larva_ik_rig->add_solver(larva_ik_solver); + + //larva_skeletal_mesh->get_pose().set_relative_rotation(larva_ik_root_bone_index, math::angle_axis(math::radians(45.0f), float3{1.0f, 0.0f, 0.0f})); + //larva_skeletal_mesh->get_pose().update(); + ctx.entity_registry->emplace(larva_eid, std::move(larva_skeletal_mesh), std::uint8_t{1}); + ctx.entity_registry->emplace(larva_eid, std::move(larva_ik_rig)); // Disable UI color clear @@ -643,7 +680,7 @@ void nest_selection_state::setup_controls() // Move up action_subscriptions.emplace_back ( - ctx.move_up_action.get_active_channel().subscribe + ctx.move_up_action.get_activated_channel().subscribe ( [&](const auto& event) { @@ -662,7 +699,7 @@ void nest_selection_state::setup_controls() // Move down action_subscriptions.emplace_back ( - ctx.move_down_action.get_active_channel().subscribe + ctx.move_down_action.get_activated_channel().subscribe ( [&](const auto& event) { diff --git a/src/game/states/nest-selection-state.hpp b/src/game/states/nest-selection-state.hpp index b363c62..3e4cd92 100644 --- a/src/game/states/nest-selection-state.hpp +++ b/src/game/states/nest-selection-state.hpp @@ -24,7 +24,7 @@ #include #include #include - +#include class nest_selection_state: public game_state { @@ -72,6 +72,8 @@ private: double first_person_camera_yaw{0.0}; double first_person_camera_pitch{0.0}; + + std::shared_ptr larva_ik_solver; }; #endif // ANTKEEPER_NEST_SELECTION_STATE_HPP diff --git a/src/game/systems/ik-system.cpp b/src/game/systems/ik-system.cpp new file mode 100644 index 0000000..21078f3 --- /dev/null +++ b/src/game/systems/ik-system.cpp @@ -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 . + */ + +#include "game/systems/ik-system.hpp" +#include "game/components/ik-component.hpp" +#include +#include +#include + +ik_system::ik_system(entity::registry& registry): + updatable_system(registry) +{} + +void ik_system::update(float t, float dt) +{ + auto view = registry.view(); + std::for_each + ( + std::execution::par_unseq, + view.begin(), + view.end(), + [&](auto entity_id) + { + const auto& component = view.get(entity_id); + + component.rig->solve(); + } + ); +} diff --git a/src/game/systems/ik-system.hpp b/src/game/systems/ik-system.hpp new file mode 100644 index 0000000..f0572f1 --- /dev/null +++ b/src/game/systems/ik-system.hpp @@ -0,0 +1,36 @@ +/* + * 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_GAME_IK_SYSTEM_HPP +#define ANTKEEPER_GAME_IK_SYSTEM_HPP + +#include "game/systems/updatable-system.hpp" + +/** + * + */ +class ik_system: + public updatable_system +{ +public: + explicit ik_system(entity::registry& registry); + void update(float t, float dt) override; +}; + +#endif // ANTKEEPER_GAME_IK_SYSTEM_HPP