[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

@@ -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).

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