leg ik

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 param = 0.2
	var dummy_side_len = (len_upper + len_middle) * clamp(dis_to_tar / total_dist, 0, 1.0)
	var max_angle = deg2rad(90)

	var base_angles = SSS_calc(dummy_side_len, len_lower, dis_to_tar)
	var next_angles = SSS_calc(len_upper, len_middle, dummy_side_len)

	#if base_angles[0] > max_angle:
	#	return

	global_rotation = base_angles[0] + next_angles[0] + base_r
	joint0.rotation = next_angles[1]
	joint1.rotation = base_angles[1] + next_angles[2]

	"""
	var angles1 = SSS_calc(len_upper + len_middle, len_lower, dis_to_tar)
	var angles2 = SSS_calc(len_upper, len_middle + len_lower, dis_to_tar)
	global_rotation = base_r + angles1[0] * param + angles2[0] * param
	joint0.rotation = PI - angles2[1] * param
	joint1.rotation = PI - angles1[1] * param
	"""


	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)