73 float threshold = 0.001f,
74 int maxIterations = 10,
75 float damping_error_threshold = 0.5f,
76 float weight_exponent = 1.0f) ->
bool
79 const size_t chain_size = chain.size();
84 float max_reach = 0.f;
85 for(
size_t i = 0; i < chain.size() - 1; ++i)
87 math::vec3 diff = chain[i + 1]->get_position_global() - chain[i]->get_position_global();
88 max_reach += math::length(diff);
92 math::vec3 target_dir = target - base_position;
93 float target_dist = math::length(target_dir);
94 if(target_dist > max_reach)
96 target_dir = math::normalize(target_dir);
97 target = base_position + target_dir * (max_reach - 0.001f);
102 for(
int iter = 0; iter < maxIterations; ++iter)
105 for(
int i =
static_cast<int>(chain_size) - 2; i >= 0; --i)
112 math::vec3 to_end = current_end_pos - bone_pos;
113 math::vec3 to_target = target - bone_pos;
115 float len_to_end = math::length(to_end);
116 float len_to_target = math::length(to_target);
119 if(len_to_end < math::epsilon<float>() || len_to_target < math::epsilon<float>())
124 to_end = math::normalize(to_end);
125 to_target = math::normalize(to_target);
128 float cos_angle = math::clamp(math::dot(to_end, to_target), -1.0f, 1.0f);
129 float angle = math::acos(cos_angle);
132 if(std::fabs(angle) < 1e-3f)
138 math::vec3 rotation_axis = math::cross(to_end, to_target);
139 if(math::length(rotation_axis) < 1e-4f)
143 rotation_axis = math::normalize(rotation_axis);
147 float global_error = math::length(target - current_end_pos);
148 float damping_factor = math::clamp(global_error / damping_error_threshold, 0.0f, 1.0f);
149 float damped_angle = angle * damping_factor;
153 math::quat rotation_delta = math::angleAxis(damped_angle, rotation_axis);
158 math::quat parent_global_rot =
160 math::quat local_rotation_delta = glm::inverse(parent_global_rot) * rotation_delta * parent_global_rot;
165 float t = float(i + 1) / float(chain_size);
166 float weight = std::pow(t, weight_exponent);
167 math::quat weighted_local_rotation_delta =
168 glm::slerp(math::identity<math::quat>(), local_rotation_delta, weight);
178 float current_error = math::length(target - current_end_pos);
179 if(current_error < threshold)
200 const math::vec3& target,
201 float threshold = 0.001f,
202 int max_iterations = 10) ->
bool
204 const size_t n = chain.size();
210 for(
size_t i = 0; i < n; ++i)
212 orig_positions[i] = chain[i]->get_position_global();
220 float total_length = 0.f;
221 for(
size_t i = 0; i < n - 1; ++i)
223 bone_lengths[i] = math::length(orig_positions[i + 1] - orig_positions[i]);
224 total_length += bone_lengths[i];
228 const math::vec3 root_pos = positions[0];
231 if(math::length(target - root_pos) > total_length)
234 math::vec3 dir = math::normalize(target - root_pos);
235 for(
size_t i = 0; i < n - 1; ++i)
237 positions[i + 1] = positions[i] + dir * bone_lengths[i];
243 for(
int iter = 0; iter < max_iterations; ++iter)
246 positions[n - 1] = target;
247 for(
int i =
static_cast<int>(n) - 2; i >= 0; --i)
249 float r = math::length(positions[i + 1] - positions[i]);
250 float lambda = bone_lengths[i] / r;
251 positions[i] = (1 - lambda) * positions[i + 1] + lambda * positions[i];
255 positions[0] = root_pos;
256 for(
size_t i = 0; i < n - 1; ++i)
258 float r = math::length(positions[i + 1] - positions[i]);
259 float lambda = bone_lengths[i] / r;
260 positions[i + 1] = (1 - lambda) * positions[i] + lambda * positions[i + 1];
264 if(math::length(positions[n - 1] - target) < threshold)
272 for(
size_t i = 0; i < n - 1; ++i)
280 math::vec3 current_dir = math::normalize(child_pos - current_pos);
283 math::vec3 desired_dir = math::normalize(positions[i + 1] - positions[i]);
286 if(math::length(current_dir) < 1e-5f || math::length(desired_dir) < 1e-5f)
291 float dot = math::clamp(math::dot(current_dir, desired_dir), -1.f, 1.f);
297 math::vec3 axis = math::cross(current_dir, desired_dir);
298 if(math::length(axis) < 1e-5f)
303 axis = math::normalize(axis);
304 float angle = math::acos(dot);
305 math::quat rotation_delta = math::angleAxis(angle, axis);
310 math::quat parent_global_rot =
313 math::quat local_rotation_delta = glm::inverse(parent_global_rot) * rotation_delta * parent_global_rot;
316 math::quat new_local = math::normalize(local_rotation_delta * bone->
get_rotation_local());
350 const glm::vec3& target,
351 const glm::vec3& mid_axis,
352 const glm::vec3& pole_vector,
367 glm::mat4 inv_start = glm::inverse(start_transform);
368 glm::mat4 inv_mid = glm::inverse(mid_transform);
376 glm::vec3 start_mid_ms = -start_ms;
377 glm::vec3 mid_end_ms = end_ms;
382 glm::vec3 start_mid_ss = mid_ss;
383 glm::vec3 mid_end_ss = end_ss - mid_ss;
384 glm::vec3 start_end_ss = end_ss;
386 float start_mid_ss_len2 = glm::dot(start_mid_ss, start_mid_ss);
387 float mid_end_ss_len2 = glm::dot(mid_end_ss, mid_end_ss);
388 float start_end_ss_len2 = glm::dot(start_end_ss, start_end_ss);
393 float start_target_ss_orig_len = glm::length(start_target_ss_orig);
394 float start_target_ss_orig_len2 = start_target_ss_orig_len * start_target_ss_orig_len;
397 float l0 = std::sqrt(start_mid_ss_len2);
398 float l1 = std::sqrt(mid_end_ss_len2);
399 float chain_length = l0 + l1;
400 float bone_diff = fabs(l0 - l1);
403 float da = chain_length * glm::clamp(soften, 0.0f, 1.0f);
404 float ds = chain_length - da;
406 glm::vec3 start_target_ss;
407 float start_target_ss_len2;
408 bool target_softened =
false;
409 if(start_target_ss_orig_len > da && start_target_ss_orig_len > bone_diff && ds > 1e-4f)
411 float alpha = (start_target_ss_orig_len - da) / ds;
413 float ratio = 1.0f - (std::pow(3.0f, 4.0f) / std::pow(alpha + 3.0f, 4.0f));
414 float new_target_len = da + ds - ds * ratio;
415 start_target_ss = glm::normalize(start_target_ss_orig) * new_target_len;
416 start_target_ss_len2 = new_target_len * new_target_len;
417 target_softened =
true;
421 start_target_ss = start_target_ss_orig;
422 start_target_ss_len2 = start_target_ss_orig_len2;
427 float cos_corrected = (start_mid_ss_len2 + mid_end_ss_len2 - start_target_ss_len2) / (2.0f * l0 * l1);
428 cos_corrected = glm::clamp(cos_corrected, -1.0f, 1.0f);
429 float corrected_angle = std::acos(cos_corrected);
432 float cos_initial = (start_mid_ss_len2 + mid_end_ss_len2 - start_end_ss_len2) / (2.0f * l0 * l1);
433 cos_initial = glm::clamp(cos_initial, -1.0f, 1.0f);
434 float initial_angle = std::acos(cos_initial);
437 glm::vec3 mid_axis_ms = glm::normalize(glm::mat3(inv_mid) * mid_axis);
438 glm::vec3 bent_side_ref = glm::cross(start_mid_ms, mid_axis_ms);
439 if(glm::dot(bent_side_ref, mid_end_ms) < 0.0f)
441 initial_angle = -initial_angle;
444 float angle_delta = corrected_angle - initial_angle;
446 glm::quat mid_rot = glm::angleAxis(angle_delta, glm::normalize(mid_axis));
450 glm::vec3 rotated_mid_end_ms = glm::rotate(mid_rot, mid_end_ms);
451 glm::vec3 rotated_mid_end_global = glm::mat3(mid_transform) * rotated_mid_end_ms;
452 glm::vec3 mid_end_ss_final = glm::mat3(inv_start) * rotated_mid_end_global;
453 glm::vec3 start_end_ss_final = start_mid_ss + mid_end_ss_final;
456 glm::quat end_to_target_rot_ss = glm::rotation(start_end_ss_final, start_target_ss);
457 glm::quat start_rot_ss = end_to_target_rot_ss;
460 if(glm::length(start_target_ss) > 1e-4f)
463 glm::vec3 pole_ss = glm::normalize(glm::mat3(inv_start) * pole_vector);
465 glm::vec3 ref_plane_normal_ss = glm::normalize(glm::cross(start_target_ss, pole_ss));
467 glm::vec3 mid_axis_ss = glm::normalize(glm::mat3(inv_start) * (glm::mat3(mid_transform) * mid_axis));
469 glm::vec3 joint_plane_normal_ss = glm::rotate(end_to_target_rot_ss, mid_axis_ss);
471 float rotate_plane_cos_angle =
472 glm::dot(glm::normalize(ref_plane_normal_ss), glm::normalize(joint_plane_normal_ss));
473 rotate_plane_cos_angle = glm::clamp(rotate_plane_cos_angle, -1.0f, 1.0f);
476 glm::vec3 rotate_plane_axis_ss = glm::normalize(start_target_ss);
477 if(glm::dot(joint_plane_normal_ss, pole_ss) < 0.0f)
479 rotate_plane_axis_ss = -rotate_plane_axis_ss;
481 glm::quat rotate_plane_ss = glm::angleAxis(std::acos(rotate_plane_cos_angle), rotate_plane_axis_ss);
484 if(fabs(twist_angle) > 1e-5f)
486 glm::quat twist_ss = glm::angleAxis(twist_angle, glm::normalize(start_target_ss));
487 start_rot_ss = twist_ss * rotate_plane_ss * end_to_target_rot_ss;
491 start_rot_ss = rotate_plane_ss * end_to_target_rot_ss;
497 if(start_rot_ss.w < 0.0f)
498 start_rot_ss = -start_rot_ss;
505 start_rot_ss = glm::slerp(glm::identity<glm::quat>(), start_rot_ss, weight);
506 mid_rot = glm::slerp(glm::identity<glm::quat>(), mid_rot, weight);
510 bool reached = target_softened && (weight >= 1.0f);