parent
f49865666b
commit
679f5935ba
3 changed files with 675 additions and 27 deletions
@ -0,0 +1,572 @@ |
||||
class_name copyMediaPipe_IKChain |
||||
extends RefCounted |
||||
|
||||
# FIXME: We pass this around internally a bunch and don't need to. |
||||
var skeleton : Skeleton3D = null |
||||
var base_bone : String |
||||
var tip_bone : String |
||||
|
||||
var tracker_object : Node = null |
||||
|
||||
var rotation_low : float = 0.1 |
||||
var rotation_high : float = 2.0 * PI - 0.1 |
||||
|
||||
var do_yaw : bool = true |
||||
var do_bone_roll : bool = false |
||||
|
||||
var main_axis_of_rotation : Vector3 = Vector3(1.0, 0.0, 0.0) |
||||
|
||||
# If do_yaw is true, then this is the "yaw" rotation axis. |
||||
var secondary_axis_of_rotation : Vector3 = Vector3(0.0, 1.0, 0.0) |
||||
|
||||
# Set to 0,0,0 if no pole target. |
||||
var pole_direction_target : Vector3 = Vector3(0.0, 0.0, 0.0) |
||||
var pole_direction_rotation_object : Node3D = null |
||||
|
||||
var _calculated_distance_to_angle_mappings = null |
||||
var _calculated_max_extension_angle = 0.0 |
||||
var _calculated_min_extension_angle = 2.0 * PI |
||||
var _calculated_max_extension_distance = 1.0 |
||||
var _calculated_min_extension_distance = 0.0 |
||||
|
||||
# FIXME: Get rid of these or make them actual config options once everything is |
||||
# working. |
||||
var do_ik_curve = true |
||||
var do_yaw_global = true |
||||
var do_point_tracker = true |
||||
var do_pole_targets = true |
||||
var do_rotate_to_match_tracker = true |
||||
|
||||
var yaw_scale : float = 0.25 |
||||
var reset_first : bool = true |
||||
|
||||
var symmetric : bool = false |
||||
# Local space to the rest position of whatever bone we're working on. |
||||
var symmetric_axis : Vector3 = Vector3(1.0, 0.0, 0.0) |
||||
var symmetric_influence_scale : float = 2.0 |
||||
var symmetric_influence_start_offset : float = 1.0 |
||||
|
||||
func _debug_print_chain_rotation_mapping( |
||||
skel : Skeleton3D, |
||||
base_bone_index : int, |
||||
tip_bone_index : int): |
||||
|
||||
# Print a graph of the head-to-hips distances based on spine bone |
||||
# pitches. Find the max extension angle. |
||||
var n = 0.0 |
||||
var found_max_extension_angle = 0.0 |
||||
var found_max_extension_angle_distance = 0.0 |
||||
while n <= PI: |
||||
var dist = attempt_spine_rotation(skel, n, base_bone_index, tip_bone_index) |
||||
var barstr = "" |
||||
var barcounter = 0 |
||||
while barcounter < dist: |
||||
barstr += "#" |
||||
barcounter += 0.01 |
||||
print("%2f = %f %s" % [n, dist, barstr]) |
||||
n += 0.02 |
||||
|
||||
if dist > found_max_extension_angle_distance: |
||||
found_max_extension_angle_distance = dist |
||||
found_max_extension_angle = n |
||||
|
||||
print("MAX EXTENSION ANGLE: ", found_max_extension_angle) |
||||
|
||||
func rotate_chain_so_tip_points_in_direction( |
||||
skel : Skeleton3D, |
||||
base_bone_index : int, |
||||
tip_bone_index : int, |
||||
target_position_worldspace : Vector3): |
||||
|
||||
# FIXME: Fix this comment. We're doing more than just hips/head with this function now. |
||||
# |
||||
# We can use "Hips" here, and have it affect the legs as well (pivoting the other way), or |
||||
# we can target the bone right above the hips, along the spine, and only move the upper |
||||
# torso. |
||||
# |
||||
# My model relies on hips rotation for tail flapping in the background, so I'm gonna leave |
||||
# this as "Hips" right now. |
||||
var bone_after_hips = base_bone_index |
||||
|
||||
# Get *current* hips and head positions. |
||||
var head_world_space = skel.get_global_transform() * skel.get_bone_global_pose(tip_bone_index) |
||||
var hips_world_space = skel.get_global_transform() * skel.get_bone_global_pose(bone_after_hips) |
||||
|
||||
# Figure out the rotation from the direction the spine is pointing, to the direction to the |
||||
# head tracker. |
||||
var delta_to_tracker = (target_position_worldspace - hips_world_space.origin).normalized() |
||||
var delta_to_head = (head_world_space.origin - hips_world_space.origin).normalized() |
||||
|
||||
var rotation_axis = -delta_to_tracker.cross(delta_to_head).normalized() |
||||
var rotation_angle = acos(delta_to_tracker.dot(delta_to_head)) |
||||
var hips_index = bone_after_hips |
||||
var root_index = skel.get_bone_parent(hips_index) |
||||
var hips_transform_worldspace = skel.get_global_transform() * skel.get_bone_global_pose(hips_index) |
||||
hips_transform_worldspace = hips_transform_worldspace.rotated(rotation_axis, rotation_angle) |
||||
var root_transform_worldspace = skel.get_global_transform() * skel.get_bone_global_pose(root_index) |
||||
var new_hips_transform = root_transform_worldspace.inverse() * hips_transform_worldspace |
||||
skel.set_bone_pose_rotation(hips_index, new_hips_transform.basis.get_rotation_quaternion()) |
||||
|
||||
func rotate_chain_to_pole_target( |
||||
skel : Skeleton3D, |
||||
base_bone_index : int, |
||||
tip_bone_index : int, |
||||
pole_direction_target_skeleton_space : Vector3): |
||||
|
||||
# If we have a reference rotation object, use that now. |
||||
if pole_direction_rotation_object: |
||||
var pole_direction_rotation_skeleton_space : Transform3D = \ |
||||
skel.global_transform.inverse() * pole_direction_rotation_object.global_transform |
||||
pole_direction_target_skeleton_space = \ |
||||
pole_direction_rotation_skeleton_space * pole_direction_target_skeleton_space |
||||
|
||||
# Get average bone direction. |
||||
var current_bone_index = skel.get_bone_parent(tip_bone_index) |
||||
var total_bone_offset = Vector3(0.0, 0.0, 0.0) |
||||
var base_bone_origin = skel.get_bone_global_pose(base_bone_index).origin |
||||
while current_bone_index != base_bone_index: |
||||
total_bone_offset += skel.get_bone_global_pose(current_bone_index).origin - base_bone_origin |
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
|
||||
# Get pole direction. |
||||
var pole_direction = \ |
||||
skel.get_bone_global_pose(tip_bone_index).origin - \ |
||||
skel.get_bone_global_pose(base_bone_index).origin |
||||
|
||||
#if (pole_direction.length() - head_dist_target) > - 0.01: |
||||
if true: |
||||
|
||||
#print(skel.get_bone_name(base_bone_index), " ", pole_direction.length() - head_dist_target) |
||||
|
||||
pole_direction = pole_direction.normalized() |
||||
|
||||
# Project average bone displacement onto rotation plane. |
||||
var s = total_bone_offset.normalized().dot(pole_direction) |
||||
var direction_offset = total_bone_offset.normalized() - pole_direction * s |
||||
direction_offset = direction_offset.normalized() |
||||
assert(abs(direction_offset.dot(pole_direction)) < 0.001) |
||||
|
||||
# Project pole target direction onto rotation plane. |
||||
var pole_direction_target_offset = pole_direction_target_skeleton_space - \ |
||||
(skel.get_bone_global_pose(base_bone_index).origin + |
||||
skel.get_bone_global_pose(tip_bone_index).origin) / 2.0 |
||||
|
||||
s = pole_direction_target_offset.dot(pole_direction) |
||||
var target_direction_offset = pole_direction_target_offset - pole_direction * s |
||||
target_direction_offset = target_direction_offset.normalized() |
||||
assert(abs(target_direction_offset.dot(pole_direction)) < 0.001) |
||||
|
||||
# print(skel.get_bone_name(tip_bone_index)) |
||||
# print(direction_offset) |
||||
# print(target_direction_offset) |
||||
|
||||
var rotation_direction = sign(direction_offset.cross(target_direction_offset).dot(pole_direction)) |
||||
var rotation_amount = acos(direction_offset.dot(target_direction_offset)) |
||||
|
||||
var parent_space = skel.get_bone_global_pose(skel.get_bone_parent(base_bone_index)).basis |
||||
|
||||
# Find second-to-last bone. |
||||
# current_bone_index = tip_bone_index |
||||
# while skel.get_bone_parent(current_bone_index) != base_bone_index: |
||||
# current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
current_bone_index = base_bone_index |
||||
|
||||
var new_global_pose = \ |
||||
skel.get_bone_global_pose(current_bone_index).basis.rotated(pole_direction, rotation_amount * rotation_direction) |
||||
skel.set_bone_pose_rotation(current_bone_index, (parent_space.inverse() * new_global_pose)) |
||||
|
||||
|
||||
|
||||
func chain_distribute_bone_roll( |
||||
base_bone_index : int, |
||||
tip_bone_index : int): |
||||
|
||||
# Count up bones. |
||||
var bone_count = 1 |
||||
var current_bone = skeleton.get_bone_parent(tip_bone_index) |
||||
while current_bone != -1 and current_bone != base_bone_index: |
||||
bone_count += 1 |
||||
current_bone = skeleton.get_bone_parent(current_bone) |
||||
|
||||
# Determine the bone roll axis for tip bone by averaging out all the child |
||||
# positions and using that as a bone direction. |
||||
var tip_bone_roll_axis = Vector3(0.0, 0.0, 0.0) |
||||
var tip_bone_children = skeleton.get_bone_children(tip_bone_index) |
||||
for tip_bone_child in tip_bone_children: |
||||
tip_bone_roll_axis += skeleton.get_bone_rest(tip_bone_child).origin |
||||
tip_bone_roll_axis = tip_bone_roll_axis.normalized() |
||||
|
||||
var current_tip_rotation : Quaternion = \ |
||||
skeleton.get_bone_rest(tip_bone_index).basis.get_rotation_quaternion().inverse() * \ |
||||
skeleton.get_bone_pose_rotation(tip_bone_index) |
||||
|
||||
# This is us trying to narrow down the roll component out of the entire |
||||
# rotation by using the dot product of the two axes of rotation as a scaling |
||||
# value for the angle. |
||||
var tip_roll = \ |
||||
lerp_angle( |
||||
0.0, |
||||
current_tip_rotation.get_angle(), |
||||
tip_bone_roll_axis.dot(current_tip_rotation.get_axis())) |
||||
|
||||
var bone_chain_index = bone_count |
||||
|
||||
current_bone = skeleton.get_bone_parent(tip_bone_index) |
||||
while current_bone != -1: |
||||
|
||||
bone_chain_index -= 1 |
||||
|
||||
var avg_child_direction = Vector3(0.0, 0.0, 0.0) |
||||
|
||||
# Save child bone rotations in parent-parent space. |
||||
var this_parent_index = skeleton.get_bone_parent(current_bone) |
||||
var child_indices = skeleton.get_bone_children(current_bone) |
||||
var this_bone_starting_rotation = skeleton.get_bone_pose_rotation(current_bone) |
||||
var preserved_rotations_in_parent_space = [] |
||||
for child in child_indices: |
||||
var child_bone_rotation = skeleton.get_bone_pose_rotation(child) |
||||
preserved_rotations_in_parent_space.append( |
||||
this_bone_starting_rotation * child_bone_rotation) |
||||
|
||||
avg_child_direction += skeleton.get_bone_rest(child).origin |
||||
|
||||
avg_child_direction = avg_child_direction.normalized() |
||||
|
||||
# Rotate the actual bone. |
||||
var rotation_alpha = float(bone_chain_index) / float(bone_count) |
||||
var new_rotation = \ |
||||
this_bone_starting_rotation * \ |
||||
Quaternion(avg_child_direction, tip_roll * rotation_alpha) |
||||
skeleton.set_bone_pose_rotation(current_bone, new_rotation) |
||||
|
||||
# Set all the children back. |
||||
var child_index = 0 |
||||
var new_bone_rotation = skeleton.get_bone_pose_rotation(current_bone) |
||||
for child in child_indices: |
||||
var new_child_bone_rotation = \ |
||||
new_bone_rotation.inverse() * \ |
||||
preserved_rotations_in_parent_space[child_index] |
||||
skeleton.set_bone_pose_rotation(child, new_child_bone_rotation) |
||||
|
||||
child_index += 1 |
||||
|
||||
if current_bone == base_bone_index: |
||||
break |
||||
current_bone = this_parent_index |
||||
|
||||
func rotate_chain_twist_on_secondary_axis( |
||||
skel : Skeleton3D, |
||||
base_bone_index : int, |
||||
tip_bone_index : int, |
||||
target_transform_worldspace : Transform3D, |
||||
forward_axis_for_secondary_rotation : Vector3, |
||||
rotation_scale : float): |
||||
|
||||
#var forward_axis_for_secondary_rotation : Vector3 = Vector3(0.0, 0.0, 1.0) |
||||
|
||||
# Count up how many bones we're going to need to distribute this among. |
||||
var bone_count_to_hips = 0 |
||||
var current_bone_index = tip_bone_index |
||||
while current_bone_index != -1 and current_bone_index != base_bone_index: |
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
bone_count_to_hips += 1 |
||||
|
||||
# Figure out angle difference between the direction where the hips are |
||||
# facing and the direction the head is facing. |
||||
var hips_forward_skelspace = skel.get_bone_global_pose(base_bone_index).basis * forward_axis_for_secondary_rotation |
||||
var head_forward_skelspace = skel.global_transform.basis.inverse() * target_transform_worldspace.basis * forward_axis_for_secondary_rotation |
||||
hips_forward_skelspace.y = 0.0 |
||||
hips_forward_skelspace = hips_forward_skelspace.normalized() |
||||
head_forward_skelspace.y = 0.0 |
||||
head_forward_skelspace = head_forward_skelspace.normalized() |
||||
|
||||
# Figure out how many radians we're off. We're actually not projecting the directions onto |
||||
# the the same plane and comparing there because we just don't have to be as precise for |
||||
# this one. |
||||
var radians_to_rotate_body = \ |
||||
sign(secondary_axis_of_rotation.dot(head_forward_skelspace.cross(hips_forward_skelspace))) * \ |
||||
-acos(hips_forward_skelspace.dot(head_forward_skelspace)) |
||||
|
||||
# Scale rotation amount for body. |
||||
# FIXME: Make configurable. |
||||
# TODO: Make it configurable. |
||||
radians_to_rotate_body *= rotation_scale |
||||
|
||||
# Go from the head down to the hips and apply an even fraction of the rotation (distributing |
||||
# the rotation among all the bones between head and hips). |
||||
current_bone_index = tip_bone_index |
||||
while current_bone_index != -1 and current_bone_index != base_bone_index: |
||||
|
||||
var bone_transform_current = skel.get_bone_global_pose(current_bone_index) |
||||
bone_transform_current = bone_transform_current.rotated( |
||||
secondary_axis_of_rotation, |
||||
radians_to_rotate_body / bone_count_to_hips) |
||||
|
||||
var parent_bone_transform = skel.get_bone_global_pose(skel.get_bone_parent(current_bone_index)) |
||||
skel.set_bone_pose_rotation(current_bone_index, (parent_bone_transform.inverse() * bone_transform_current).basis.get_rotation_quaternion()) |
||||
|
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
|
||||
func rotate_bone_to_match_object( |
||||
skel: Skeleton3D, tip_bone_index: int, |
||||
target_transform_worldspace: Transform3D, |
||||
) -> void: |
||||
var target_basis := skel.global_basis.inverse() * target_transform_worldspace.basis |
||||
var parent_index := skel.get_bone_parent(tip_bone_index) |
||||
var parent_basis := skel.get_bone_global_pose(parent_index).basis |
||||
var new_basis := parent_basis.inverse() * target_basis |
||||
skel.set_bone_pose_rotation(tip_bone_index, new_basis.get_rotation_quaternion()) |
||||
|
||||
func do_ik_chain(): |
||||
|
||||
var target_transform : Transform3D = \ |
||||
tracker_object.global_transform |
||||
|
||||
if _calculated_distance_to_angle_mappings == null: |
||||
evaluate_bone_chain_limit() |
||||
|
||||
var base_bone_index = skeleton.find_bone(base_bone) |
||||
var tip_bone_index = skeleton.find_bone(tip_bone) |
||||
var current_bone_index = tip_bone_index |
||||
|
||||
# Reset all rotations. |
||||
if reset_first: |
||||
while current_bone_index != -1: |
||||
skeleton.reset_bone_pose(current_bone_index) |
||||
current_bone_index = skeleton.get_bone_parent(current_bone_index) |
||||
|
||||
if current_bone_index == base_bone_index: |
||||
break |
||||
|
||||
if do_ik_curve: |
||||
|
||||
var hips_global = skeleton.global_transform * skeleton.get_bone_global_pose(base_bone_index).origin |
||||
var head_tracker_global = target_transform.origin |
||||
|
||||
var head_dist_target : float = (hips_global - head_tracker_global).length() |
||||
|
||||
var best_angle : float = _calculated_max_extension_angle |
||||
|
||||
if head_dist_target <= _calculated_min_extension_distance: |
||||
best_angle = _calculated_min_extension_angle |
||||
elif head_dist_target >= _calculated_max_extension_distance: |
||||
best_angle = _calculated_max_extension_angle |
||||
else: |
||||
for k in range(0, len(_calculated_distance_to_angle_mappings) - 1): |
||||
var dist_high = _calculated_distance_to_angle_mappings[k+1][0] |
||||
var dist_low = _calculated_distance_to_angle_mappings[k][0] |
||||
if dist_low >= head_dist_target and \ |
||||
dist_high <= head_dist_target: |
||||
|
||||
var alpha = (head_dist_target - dist_low) / (dist_high - dist_low) |
||||
|
||||
best_angle = lerp( |
||||
_calculated_distance_to_angle_mappings[k][1], |
||||
_calculated_distance_to_angle_mappings[k+1][1], |
||||
alpha) |
||||
|
||||
break |
||||
|
||||
|
||||
if symmetric: |
||||
var global_symmetric_axis : Vector3 = skeleton.global_transform.basis * skeleton.get_bone_global_pose(base_bone_index).basis * symmetric_axis |
||||
var global_symmetry_check_point : Vector3 = head_tracker_global - hips_global |
||||
var dp = global_symmetric_axis.dot(global_symmetry_check_point) |
||||
best_angle *= -dp * symmetric_influence_scale |
||||
#var symmetric_influence_start_offset : float = -1.0 |
||||
|
||||
attempt_spine_rotation( |
||||
skeleton, best_angle, |
||||
base_bone_index, |
||||
tip_bone_index) |
||||
|
||||
# ----------------------------------------------------------------------------------------- |
||||
# Simpler approach attempt |
||||
|
||||
# var max_dist_angle = 0.0 |
||||
# var min_dist_angle = PI |
||||
# var max_dist = attempt_spine_rotation(max_dist_angle, base_bone_index, tip_bone_index) |
||||
# var min_dist = attempt_spine_rotation(min_dist_angle, base_bone_index, tip_bone_index) |
||||
# |
||||
# var lerp_alpha = (head_dist_target - min_dist) / (max_dist - min_dist) |
||||
# lerp_alpha = clamp(lerp_alpha, 0.0, 1.0) |
||||
# var use_angle = lerp(max_dist_angle, min_dist_angle, 1.0 - lerp_alpha) |
||||
# attempt_spine_rotation(use_angle, base_bone_index, tip_bone_index) |
||||
|
||||
|
||||
# ----------------------------------------------------------------------------------------- |
||||
# Yaw chest to head orientation |
||||
|
||||
if do_yaw and do_yaw_global: |
||||
rotate_chain_twist_on_secondary_axis( |
||||
skeleton, base_bone_index, tip_bone_index, |
||||
target_transform, |
||||
Vector3(0.0, 0.0, 1.0), yaw_scale) |
||||
|
||||
# ----------------------------------------------------------------------------------------- |
||||
# Rotate whole spine section to point towards the head tracker. |
||||
|
||||
if do_point_tracker: |
||||
rotate_chain_so_tip_points_in_direction(skeleton, base_bone_index, tip_bone_index, target_transform.origin) |
||||
|
||||
# ----------------------------------------------------------------------------------------- |
||||
# Aim at pole target |
||||
|
||||
if do_pole_targets: |
||||
if pole_direction_target != Vector3(0.0, 0.0, 0.0): |
||||
rotate_chain_to_pole_target(skeleton, base_bone_index, tip_bone_index, pole_direction_target) |
||||
|
||||
# ----------------------------------------------------------------------------------------- |
||||
# Rotate the head to face the same direction as the head tracker. |
||||
# |
||||
# We have to do this after everything else so the transform of the bone below it is already |
||||
# fully calculated and finalized. |
||||
|
||||
if do_rotate_to_match_tracker: |
||||
rotate_bone_to_match_object(skeleton, tip_bone_index, target_transform) |
||||
|
||||
if do_bone_roll: |
||||
chain_distribute_bone_roll(base_bone_index, tip_bone_index) |
||||
|
||||
|
||||
# This function will rotate a bone in the global (skeleton object) coordiate |
||||
# space, as though it were in its rest position. So hardcoded Y axis can be |
||||
# used for elbow, hardcoded X axis can be used for spine, etc. |
||||
func rotate_bone_in_global_space( |
||||
skel : Skeleton3D, |
||||
bone_index : int, |
||||
axis : Vector3, |
||||
angle : float): |
||||
|
||||
var parent_bone_index = skel.get_bone_parent(bone_index) |
||||
var gs_rotation = Basis(axis.normalized(), angle).get_rotation_quaternion() |
||||
var gs_rotation_parent = skel.get_bone_global_rest(parent_bone_index).basis.get_rotation_quaternion() |
||||
var gs_rotation_rest = skel.get_bone_global_rest(bone_index).basis.get_rotation_quaternion() |
||||
var bs_rotation = gs_rotation_parent.inverse() * gs_rotation * gs_rotation_rest |
||||
skel.set_bone_pose_rotation( |
||||
bone_index, |
||||
bs_rotation) |
||||
|
||||
func attempt_spine_rotation( |
||||
skel : Skeleton3D, |
||||
rotation_amount, base_bone_index, |
||||
tip_bone_index): |
||||
|
||||
var current_bone_index = tip_bone_index |
||||
var first_bone = current_bone_index |
||||
var last_bone = current_bone_index |
||||
|
||||
# Count up bones so we can evenly distribute the rotation out among all of |
||||
# them. |
||||
var bone_count = 0 |
||||
while current_bone_index != -1 and current_bone_index != base_bone_index: |
||||
bone_count += 1 |
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
current_bone_index = tip_bone_index |
||||
|
||||
# Switch over to per-bone rotation amount. |
||||
rotation_amount /= bone_count |
||||
|
||||
while current_bone_index != -1 and current_bone_index != base_bone_index: |
||||
rotate_bone_in_global_space(skel, current_bone_index, main_axis_of_rotation, rotation_amount) |
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
last_bone = current_bone_index |
||||
|
||||
# FIXME: Make this a normal error (non-compliant model). |
||||
assert(last_bone == base_bone_index) |
||||
|
||||
var head_dist = \ |
||||
(skel.get_bone_global_pose(first_bone).origin - |
||||
skel.get_bone_global_pose(last_bone).origin).length() |
||||
|
||||
return head_dist |
||||
|
||||
func evaluate_bone_chain_limit(): |
||||
|
||||
var base_bone_index = skeleton.find_bone(base_bone) |
||||
var tip_bone_index = skeleton.find_bone(tip_bone) |
||||
|
||||
var n = rotation_low |
||||
|
||||
var found_max_extension_angle = 0.0 |
||||
var found_max_extension_angle_distance = 0.0 |
||||
|
||||
var found_min_extension_angle = 0.0 |
||||
var found_min_extension_angle_distance = 99999999.0 |
||||
|
||||
var output_mapping = [] |
||||
|
||||
var sample_count = 64.0 |
||||
|
||||
while n <= rotation_high: |
||||
var dist = attempt_spine_rotation( |
||||
skeleton, n, base_bone_index, |
||||
tip_bone_index) |
||||
|
||||
# # Debug display. |
||||
# var barstr = "" |
||||
# var barcounter = 0 |
||||
# while barcounter < dist: |
||||
# barstr += "#" |
||||
# barcounter += 0.02 |
||||
# print("%2f = %f %s" % [n, dist, barstr]) |
||||
|
||||
n += (rotation_high - rotation_low) / sample_count |
||||
|
||||
if dist > found_max_extension_angle_distance: |
||||
found_max_extension_angle_distance = dist |
||||
found_max_extension_angle = n |
||||
|
||||
if dist < found_min_extension_angle_distance: |
||||
found_min_extension_angle_distance = dist |
||||
found_min_extension_angle = n |
||||
|
||||
# Go through an resample the entire curve given the min/max range that we now know. |
||||
for i in range(0, sample_count): |
||||
var a = float(i) / float(sample_count) |
||||
var angle = lerp(found_max_extension_angle, found_min_extension_angle, a) |
||||
|
||||
var dist = attempt_spine_rotation( |
||||
skeleton, angle, base_bone_index, |
||||
tip_bone_index) |
||||
output_mapping.append( [dist, angle] ) |
||||
|
||||
# Sanity check. |
||||
for k in range(1, len(output_mapping)): |
||||
if output_mapping[k-1][0] < output_mapping[k][0]: |
||||
# FIXME: Normal error message. |
||||
print("BAD SAMPLES") |
||||
|
||||
_calculated_distance_to_angle_mappings = output_mapping |
||||
_calculated_max_extension_angle = found_max_extension_angle |
||||
_calculated_min_extension_angle = found_min_extension_angle |
||||
_calculated_max_extension_distance = found_max_extension_angle_distance |
||||
_calculated_min_extension_distance = found_min_extension_angle_distance |
||||
|
||||
if symmetric: |
||||
_calculated_max_extension_distance += symmetric_influence_start_offset |
||||
|
||||
return [ |
||||
found_max_extension_angle, found_max_extension_angle_distance, |
||||
found_min_extension_angle, found_min_extension_angle_distance, output_mapping ] |
||||
|
||||
# Reset every bone in a bone chain, inclusive of both tip and base bones, to its |
||||
# resting pose. |
||||
func reset_bone_chain( |
||||
skel : Skeleton3D, |
||||
base_bone_index, tip_bone_index): |
||||
|
||||
if base_bone_index is String: |
||||
base_bone_index = skel.find_bone(base_bone_index) |
||||
if tip_bone_index is String: |
||||
tip_bone_index = skel.find_bone(tip_bone_index) |
||||
|
||||
var current_bone_index = tip_bone_index |
||||
while current_bone_index != -1: |
||||
|
||||
skel.reset_bone_pose(current_bone_index) |
||||
|
||||
if current_bone_index == base_bone_index: |
||||
break |
||||
|
||||
current_bone_index = skel.get_bone_parent(current_bone_index) |
||||
Loading…
Reference in new issue