Inverse Kinematics is the mathematical process of calculating the variable joint parameters needed to place the end of a kinematic shain, in this case, a chain composed by bones.
Inside this code you can find three different approaches: Two Bone IK, Forwad and Backward Reaching Inverse Kinematics or FABRIK and Cyclic Coordinate Descent or CCD.
/*!
* \brief Constructor
*/
ik::ik(unsigned num_bones, ik_method method, glm::vec3 start, glm::vec3 end) {
//allocate memory for all the bones
m_bones.resize(num_bones);
//set up the method in order to update
m_method = method;
//set the goal as the end
m_goal = end;
//set the start or origin position of the first bone
m_start = start;
}
/*!
* \brief update to Inverse Kinematics, result defines outcome of update
* \return ik_failure, goal position can't be reached.
* ik_processing if current iterations have reached the limit and has stopped
* updating this frame.
* ik_success goal position was reached and ik was updated accordingy.
*/
ik::ik_result ik::update(glm::vec3 goal) {
//set up the new goal (end) position
m_goal = goal;
//check if the goal is reachable, if not leave the ik as it was last frame
if (!to_reach(goal))
return ik::ik_result::ik_failure;
//call pertinent update depending on method.
switch (m_method) {
case ik_method::ik_fabrik:
return update_fabrik(goal);
case ik_method::ik_ccd:
return update_ccd(goal);
case ik_method::ik_2_bone:
return update_2_bone(goal);
}
}
/*!
* \brief checks if the goal is reachable or outside the reach of the bones.
*/
bool ik::to_reach(glm::vec3 goal) {
float total_length{ 0.0f };
//get the total length of all bones combineed
for (ik_bone& b : m_bones)
total_length += b.m_length;
//if the distance to the goal is larger than the actual length of the object
if (glm::distance2(m_start, goal) > (total_length * total_length)) {
//get the vector direction unit to the goal from start and add the total length and place each bone there
glm::vec3 vec_goal = goal - m_start;
vec_goal = glm::normalize(vec_goal);
glm::vec3 start = m_start;
//loop through all the bones
for (ik_bone& b : m_bones) {
//set up the position (beign and end) position of the current bone
b.m_start = start;
start += vec_goal * b.m_length; //add the length of the bone
b.m_end = start;
}
//false since it is not in reach
return false;
}
//true since it can be reached
return true;
}
/*!
* \brief update with fabrik approach, result defines outcome of update
*/
ik::ik_result ik::update_fabrik(glm::vec3 goal) {
//loop through the max iterations unless we have found an ideal result within
//error margin
for (unsigned i = 0; i < m_max_iterations; i++) {
//do backward step from fabrick method and pass as parameter the final position
fabrik_backward(goal);
//do forward step from fabrik
fabrik_forward();
//check if the distance from the last bone to the goal is within the margin error
float distance_to_goal_sq = glm::distance2(m_bones.back().m_end, goal);
if (distance_to_goal_sq < (m_error * m_error))
return ik::ik_result::ik_success;
}
// if we have done all iterations then we return for this frame and we will continue next one
return ik::ik_result::ik_processing;
}
/*!
* \brief update with fabrik-backward step.
*/
void ik::fabrik_backward(const glm::vec3& target) {
// take goal
glm::vec3 current_node_target = target;
//loop through the bones from last to first
for (int i = m_bones.size() - 1; i >= 0; i--) {
//vector from target to bone start
glm::vec3 vec_target_bone = m_bones[i].m_start - current_node_target;
//making sure the length of the vector is the length of current bone
vec_target_bone = glm::normalize(vec_target_bone) * m_bones[i].m_length;
//position the bone start at new position
glm::vec3 new_bone_start = current_node_target + vec_target_bone;
//set up end and start values
m_bones[i].m_end = current_node_target;
m_bones[i].m_start = new_bone_start;
//update the current node target to the previous bone start
current_node_target = new_bone_start;
}
}
/*!
* \brief update with fabrik-forward step.
*/
void ik::fabrik_forward() {
//set the start as current start
glm::vec3 current_node_start = m_start;
//loop forward from first to last bone
for (int i = 0; i < m_bones.size(); i++) {
//get vector from start to end of current bone
glm::vec3 vec_start_bone = m_bones[i].m_end - current_node_start;
//making sure the lenth of the vector is the length of the current bone
vec_start_bone = glm::normalize(vec_start_bone) * m_bones[i].m_length;
//calculate the new end position of current bone
glm::vec3 new_end = current_node_start + vec_start_bone;
//set up the new information of the bone
m_bones[i].m_start = current_node_start;
m_bones[i].m_end = new_end;
//update de current bone start as the previous bone end
current_node_start = new_end;
}
}
/*!
* \brief update with 2-bone approach, result defines outcome of update
*/
ik::ik_result ik::update_2_bone(glm::vec3 goal) {
//get root bone
ik_bone root = m_bones.front();
//get from root
float length_root_goal = glm::length2(goal - m_start);
//get the angle between both bones
float two_bone_length_sq = root.m_length * root.m_length + m_bones[1].m_length * m_bones[1].m_length;
float angle_bones = (length_root_goal- two_bone_length_sq) / (2.0f * root.m_length * m_bones[1].m_length);
angle_bones = glm::clamp(angle_bones, -1.0f, 1.0f);
// angle difference from the initial position of the root bone
float inv_cos_angle_bones = glm::acos(angle_bones);
float cos_root = glm::cos(inv_cos_angle_bones);
float sin_root = glm::sin(inv_cos_angle_bones);
//get the direction from the origin to the
float direction_to_goal_angle = root.m_length + m_bones[1].m_length * cos_root;
float y_difference = goal.y * direction_to_goal_angle - goal.x * (m_bones[1].m_length * sin_root);
float x_difference = goal.x * direction_to_goal_angle + goal.y * (m_bones[1].m_length * sin_root);
//compute the new angle
float angle_root = std::atan2(y_difference, x_difference);
float real_theta_2 = angle_root + inv_cos_angle_bones;
//set up the initial end position of bone 0 and 1 and end position of last bone.
m_bones[0].m_end = root.m_length * glm::vec3{ glm::cos(angle_t_1), glm::sin(angle_t_1), 0.0f };
m_bones[1].m_end = m_bones[1].m_start + m_bones[1].m_length * glm::vec3{ glm::cos(real_theta_2), glm::sin(real_theta_2), 0.0f };
m_bones[1].m_start = m_bones[0].m_end;
//since we have found a solution, return success
return ik::ik_result::ik_success;
}
/*!
* \brief update with CCD approach, result defines outcome of update
*/
ik::ik_result ik::update_ccd(glm::vec3 goal) {
//loop though all the iterations
for (int iter = 0; iter < m_max_iterations; iter++) {
//loop though all the bones from the last to the first one
for (int i = m_bones.size() - 1; i >= 0; i--) {
//get a reference to the current bone
ik_bone& current_bone = m_bones[i];
//get the normalized bector from the current bone to the current end position of last bone
glm::vec3 vec_current_bone_end = glm::normalize(m_bones.back().m_end - current_bone.m_start);
//the actual normalized vector from the start of current bone to the goal
glm::vec3 vec_current_bone_goal = glm::normalize(goal - current_bone.m_start);
//clamp the dot product of previous vectors between -1 and 1
float dot = glm::clamp(glm::dot(vec_current_bone_end, vec_current_bone_goal), -1.0f, 1.0f);
//get the axis of rotation
glm::vec3 axis_rot = glm::normalize(glm::cross(vec_current_bone_end, vec_current_bone_goal));
//get the angle of rotation
float angle = glm::acos(dot);
//get the normalized vector director of the bone
glm::vec3 vec_dir = glm::normalize(current_bone.m_end - current_bone.m_start);
//calculate new direction of the bone
glm::vec3 new_dir = glm::normalize(glm::rotate(vec_dir, angle, axis_rot));
glm::angleAxis(angle, axis_rot);
//update the rotation and position of the current bone
current_bone.m_local.m_quaternion = glm::normalize(current_bone.m_local.m_quaternion * glm::rotation(vec_current_bone_end, vec_current_bone_goal));
current_bone.m_end = current_bone.m_start + new_dir * current_bone.m_length;
}
}
return ik_result::ik_success;
}