diff --git a/copyMediaPipe.gd b/copyMediaPipe.gd index e4723bd..f8568b0 100644 --- a/copyMediaPipe.gd +++ b/copyMediaPipe.gd @@ -1,17 +1,28 @@ class_name copyMediaPipe extends Mod_Base +# ----------------------------------------------------------------------------- +# Potentially configurable variables. +# ----------------------------------------------------------------------------- + var arm_rest_angle := 65 -var time_to_rest := 0.1 # Time without tracking data before returning to the rest pose. var interpolation_factor := 0.000000001 # Yes this value needs to be THAT small. -var rest_interpolation_factor := 0.2 # "Lerp about 80% in one second." +var rest_interpolation_factor := 0.2 # "Lerp about 80% of the way in one second." +var min_confidence_threshold := 0.85 +var time_to_rest := 0.1 # Time without tracking data before returning to the rest pose. # TODO: Change this via calibration! var camera_transform := Transform3D(Basis(), Vector3(0.0, 0.0, 0.3)) +# ----------------------------------------------------------------------------- +# ----------------------------------------------------------------------------- + # FIXME: Best to get this from the tracker process (if possible). var camera_aspect_ratio := 4.0 / 3.0 # Logitech C920 default? +# TODO: Ensure that this works with the model offset from the world origin. +var ik_chains: Array[copyMediaPipe_IKChain] = [] + @onready var tracking_root: Node3D = $TrackingRoot @onready var landmark_template: MeshInstance3D = $TrackingRoot/LandmarkTemplate @@ -19,7 +30,7 @@ var camera_aspect_ratio := 4.0 / 3.0 # Logitech C920 default? last_data = null, # Most recent tracking data received. last_received = INF, # How long ago it was received (in seconds). tracker = $TrackingRoot/Head, # Node for visualizing tracking data. - rest_pose = Transform3D.IDENTITY, # Rest position of the head (from 0,0,0). + rest_pose = Transform3D.IDENTITY, # Rest position of the head. } @onready var hands := { @@ -39,6 +50,9 @@ var camera_aspect_ratio := 4.0 / 3.0 # Logitech C920 default? }, } +# ----------------------------------------------------------------------------- +# ----------------------------------------------------------------------------- + func _ready() -> void: setup_hand_landmarks() @@ -60,16 +74,18 @@ func _exit_tree() -> void: # Called after mod is initialized or model is changed. func scene_init() -> void: initialize_rest_pose() + initialize_ik_chains() # Called before mod is removed, model is changed or application is shut down. func scene_shutdown() -> void: - pass + ik_chains = [] func _process(delta: float) -> void: increase_last_received(delta) if is_tracker_running(): receive_tracker_packets() update_visual_trackers(delta) + update_ik_chains() ## Sets up 21 nodes for the landmarks that make up hand/finger tracking. func setup_hand_landmarks() -> void: @@ -82,32 +98,85 @@ func setup_hand_landmarks() -> void: hand.tracker.add_child(landmark) hand.landmarks.append(landmark) +# ----------------------------------------------------------------------------- +# Initialization functions that are called when a new model is loaded. +# ----------------------------------------------------------------------------- + ## Initialized the stored rest positions for the head and hands. ## Also applies a rotation to the arms so they're not T-posing. func initialize_rest_pose() -> void: var skel := get_skeleton() if not skel: return - var head_idx := skel.find_bone("Head") - var head_origin := skel.get_bone_global_rest(head_idx).origin + var head_idx := skel.find_bone("Head") + var head_rest := skel.get_bone_global_rest(head_idx) + + # Move the tracking root such that it is at the height of the head. + tracking_root.transform = camera_transform * head_rest - tracking_root.transform = camera_transform * Transform3D(Basis(), head_origin) - head.rest_pose = camera_transform.inverse() + head.rest_pose = tracking_root.transform.inverse() * head_rest for side in hands: - var shoulder_idx := skel.find_bone(side.capitalize() + "Shoulder") - var hand_idx := skel.find_bone(side.capitalize() + "Hand") - var shoulder_transform := skel.get_bone_global_rest(shoulder_idx) - var hand_transform := skel.get_bone_global_rest(hand_idx) + var shoulder_idx := skel.find_bone(side.capitalize() + "Shoulder") + var hand_idx := skel.find_bone(side.capitalize() + "Hand") + var shoulder_rest := skel.get_bone_global_rest(shoulder_idx) + var hand_rest := skel.get_bone_global_rest(hand_idx) # First, get relative transform of hand to shoulder. - var hand_to_shoulder := shoulder_transform.inverse() * hand_transform + var hand_to_shoulder := shoulder_rest.inverse() * hand_rest # Next, rotate this relative transform by arm_rest_angle. hand_to_shoulder = hand_to_shoulder.rotated(Vector3.LEFT, deg_to_rad(arm_rest_angle)) # Finally, put the relative transform back into skeleton-relative coordinates. - var hand_rest_transform := shoulder_transform * hand_to_shoulder + var new_hand_transform := shoulder_rest * hand_to_shoulder + + hands[side].rest_pose = tracking_root.transform.inverse() * new_hand_transform + +## Sets up the inverse kinematics chains to move the model depending on the location of the visual trackers. +func initialize_ik_chains() -> void: + ik_chains = [] + + var chain_spine := copyMediaPipe_IKChain.new() + chain_spine.skeleton = get_skeleton() + chain_spine.base_bone = "Hips" + chain_spine.tip_bone = "Head" + chain_spine.rotation_low = 0.0 * TAU + chain_spine.rotation_high = 1.0 * TAU + chain_spine.do_yaw = true + chain_spine.main_axis_of_rotation = Vector3.RIGHT + chain_spine.secondary_axis_of_rotation = Vector3.UP + chain_spine.pole_direction_target = Vector3.ZERO # No pole target. + chain_spine.tracker_object = head.tracker + chain_spine.yaw_scale = 0.25 # chest_yaw_scale (Unsure what this does.) + ik_chains.append(chain_spine) + + var x_pole_dist = 10.0 + var y_pole_dist = 5.0 + var z_pole_dist = 10.0 + var arm_rotation_axis = Vector3.UP - hands[side].rest_pose = tracking_root.transform.inverse() * hand_rest_transform + for side in hands: + var hand = hands[side] + + var chain_hand := copyMediaPipe_IKChain.new() + chain_hand.skeleton = get_skeleton() + chain_hand.base_bone = side.capitalize() + "UpperArm" + chain_hand.tip_bone = side.capitalize() + "Hand" + chain_hand.rotation_low = 0.025 * TAU + chain_hand.rotation_high = 0.990 * TAU + chain_hand.do_yaw = false + chain_hand.do_bone_roll = true + chain_hand.secondary_axis_of_rotation = Vector3.UP + + if side == "left": + chain_hand.main_axis_of_rotation = -arm_rotation_axis + chain_hand.pole_direction_target = Vector3(x_pole_dist, -y_pole_dist, -z_pole_dist) + chain_hand.tracker_object = hand.tracker + else: + chain_hand.main_axis_of_rotation = arm_rotation_axis + chain_hand.pole_direction_target = Vector3(-x_pole_dist, -y_pole_dist, -z_pole_dist) + chain_hand.tracker_object = hand.tracker + + ik_chains.append(chain_hand) # ----------------------------------------------------------------------------- # Functions to start/stop the PYTHON TRACKER PROCESS and communicate with it. @@ -199,11 +268,7 @@ func process_tracker_data(data: Dictionary) -> void: # Face matrix is in centimeters, convert to meters. data["face"]["transform"].origin /= 100 - # TODO: Make this configurable. - var min_confidence_threshold := 0.85 - # NOTE: Face confidence currently either 0.0 or 1.0. - if data["face"]["confidence"] > min_confidence_threshold: head.last_data = data["face"] head.last_received = 0.0 @@ -236,12 +301,12 @@ func to_vector(array) -> Vector3: func to_transform(matrix) -> Transform3D: return Transform3D( Basis(Vector3(matrix[0][0], matrix[1][0], matrix[2][0]), - Vector3(matrix[0][1], matrix[1][1], matrix[2][1]), - Vector3(matrix[0][2], matrix[1][2], matrix[2][2])), + Vector3(matrix[0][1], matrix[1][1], matrix[2][1]), + Vector3(matrix[0][2], matrix[1][2], matrix[2][2])), Vector3(matrix[0][3], matrix[1][3], matrix[2][3])) # ----------------------------------------------------------------------------- -# Functions that take the CONVERTED DATA and update the VISUAL TRACKER nodes. +# Functions for updating VISUAL TRACKERS and THE MODEL itself. # ----------------------------------------------------------------------------- func update_visual_trackers(delta: float) -> void: @@ -279,14 +344,15 @@ func update_visual_trackers(delta: float) -> void: var pos := world_landmarks[i] - wrist_position hand.landmarks[i].position = hand_rotation.inverse() * pos -## Smoothly interpolates transforms in a framerate-independent way. -## For example, using a factor of 0.2, will move roughly 80% of the remaining distance in a second. -func fi_slerp(value: Transform3D, target: Transform3D, factor: float, delta: float) -> Transform3D: - return value.interpolate_with(target, 1 - factor ** delta) +func update_ik_chains() -> void: + for chain in ik_chains: + chain.do_ik_chain() # ----------------------------------------------------------------------------- +# Utility functions, currently only relating to update_visual_trackers. # ----------------------------------------------------------------------------- +# Indices of hand landmarks. const WRIST := 0 const THUMB_CMC := 1 const THUMB_MCP := 2 @@ -326,7 +392,7 @@ func get_hand_rotation(side: String, landmarks: Array[Vector3]) -> Basis: ## to force the hand and head into the same scale range, roughly. func get_hand_viewspace_origin( image_landmarks: Array[Vector3], - _world_landmarks: Array[Vector3], + _world_landmarks: Array[Vector3], # unused hand_to_head_scale: float, ) -> Vector3: # Values found through experimentation. @@ -404,3 +470,8 @@ func ndc_to_viewspace(v: Vector3, z_offset: float) -> Vector3: var x := (v[0] - px) * z / fx var y := (v[1] - py) * z / fy return Vector3(x, y, z) + +## Smoothly interpolates transforms in a framerate-independent way. +## For example, using a factor of 0.2, will move roughly 80% of the remaining distance in a second. +func fi_slerp(value: Transform3D, target: Transform3D, factor: float, delta: float) -> Transform3D: + return value.interpolate_with(target, 1 - factor ** delta) diff --git a/copyMediaPipe.tscn b/copyMediaPipe.tscn index 92644ba..390ed9c 100644 --- a/copyMediaPipe.tscn +++ b/copyMediaPipe.tscn @@ -23,21 +23,26 @@ script = ExtResource("1_0kpr8") [node name="TrackingRoot" type="Node3D" parent="."] [node name="DebugVisuals" parent="TrackingRoot" instance=ExtResource("2_8wmot")] +visible = false [node name="Head" type="MeshInstance3D" parent="TrackingRoot"] +visible = false mesh = SubResource("BoxMesh_wtdv4") [node name="DebugVisuals" parent="TrackingRoot/Head" instance=ExtResource("2_8wmot")] +visible = false [node name="LeftHand" type="Node3D" parent="TrackingRoot"] transform = Transform3D(-4.37114e-08, 1, -4.37114e-08, 0, -4.37114e-08, -1, -1, -4.37114e-08, 1.91069e-15, 0.5, 0, 0) [node name="DebugVisuals" parent="TrackingRoot/LeftHand" instance=ExtResource("2_8wmot")] +visible = false [node name="RightHand" type="Node3D" parent="TrackingRoot"] transform = Transform3D(1.91069e-15, -1, 4.37114e-08, -4.37114e-08, -4.37114e-08, -1, 1, 0, -4.37114e-08, -0.5, 0, 0) [node name="DebugVisuals" parent="TrackingRoot/RightHand" instance=ExtResource("2_8wmot")] +visible = false [node name="LandmarkTemplate" type="MeshInstance3D" parent="TrackingRoot"] transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.5, 0) diff --git a/copyMediaPipe_IKChain.gd b/copyMediaPipe_IKChain.gd new file mode 100644 index 0000000..9330dbf --- /dev/null +++ b/copyMediaPipe_IKChain.gd @@ -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)