You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
70 lines
2.0 KiB
70 lines
2.0 KiB
extends Node
|
|
|
|
@export_node_path("Node2D")
|
|
var target_node_path = NodePath()
|
|
|
|
@export var flip_bend_direction = false
|
|
@export var joint_one_bone_index = -1
|
|
@export var joint_two_bone_index = -1
|
|
|
|
var _angle_a = 0.0
|
|
var _angle_b = 0.0
|
|
|
|
|
|
func _process(delta: float) -> void:
|
|
_update_two_bone_ik_angles()
|
|
|
|
|
|
func _update_two_bone_ik_angles():
|
|
assert(joint_one_bone_index != -1)
|
|
assert(joint_two_bone_index != -1)
|
|
|
|
if target_node_path.is_empty():
|
|
return
|
|
|
|
var target = get_node(target_node_path) as Node2D
|
|
var bone_a = get_parent().get_bone(joint_one_bone_index)
|
|
var bone_b = get_parent().get_bone(joint_two_bone_index)
|
|
|
|
var bone_a_len = bone_a.get_length()
|
|
var bone_b_len = bone_b.get_length()
|
|
|
|
var sin_angle2 = 0.0
|
|
var cos_angle2 = 1.0
|
|
|
|
_angle_b = 0.0
|
|
|
|
var cos_angle2_denom = 2.0 * bone_a_len * bone_b_len
|
|
if not is_zero_approx(cos_angle2_denom):
|
|
var target_len_sqr = _distance_squared_between(bone_a, target)
|
|
var bone_a_len_sqr = bone_a_len * bone_a_len
|
|
var bone_b_len_sqr = bone_b_len * bone_b_len
|
|
|
|
cos_angle2 = (target_len_sqr - bone_a_len_sqr - bone_b_len_sqr) / cos_angle2_denom
|
|
cos_angle2 = clamp(cos_angle2, -1.0, 1.0);
|
|
|
|
_angle_b = acos(cos_angle2)
|
|
if flip_bend_direction:
|
|
_angle_b = -_angle_b
|
|
|
|
sin_angle2 = sin(_angle_b)
|
|
|
|
var tri_adjacent = bone_a_len + bone_b_len * cos_angle2
|
|
var tri_opposite = bone_b_len * sin_angle2
|
|
|
|
var xform_inv = bone_a.get_parent().global_transform.affine_inverse()
|
|
var target_pos = xform_inv * target.global_position - bone_a.position
|
|
|
|
var tan_y = target_pos.y * tri_adjacent - target_pos.x * tri_opposite
|
|
var tan_x = target_pos.x * tri_adjacent + target_pos.y * tri_opposite
|
|
_angle_a = atan2(tan_y, tan_x)
|
|
|
|
var bone_a_angle = bone_a.get_bone_angle()
|
|
var bone_b_angle = bone_b.get_bone_angle()
|
|
bone_a.rotation = _angle_a - bone_a_angle
|
|
bone_b.rotation = _angle_b - angle_difference(bone_a_angle, bone_b_angle)
|
|
|
|
|
|
func _distance_squared_between(node_a: Node2D, node_b: Node2D) -> float:
|
|
return node_a.global_position.distance_squared_to(node_b.global_position)
|