ccd-ik-solver.cpp

Go to the documentation of this file.

/*
 * 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();
        }
    }
}