From f863ebe52c38f2c39687cc3fce9730706576e379 Mon Sep 17 00:00:00 2001 From: Aaron Franke Date: Mon, 3 Jun 2019 16:15:22 -0400 Subject: [PATCH] [IK] Refactor ik_fabrik.gd Add static typing in many places, cache values to improve performance, use printerr, use Vector3.UP, and use linear_interpolate method --- 3d/ik/addons/sade/ik_fabrik.gd | 96 +++++++++++++++++----------------- 3d/ik/fabrik_ik.tscn | 3 -- 2 files changed, 47 insertions(+), 52 deletions(-) diff --git a/3d/ik/addons/sade/ik_fabrik.gd b/3d/ik/addons/sade/ik_fabrik.gd index 14440b77..cbcc04f7 100644 --- a/3d/ik/addons/sade/ik_fabrik.gd +++ b/3d/ik/addons/sade/ik_fabrik.gd @@ -11,9 +11,9 @@ export (PoolRealArray) var bones_in_chain_lengths setget _set_bone_chain_lengths export (int, "_process", "_physics_process", "_notification", "none") var update_mode = 0 setget _set_update_mode -var target = null +var target: Spatial = null -var skeleton +var skeleton: Skeleton # A dictionary holding all of the bone IDs (from the skeleton) and a dictionary holding # all of the bone helper nodes @@ -21,30 +21,30 @@ var bone_IDs = {} var bone_nodes = {} # The position of the origin -var chain_origin = null +var chain_origin: Vector3 # The combined length of every bone in the bone chain -var total_length = null +var total_length: float = INF # The delta/tolerance for the bone chain (how do the bones need to be before it is considered satisfactory) -const CHAIN_TOLERANCE = 0.01 +const CHAIN_TOLERANCE: float = 0.01 # The amount of interations the bone chain will go through in an attempt to get to the target position -const CHAIN_MAX_ITER = 10 +const CHAIN_MAX_ITER: int = 10 # The amount of iterations we've been through, and whether or not we want to limit our solver to CHAIN_MAX_ITER # amounts of interations. -export (int) var chain_iterations = 0 -export (bool) var limit_chain_iterations = true +export (int) var chain_iterations: int = 0 +export (bool) var limit_chain_iterations := true # Should we reset chain_iterations on movement during our update method? -export (bool) var reset_iterations_on_update = false +export (bool) var reset_iterations_on_update := false # A boolean to track whether or not we want to move the middle joint towards middle joint target. -export (bool) var use_middle_joint_target = false -var middle_joint_target = null +export (bool) var use_middle_joint_target := false +var middle_joint_target: Spatial = null # Have we called _set_skeleton_path or not already. Due to some issues using exported NodePaths, # we need to ignore the first _set_skeleton_path call. -var first_call = true +var first_call := true # A boolean to track whether or not we want to print debug messages -var debug_messages = false +var debug_messages := false func _ready(): @@ -136,7 +136,7 @@ func _set_update_mode(new_value): set_notify_transform(true) else: if debug_messages == true: - print (name, " - IK_FABRIK: Unknown update mode. NOT updating skeleton") + printerr (name, " - IK_FABRIK: Unknown update mode. NOT updating skeleton") return @@ -151,7 +151,7 @@ func _set_skeleton_path(new_value): if skeleton_path == null: if debug_messages == true: - print (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!") + printerr (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!") return var temp = get_node(skeleton_path) @@ -165,15 +165,15 @@ func _set_skeleton_path(new_value): _make_bone_nodes() if debug_messages == true: - print (name, " - IK_FABRIK: Attached to a new skeleton") + printerr (name, " - IK_FABRIK: Attached to a new skeleton") # If not, then it's (likely) not a Skeleton node else: skeleton = null if debug_messages == true: - print (name, " - IK_FABRIK: skeleton_path does not point to a skeleton!") + printerr (name, " - IK_FABRIK: skeleton_path does not point to a skeleton!") else: if debug_messages == true: - print (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!") + printerr (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!") ############# OTHER (NON IK SOLVER RELATED) FUNCTIONS ############# @@ -213,7 +213,7 @@ func _set_bone_chain_bones(new_value): func _set_bone_chain_lengths(new_value): bones_in_chain_lengths = new_value - total_length = null + total_length = INF # Various upate methods @@ -253,16 +253,16 @@ func update_skeleton(): if bones_in_chain == null: if debug_messages == true: - print (name, " - IK_FABRIK: No Bones in IK chain defined!") + printerr (name, " - IK_FABRIK: No Bones in IK chain defined!") return if bones_in_chain_lengths == null: if debug_messages == true: - print (name, " - IK_FABRIK: No Bone lengths in IK chain defined!") + printerr (name, " - IK_FABRIK: No Bone lengths in IK chain defined!") return if bones_in_chain.size() != bones_in_chain_lengths.size(): if debug_messages == true: - print (name, " - IK_FABRIK: bones_in_chain and bones_in_chain_lengths!") + printerr (name, " - IK_FABRIK: bones_in_chain and bones_in_chain_lengths!") return ################################ @@ -277,12 +277,12 @@ func update_skeleton(): bone_nodes[i].global_transform = get_bone_transform(i) # If this is not the last bone in the bone chain, make it look at the next bone in the bone chain if i < bone_IDs.size()-1: - bone_nodes[i].look_at(get_bone_transform(i+1).origin + skeleton.global_transform.origin, Vector3(0, 1, 0)) + bone_nodes[i].look_at(get_bone_transform(i+1).origin + skeleton.global_transform.origin, Vector3.UP) i += 1 # Set the total length of the bone chain, if it is not already set - if total_length == null: + if total_length == INF: total_length = 0 for bone_length in bones_in_chain_lengths: total_length += bone_length @@ -303,12 +303,11 @@ func solve_chain(): chain_iterations = 0 # Update the origin with the current bone's origin - chain_origin = get_bone_transform(0) + chain_origin = get_bone_transform(0).origin # Get the direction of the final bone by using the next to last bone if there is more than 2 bones. # If there are only 2 bones, we use the target's forward Z vector instead (not ideal, but it works fairly well) - #var dir = -target.global_transform.basis.z.normalized() - var dir + var dir: Vector3 if bone_nodes.size() > 2: dir = bone_nodes[bone_nodes.size()-2].global_transform.basis.z.normalized() else: @@ -324,26 +323,26 @@ func solve_chain(): bone_nodes[bone_nodes.size()/2].global_transform.origin = middle_point_pos.origin # Get the distance from the origin to the target - var distance = (chain_origin.origin - target_pos).length() + var distance = (chain_origin - target_pos).length() # If the distance is farther than our total reach, the target cannot be reached. # Make the bone chain a straight line pointing towards the target if distance > total_length: for i in range (0, bones_in_chain.size()): # Create a direct line to target and make this bone travel down that line - - var r = (target_pos - bone_nodes[i].global_transform.origin).length() + var curr_origin: Vector3 = bone_nodes[i].global_transform.origin + var r = (target_pos - curr_origin).length() var l = bones_in_chain_lengths[i] / r # Find new join position - var new_pos = (1-l) * bone_nodes[i].global_transform.origin + l * target_pos + var new_pos = curr_origin.linear_interpolate(target_pos, l) # Apply it to the bone node - bone_nodes[i].look_at(new_pos, Vector3(0, 1, 0)) + bone_nodes[i].look_at(new_pos, Vector3.UP) bone_nodes[i].global_transform.origin = new_pos # Apply the rotation to the first node in the bone chain, making it look at the next bone in the bone chain - bone_nodes[0].look_at(bone_nodes[1].global_transform.origin, Vector3(0, 1, 0)) + bone_nodes[0].look_at(bone_nodes[1].global_transform.origin, Vector3.UP) # If the distance is NOT farther than our total reach, the target can be reached. else: @@ -377,7 +376,7 @@ func chain_backward(): # Get the direction of the final bone by using the next to last bone if there is more than 2 bones. # If there are only 2 bones, we use the target's forward Z vector instead (not ideal, but it works fairly well) - var dir + var dir: Vector3 if bone_nodes.size() > 2: dir = bone_nodes[bone_nodes.size()-2].global_transform.basis.z.normalized() else: @@ -389,33 +388,32 @@ func chain_backward(): # For all of the other bones, move them towards the target var i = bones_in_chain.size() - 1 while i >= 1: - + var prev_origin: Vector3 = bone_nodes[i].global_transform.origin i -= 1 + var curr_origin: Vector3 = bone_nodes[i].global_transform.origin - var r = bone_nodes[i+1].global_transform.origin - bone_nodes[i].global_transform.origin + var r = prev_origin - curr_origin var l = bones_in_chain_lengths[i] / r.length() # Apply the new joint position - bone_nodes[i].global_transform.origin = (1 - l) * bone_nodes[i+1].global_transform.origin + l * bone_nodes[i].global_transform.origin + bone_nodes[i].global_transform.origin = prev_origin.linear_interpolate(curr_origin, l) func chain_forward(): # Forward reaching pass # Set root at initial position - bone_nodes[0].global_transform.origin = chain_origin.origin + bone_nodes[0].global_transform.origin = chain_origin # Go through every bone in the bone chain var i = 0 while i < bones_in_chain.size() - 1: + var curr_origin: Vector3 = bone_nodes[i].global_transform.origin + var next_origin: Vector3 = bone_nodes[i+1].global_transform.origin - var r = (bone_nodes[i+1].global_transform.origin - bone_nodes[i].global_transform.origin) + var r = next_origin - curr_origin var l = bones_in_chain_lengths[i] / r.length() - - # Set the new joint position - var new_pos = (1 - l) * bone_nodes[i].global_transform.origin + l * bone_nodes[i+1].global_transform.origin - # Apply the new joint position, (potentially with constraints), to the bone node - bone_nodes[i+1].global_transform.origin = new_pos + bone_nodes[i+1].global_transform.origin = curr_origin.linear_interpolate(next_origin, l) i += 1 @@ -447,11 +445,11 @@ func chain_apply_rotation(): var dir = (target.global_transform.origin - b_target_two.origin).normalized() # Make this bone look in the same the direction as the last bone - bone_trans = bone_trans.looking_at(b_target.origin + dir, Vector3(0, 1, 0)) + bone_trans = bone_trans.looking_at(b_target.origin + dir, Vector3.UP) else: var b_target = target.global_transform b_target.origin = skeleton.global_transform.xform_inv(b_target.origin) - bone_trans = bone_trans.looking_at(b_target.origin, Vector3(0, 1, 0)) + bone_trans = bone_trans.looking_at(b_target.origin, Vector3.UP) # If this is NOT the last bone in the bone chain, rotate the bone to look at the next # bone in the bone chain. @@ -465,10 +463,10 @@ func chain_apply_rotation(): b_target_two.origin = skeleton.global_transform.xform_inv(b_target_two.origin) # Get the direction towards the next bone - var dir = (b_target_two.origin - b_target.origin).normalized() + var dir: Vector3 = (b_target_two.origin - b_target.origin).normalized() # Make this bone look towards the direction of the next bone - bone_trans = bone_trans.looking_at(b_target.origin + dir, Vector3(0, 1, 0)) + bone_trans = bone_trans.looking_at(b_target.origin + dir, Vector3.UP) # The the bone's (updated) transform set_bone_transform(i, bone_trans) @@ -477,7 +475,7 @@ func chain_apply_rotation(): func get_bone_transform(bone, convert_to_world_space=true): # Get the global transform of the bone - var ret = skeleton.get_bone_global_pose(bone_IDs[bones_in_chain[bone]]) + var ret: Transform = skeleton.get_bone_global_pose(bone_IDs[bones_in_chain[bone]]) # If we need to convert the bone position from bone/skeleton space to world space, we # use the Xform of the skeleton (because bone/skeleton space is relative to the position of the skeleton node). diff --git a/3d/ik/fabrik_ik.tscn b/3d/ik/fabrik_ik.tscn index 14fd024c..d06c3c28 100644 --- a/3d/ik/fabrik_ik.tscn +++ b/3d/ik/fabrik_ik.tscn @@ -67,7 +67,6 @@ material/0 = ExtResource( 3 ) material/1 = ExtResource( 4 ) [node name="Camera" type="Camera" parent="."] -editor/display_folded = true transform = Transform( 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 11.5, 8.8 ) fov = 74.0 script = ExtResource( 5 ) @@ -95,7 +94,6 @@ __meta__ = { skeleton_path = NodePath("../../../BattleBot/Armature/Skeleton") bones_in_chain = PoolStringArray( "Left_UpperArm", "Left_LowerArm" ) bones_in_chain_lengths = PoolRealArray( 1.97, 3 ) -chain_iterations = 10 limit_chain_iterations = false use_middle_joint_target = true @@ -131,7 +129,6 @@ __meta__ = { skeleton_path = NodePath("../../../BattleBot/Armature/Skeleton") bones_in_chain = PoolStringArray( "Right_UpperArm", "Right_LowerArm", "Right_Hand" ) bones_in_chain_lengths = PoolRealArray( 1.97, 3, 1.2 ) -chain_iterations = 2 limit_chain_iterations = false use_middle_joint_target = true