fake 3 joint ik second obf

tool
extends Node2D

onready var joint0 = $Joint0
onready var joint1 = $Joint0/Joint1
onready var hand = $Joint0/Joint1/Hand

var len_upper = 0
var len_middle = 0
var len_lower = 0

func _ready():
	update_limb()

func update_limb():
	joint0 = $Joint0
	joint1 = $Joint0/Joint1
	hand = $Joint0/Joint1/Hand
	len_upper = joint0.position.x
	len_middle = joint1.position.x
	len_lower = hand.position.x

func _process(delta):
	if Engine.is_editor_hint():
		update_limb()

	var t_pos = get_global_mouse_position()
	var diff = t_pos - global_position
	var dis_to_tar = diff.length()
	var base_r = atan2(diff.y, diff.x)

	var total_dist = len_upper + len_middle + len_lower

	var dummy_side_len = (len_lower + len_middle) * clamp(dis_to_tar / total_dist, 0, 1.0)

	var base_angles = SSS_calc(len_upper, dummy_side_len, dis_to_tar)
	var next_angles = SSS_calc(len_middle, len_lower, dummy_side_len)
	#print(base_angles)
	global_rotation = base_angles[0] + base_r
	joint0.rotation = base_angles[1] + next_angles[0]
	joint1.rotation = next_angles[1]
	update()

func SSS_calc(side_a, side_b, side_c):

	if side_c >= side_a + side_b:
		return [0, 0, 0]

	var angle_a = law_of_cos(side_c, side_a, side_b)
	var angle_b = (law_of_cos(side_a, side_b, side_c) + PI)
	var angle_c = 2 * PI - angle_a - angle_b

	return [angle_a, angle_b, angle_c]


func law_of_cos(a, b, c):
	return acos( (a * a + b * b - c * c) / ( 2 * a * b) )

func _draw():
	var color = Color(1, 1, 1)
	var joint_size = 4
	var line_size = 3
	var f_pos = Vector2(0, 0)
	var s_pos = joint0.position
	var e_pos = to_local(joint1.global_position)
	var h_pos = to_local(hand.global_position)
	draw_circle(f_pos, joint_size, color)
	draw_circle(s_pos, joint_size, color)
	draw_circle(e_pos, joint_size, color)
	draw_circle(h_pos, joint_size, color)
	draw_line(f_pos, s_pos, color, line_size)
	draw_line(s_pos, e_pos, color, line_size)
	draw_line(h_pos, e_pos, color, line_size)