Adds a dual sphere colllision to a two bone chain, needs a target end effector and a target bend effector

ik_skeleton.gd

tool
extends Skeleton

var ik_root = find_bone("Thigh.Right")
var ik_root2= find_bone("Thigh.Left")

func _ready():
	set_process(true)

func _init():
	set_process(true)

func _process(delta):
	solve_2bone_ik(self,ik_root, $target.transform.origin, $bend.transform.origin)
	solve_2bone_ik(self,ik_root2, $target2.transform.origin, $bend2.transform.origin)

func solve_2bone_ik(skeleton, ik_root, target_t, bend_t, delta=0.5):
	var pre_solve = get_points(skeleton, ik_root)
	var solution  = get_solve_points_chain2(skeleton, ik_root, target_t, bend_t)
	zipper_solve(skeleton, ik_root, pre_solve, solution, delta)

func get_solve_points_chain2(skeleton, ik_root, target_t, bend_t):
	# Expects target_t and bend_t as vector3s in skeleton space
	
	var pre_solve_points = get_points(skeleton, ik_root)
	
	var bone1_len = (pre_solve_points[1] - pre_solve_points[0]).length()
	var bone2_len = (pre_solve_points[2] - pre_solve_points[1]).length()
	# solve the desired location
	var circle = sphere_sphere_intersection(pre_solve_points[0], bone1_len, target_t, bone2_len)
	if circle:
		var solve_point = point_on_circle(circle, bend_t)
		var quat1 = quat_between(pre_solve_points[1] - pre_solve_points[0], solve_point - pre_solve_points[0])
		var quat2 = quat_between(pre_solve_points[2] - pre_solve_points[1], target_t - solve_point)
		
		return [pre_solve_points[0], solve_point, target_t]
	else:
		var d = (target_t - pre_solve_points[0]).normalized()
		
		return [pre_solve_points[0], pre_solve_points[0] + d * bone1_len,  pre_solve_points[0] + d * (bone1_len+bone2_len)]

func zipper_solve(sk, root, old_points, new_points,delta=0.5, e=0.0001):
	for i in range(2):
		var old_bone = sk.get_bone_global_pose(root+i+1).origin - sk.get_bone_global_pose(root+i).origin
		var new_bone = new_points[i+1] - new_points[i]
		var theta = old_bone.angle_to(new_bone)
		if theta < e:
			continue
		old_bone = sk.get_bone_global_pose(root+i+1).origin - sk.get_bone_global_pose(root+i).origin
		var a = old_bone.cross(new_bone).normalized()
		
		var pose = sk.get_bone_global_pose(root+i)
		pose.basis = pose.rotated(a, theta).basis
		sk.set_bone_global_pose(root+i, pose)

func sphere_sphere_intersection(c1,r1,c2,r2):
	var d = c2 - c1
	#Edge cases
	if d.length() > r1+r2:
		# Sphere's are too far apart
		return false
	if d.length() == r1+r2:
		# spheres connected at one single point
		print("one point")
		return 
	if d.length() + min(r1,r2) < max(r1,r2):
		# one sphere is inside another
		return false
	
	# is something wrong with this?
	var h = 0.5 + ((r1*r1) - (r2 * r2))/(2 * (d.length()*d.length()))
	
	var c_i = c1 + h * d
	var r_i = sqrt(r1*r1 - h*h*d.length()*d.length())
	
	var n = d.normalized()
	
	var circle = [c_i, r_i, n]
	return circle

func point_on_circle(circle, v):
	if not circle:
		return
	var plane = Plane(circle[2].normalized(), 0)
	plane.distance_to(v + circle[0])
	var out = plane.project(v).normalized() * circle[1] + circle[0]
	return out

func quat_between(v1,v2):
	#return the quat with the shortest arc between v1 and v2
	
	var q = Quat()
	var a = v1.cross(v2)
	q.x = a.x
	q.y = a.y
	q.z = a.z
	q.w = sqrt(pow(v1.length(), 2) * pow(v2.length(), 2)) + v1.dot(v2)
	return q

func get_points(sk, id):
	var points = []
	points.append(sk.get_bone_global_pose(id).origin)
	points.append(sk.get_bone_global_pose(id+1).origin)
	points.append(sk.get_bone_global_pose(id+2).origin)
	return points