💿🐜 Antkeeper source code
https://antkeeper.com
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
109 lines
3.9 KiB
109 lines
3.9 KiB
/*
|
|
* 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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <engine/animation/ik/solvers/ccd-ik-solver.hpp>
|
|
#include <engine/animation/ik/ik-rig.hpp>
|
|
#include <engine/debug/log.hpp>
|
|
#include <stdexcept>
|
|
|
|
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_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
|
|
const auto ps_effector_position = ps_effector_bone_transform * m_effector_position;
|
|
|
|
// 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
|
|
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 effector
|
|
const auto ps_effector_direction = math::normalize(ps_effector_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);
|
|
|
|
// 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
|
|
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);
|
|
|
|
// Update pose
|
|
//pose.update(bone_index, j + 1);
|
|
pose.update();
|
|
}
|
|
}
|
|
}
|