|
@@ -1,15 +1,17 @@
|
|
|
tool
|
|
|
extends Spatial
|
|
|
+# A FABRIK IK chain with a middle joint helper.
|
|
|
|
|
|
-"""
|
|
|
-A FABRIK IK chain with a middle joint helper.
|
|
|
-"""
|
|
|
+# The delta/tolerance for the bone chain (how do the bones need to be before it is considered satisfactory)
|
|
|
+const CHAIN_TOLERANCE = 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
|
|
|
|
|
|
-export (NodePath) var skeleton_path setget _set_skeleton_path
|
|
|
-export (PoolStringArray) var bones_in_chain setget _set_bone_chain_bones
|
|
|
-export (PoolRealArray) var bones_in_chain_lengths setget _set_bone_chain_lengths
|
|
|
+export(NodePath) var skeleton_path setget _set_skeleton_path
|
|
|
+export(PoolStringArray) var bones_in_chain setget _set_bone_chain_bones
|
|
|
+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
|
|
|
+export(int, "_process", "_physics_process", "_notification", "none") var update_mode = 0 setget _set_update_mode
|
|
|
|
|
|
var target: Spatial = null
|
|
|
|
|
@@ -21,34 +23,30 @@ var bone_IDs = {}
|
|
|
var bone_nodes = {}
|
|
|
|
|
|
# The position of the origin
|
|
|
-var chain_origin: Vector3
|
|
|
+var chain_origin = Vector3()
|
|
|
# The combined length of every bone in the bone chain
|
|
|
-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: 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: int = 10
|
|
|
+var total_length = INF
|
|
|
# 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: int = 0
|
|
|
-export (bool) var limit_chain_iterations := true
|
|
|
+export(int) var chain_iterations = 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
|
|
|
+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():
|
|
|
- if (target == null):
|
|
|
+ if target == null:
|
|
|
# NOTE: you HAVE to have a node called target as a child of this node!
|
|
|
# so we create one if one doesn't already exist
|
|
|
if has_node("target") == false:
|
|
@@ -93,131 +91,7 @@ func _ready():
|
|
|
_set_update_mode(update_mode)
|
|
|
|
|
|
|
|
|
-func _make_editor_sphere_at_node(node, color):
|
|
|
- # So we can see the target in the editor, let's create a mesh instance,
|
|
|
- # Add it as our child, and name it
|
|
|
- var indicator = MeshInstance.new()
|
|
|
- node.add_child(indicator)
|
|
|
- indicator.name = "(EditorOnly) Visual indicator"
|
|
|
-
|
|
|
- # We need to make a mesh for the mesh instance.
|
|
|
- # The code below makes a small sphere mesh
|
|
|
- var indicator_mesh = SphereMesh.new()
|
|
|
- indicator_mesh.radius = 0.1
|
|
|
- indicator_mesh.height = 0.2
|
|
|
- indicator_mesh.radial_segments = 8
|
|
|
- indicator_mesh.rings = 4
|
|
|
-
|
|
|
- # The mesh needs a material (unless we want to use the defualt one).
|
|
|
- # Let's create a material and use the EditorGizmoTexture to texture it.
|
|
|
- var indicator_material = SpatialMaterial.new()
|
|
|
- indicator_material.flags_unshaded = true
|
|
|
- indicator_material.albedo_texture = preload("editor_gizmo_texture.png")
|
|
|
- indicator_material.albedo_color = color
|
|
|
- indicator_mesh.material = indicator_material
|
|
|
- indicator.mesh = indicator_mesh
|
|
|
-
|
|
|
-
|
|
|
-############# SETGET FUNCTIONS #############
|
|
|
-
|
|
|
-
|
|
|
-func _set_update_mode(new_value):
|
|
|
- update_mode = new_value
|
|
|
-
|
|
|
- set_process(false)
|
|
|
- set_physics_process(false)
|
|
|
- set_notify_transform(false)
|
|
|
-
|
|
|
- if update_mode == 0:
|
|
|
- set_process(true)
|
|
|
- elif update_mode == 1:
|
|
|
- set_process(true)
|
|
|
- elif update_mode == 2:
|
|
|
- set_notify_transform(true)
|
|
|
- else:
|
|
|
- if debug_messages == true:
|
|
|
- printerr (name, " - IK_FABRIK: Unknown update mode. NOT updating skeleton")
|
|
|
- return
|
|
|
-
|
|
|
-
|
|
|
-func _set_skeleton_path(new_value):
|
|
|
-
|
|
|
- # Because get_node doesn't work in the first call, we just want to assign instead
|
|
|
- if first_call == true:
|
|
|
- skeleton_path = new_value
|
|
|
- return
|
|
|
-
|
|
|
- skeleton_path = new_value
|
|
|
-
|
|
|
- if skeleton_path == null:
|
|
|
- if debug_messages == true:
|
|
|
- printerr (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!")
|
|
|
- return
|
|
|
-
|
|
|
- var temp = get_node(skeleton_path)
|
|
|
- if temp != null:
|
|
|
- # If it has the method "get_bone_global_pose" it is likely a Skeleton
|
|
|
- if temp.has_method("get_bone_global_pose") == true:
|
|
|
- skeleton = temp
|
|
|
- bone_IDs = {}
|
|
|
-
|
|
|
- # (Delete all of the old bone nodes and) Make all of the bone nodes for each bone in the IK chain
|
|
|
- _make_bone_nodes()
|
|
|
-
|
|
|
- if debug_messages == true:
|
|
|
- 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:
|
|
|
- printerr (name, " - IK_FABRIK: skeleton_path does not point to a skeleton!")
|
|
|
- else:
|
|
|
- if debug_messages == true:
|
|
|
- printerr (name, " - IK_FABRIK: No Nodepath selected for skeleton_path!")
|
|
|
-
|
|
|
-############# OTHER (NON IK SOLVER RELATED) FUNCTIONS #############
|
|
|
-
|
|
|
-
|
|
|
-func _make_bone_nodes():
|
|
|
- # Remove all of the old bone nodes
|
|
|
- # TODO: (not a huge concern, as these can be removed in the editor)
|
|
|
-
|
|
|
- for bone in range(0, bones_in_chain.size()):
|
|
|
-
|
|
|
- var bone_name = bones_in_chain[bone]
|
|
|
- if has_node(bone_name) == false:
|
|
|
- var new_node = Spatial.new()
|
|
|
- bone_nodes[bone] = new_node
|
|
|
- add_child(bone_nodes[bone])
|
|
|
-
|
|
|
- if Engine.editor_hint == true:
|
|
|
- if get_tree() != null:
|
|
|
- if get_tree().edited_scene_root != null:
|
|
|
- bone_nodes[bone].set_owner(get_tree().edited_scene_root)
|
|
|
-
|
|
|
- bone_nodes[bone].name = bone_name
|
|
|
-
|
|
|
- else:
|
|
|
- bone_nodes[bone] = get_node(bone_name)
|
|
|
-
|
|
|
- # If we are in the editor, we want to make a sphere at this node
|
|
|
- if Engine.editor_hint == true:
|
|
|
- _make_editor_sphere_at_node(bone_nodes[bone], Color(0.65, 0, 1, 1))
|
|
|
-
|
|
|
-
|
|
|
-func _set_bone_chain_bones(new_value):
|
|
|
- bones_in_chain = new_value
|
|
|
-
|
|
|
- _make_bone_nodes()
|
|
|
-
|
|
|
-
|
|
|
-func _set_bone_chain_lengths(new_value):
|
|
|
- bones_in_chain_lengths = new_value
|
|
|
- total_length = INF
|
|
|
-
|
|
|
-
|
|
|
# Various upate methods
|
|
|
-# ---------------------
|
|
|
func _process(_delta):
|
|
|
if reset_iterations_on_update == true:
|
|
|
chain_iterations = 0
|
|
@@ -240,7 +114,6 @@ func _notification(what):
|
|
|
############# IK SOLVER RELATED FUNCTIONS #############
|
|
|
|
|
|
func update_skeleton():
|
|
|
-
|
|
|
#### ERROR CHECKING conditions
|
|
|
if first_call == true:
|
|
|
_set_skeleton_path(skeleton_path)
|
|
@@ -253,16 +126,16 @@ func update_skeleton():
|
|
|
|
|
|
if bones_in_chain == null:
|
|
|
if debug_messages == true:
|
|
|
- printerr (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:
|
|
|
- printerr (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:
|
|
|
- printerr (name, " - IK_FABRIK: bones_in_chain and bones_in_chain_lengths!")
|
|
|
+ printerr(name, " - IK_FABRIK: bones_in_chain and bones_in_chain_lengths!")
|
|
|
return
|
|
|
|
|
|
################################
|
|
@@ -287,14 +160,11 @@ func update_skeleton():
|
|
|
for bone_length in bones_in_chain_lengths:
|
|
|
total_length += bone_length
|
|
|
|
|
|
-
|
|
|
# Solve the bone chain
|
|
|
solve_chain()
|
|
|
|
|
|
|
|
|
-
|
|
|
func solve_chain():
|
|
|
-
|
|
|
# If we have reached our max chain iteration, and we are limiting ourselves, then return.
|
|
|
# Otherwise set chain_iterations to zero (so we constantly update)
|
|
|
if chain_iterations >= CHAIN_MAX_ITER and limit_chain_iterations == true:
|
|
@@ -307,7 +177,7 @@ func solve_chain():
|
|
|
|
|
|
# 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: Vector3
|
|
|
+ var dir
|
|
|
if bone_nodes.size() > 2:
|
|
|
dir = bone_nodes[bone_nodes.size()-2].global_transform.basis.z.normalized()
|
|
|
else:
|
|
@@ -330,8 +200,8 @@ func solve_chain():
|
|
|
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 curr_origin: Vector3 = bone_nodes[i].global_transform.origin
|
|
|
- var r = (target_pos - curr_origin).length()
|
|
|
+ var curr_origin = 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
|
|
@@ -365,20 +235,19 @@ func solve_chain():
|
|
|
break
|
|
|
|
|
|
# Reset the bone node transforms to the skeleton bone transforms
|
|
|
- #if (constrained == false): # Resetting seems to break bone constraints...
|
|
|
+ #if constrained == false: # Resetting seems to break bone constraints...
|
|
|
for i in range(0, bone_nodes.size()):
|
|
|
var reset_bone_trans = get_bone_transform(i)
|
|
|
bone_nodes[i].global_transform = reset_bone_trans
|
|
|
|
|
|
|
|
|
+# Backward reaching pass
|
|
|
func chain_backward():
|
|
|
- # Backward reaching pass
|
|
|
-
|
|
|
# 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: Vector3
|
|
|
+ var dir
|
|
|
if bone_nodes.size() > 2:
|
|
|
- dir = bone_nodes[bone_nodes.size()-2].global_transform.basis.z.normalized()
|
|
|
+ dir = bone_nodes[bone_nodes.size() - 2].global_transform.basis.z.normalized()
|
|
|
else:
|
|
|
dir = -target.global_transform.basis.z.normalized()
|
|
|
|
|
@@ -388,9 +257,9 @@ 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
|
|
|
+ var prev_origin = bone_nodes[i].global_transform.origin
|
|
|
i -= 1
|
|
|
- var curr_origin: Vector3 = bone_nodes[i].global_transform.origin
|
|
|
+ var curr_origin = bone_nodes[i].global_transform.origin
|
|
|
|
|
|
var r = prev_origin - curr_origin
|
|
|
var l = bones_in_chain_lengths[i] / r.length()
|
|
@@ -398,40 +267,32 @@ func chain_backward():
|
|
|
bone_nodes[i].global_transform.origin = prev_origin.linear_interpolate(curr_origin, l)
|
|
|
|
|
|
|
|
|
+# Forward reaching pass
|
|
|
func chain_forward():
|
|
|
- # Forward reaching pass
|
|
|
-
|
|
|
# Set root at initial position
|
|
|
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
|
|
|
+ for i in range(bones_in_chain.size() - 1):
|
|
|
+ var curr_origin = bone_nodes[i].global_transform.origin
|
|
|
+ var next_origin = bone_nodes[i + 1].global_transform.origin
|
|
|
|
|
|
var r = next_origin - curr_origin
|
|
|
var l = bones_in_chain_lengths[i] / r.length()
|
|
|
# Apply the new joint position, (potentially with constraints), to the bone node
|
|
|
- bone_nodes[i+1].global_transform.origin = curr_origin.linear_interpolate(next_origin, l)
|
|
|
-
|
|
|
- i += 1
|
|
|
+ bone_nodes[i + 1].global_transform.origin = curr_origin.linear_interpolate(next_origin, l)
|
|
|
|
|
|
|
|
|
+# Make all of the bones rotated correctly.
|
|
|
func chain_apply_rotation():
|
|
|
- # Make all of the bones rotated correctly.
|
|
|
-
|
|
|
# For each bone in the bone chain
|
|
|
for i in range(0, bones_in_chain.size()):
|
|
|
-
|
|
|
# Get the bone's transform, NOT converted to world space
|
|
|
var bone_trans = get_bone_transform(i, false)
|
|
|
-
|
|
|
# If this is the last bone in the bone chain, rotate the bone so it faces
|
|
|
# the same direction as the next to last bone in the bone chain if there are more than
|
|
|
# two bones. If there are only two bones, rotate the end effector towards the target
|
|
|
- if i == bones_in_chain.size()-1:
|
|
|
-
|
|
|
+ if i == bones_in_chain.size() - 1:
|
|
|
if bones_in_chain.size() > 2:
|
|
|
# Get the bone node for this bone, and the previous bone
|
|
|
var b_target = bone_nodes[i].global_transform
|
|
@@ -463,7 +324,7 @@ 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: Vector3 = (b_target_two.origin - b_target.origin).normalized()
|
|
|
+ var dir = (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.UP)
|
|
@@ -472,8 +333,7 @@ func chain_apply_rotation():
|
|
|
set_bone_transform(i, bone_trans)
|
|
|
|
|
|
|
|
|
-func get_bone_transform(bone, convert_to_world_space=true):
|
|
|
-
|
|
|
+func get_bone_transform(bone, convert_to_world_space = true):
|
|
|
# Get the global transform of the bone
|
|
|
var ret: Transform = skeleton.get_bone_global_pose(bone_IDs[bones_in_chain[bone]])
|
|
|
|
|
@@ -489,4 +349,125 @@ func set_bone_transform(bone, trans):
|
|
|
# Set the global transform of the bone
|
|
|
skeleton.set_bone_global_pose(bone_IDs[bones_in_chain[bone]], trans)
|
|
|
|
|
|
+############# END OF IK SOLVER RELATED FUNCTIONS #############
|
|
|
+
|
|
|
+
|
|
|
+func _make_editor_sphere_at_node(node, color):
|
|
|
+ # So we can see the target in the editor, let's create a mesh instance,
|
|
|
+ # Add it as our child, and name it
|
|
|
+ var indicator = MeshInstance.new()
|
|
|
+ node.add_child(indicator)
|
|
|
+ indicator.name = "(EditorOnly) Visual indicator"
|
|
|
+
|
|
|
+ # We need to make a mesh for the mesh instance.
|
|
|
+ # The code below makes a small sphere mesh
|
|
|
+ var indicator_mesh = SphereMesh.new()
|
|
|
+ indicator_mesh.radius = 0.1
|
|
|
+ indicator_mesh.height = 0.2
|
|
|
+ indicator_mesh.radial_segments = 8
|
|
|
+ indicator_mesh.rings = 4
|
|
|
|
|
|
+ # The mesh needs a material (unless we want to use the defualt one).
|
|
|
+ # Let's create a material and use the EditorGizmoTexture to texture it.
|
|
|
+ var indicator_material = SpatialMaterial.new()
|
|
|
+ indicator_material.flags_unshaded = true
|
|
|
+ indicator_material.albedo_texture = preload("editor_gizmo_texture.png")
|
|
|
+ indicator_material.albedo_color = color
|
|
|
+ indicator_mesh.material = indicator_material
|
|
|
+ indicator.mesh = indicator_mesh
|
|
|
+
|
|
|
+
|
|
|
+############# SETGET FUNCTIONS #############
|
|
|
+
|
|
|
+func _set_update_mode(new_value):
|
|
|
+ update_mode = new_value
|
|
|
+
|
|
|
+ set_process(false)
|
|
|
+ set_physics_process(false)
|
|
|
+ set_notify_transform(false)
|
|
|
+
|
|
|
+ if update_mode == 0:
|
|
|
+ set_process(true)
|
|
|
+ elif update_mode == 1:
|
|
|
+ set_process(true)
|
|
|
+ elif update_mode == 2:
|
|
|
+ set_notify_transform(true)
|
|
|
+ else:
|
|
|
+ if debug_messages == true:
|
|
|
+ printerr(name, " - IK_FABRIK: Unknown update mode. NOT updating skeleton")
|
|
|
+ return
|
|
|
+
|
|
|
+
|
|
|
+func _set_skeleton_path(new_value):
|
|
|
+ # Because get_node doesn't work in the first call, we just want to assign instead
|
|
|
+ if first_call == true:
|
|
|
+ skeleton_path = new_value
|
|
|
+ return
|
|
|
+
|
|
|
+ skeleton_path = new_value
|
|
|
+
|
|
|
+ if skeleton_path == null:
|
|
|
+ if debug_messages == true:
|
|
|
+ printerr(name, " - IK_FABRIK: No Nodepath selected for skeleton_path!")
|
|
|
+ return
|
|
|
+
|
|
|
+ var temp = get_node(skeleton_path)
|
|
|
+ if temp != null:
|
|
|
+ # If it has the method "get_bone_global_pose" it is likely a Skeleton
|
|
|
+ if temp.has_method("get_bone_global_pose") == true:
|
|
|
+ skeleton = temp
|
|
|
+ bone_IDs = {}
|
|
|
+
|
|
|
+ # (Delete all of the old bone nodes and) Make all of the bone nodes for each bone in the IK chain
|
|
|
+ _make_bone_nodes()
|
|
|
+
|
|
|
+ if debug_messages == true:
|
|
|
+ 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:
|
|
|
+ printerr(name, " - IK_FABRIK: skeleton_path does not point to a skeleton!")
|
|
|
+ else:
|
|
|
+ if debug_messages == true:
|
|
|
+ printerr(name, " - IK_FABRIK: No Nodepath selected for skeleton_path!")
|
|
|
+
|
|
|
+
|
|
|
+############# OTHER (NON IK SOLVER RELATED) FUNCTIONS #############
|
|
|
+
|
|
|
+func _make_bone_nodes():
|
|
|
+ # Remove all of the old bone nodes
|
|
|
+ # TODO: (not a huge concern, as these can be removed in the editor)
|
|
|
+
|
|
|
+ for bone in range(0, bones_in_chain.size()):
|
|
|
+
|
|
|
+ var bone_name = bones_in_chain[bone]
|
|
|
+ if has_node(bone_name) == false:
|
|
|
+ var new_node = Spatial.new()
|
|
|
+ bone_nodes[bone] = new_node
|
|
|
+ add_child(bone_nodes[bone])
|
|
|
+
|
|
|
+ if Engine.editor_hint == true:
|
|
|
+ if get_tree() != null:
|
|
|
+ if get_tree().edited_scene_root != null:
|
|
|
+ bone_nodes[bone].set_owner(get_tree().edited_scene_root)
|
|
|
+
|
|
|
+ bone_nodes[bone].name = bone_name
|
|
|
+
|
|
|
+ else:
|
|
|
+ bone_nodes[bone] = get_node(bone_name)
|
|
|
+
|
|
|
+ # If we are in the editor, we want to make a sphere at this node
|
|
|
+ if Engine.editor_hint == true:
|
|
|
+ _make_editor_sphere_at_node(bone_nodes[bone], Color(0.65, 0, 1, 1))
|
|
|
+
|
|
|
+
|
|
|
+func _set_bone_chain_bones(new_value):
|
|
|
+ bones_in_chain = new_value
|
|
|
+
|
|
|
+ _make_bone_nodes()
|
|
|
+
|
|
|
+
|
|
|
+func _set_bone_chain_lengths(new_value):
|
|
|
+ bones_in_chain_lengths = new_value
|
|
|
+ total_length = INF
|