IKSolve.lua

-- Localized variables that I didn't copy over go here.

local function Solve(r0, r1, c, p)
	local x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22 = c:components()
	local px,py,pz = p.x-x,p.y-y,p.z-z
	local tx = r00*px+r10*py+r20*pz
	local ty = r01*px+r11*py+r21*pz
	local tz = r02*px+r12*py+r22*pz
	local d = (tx*tx+ty*ty+tz*tz)^0.5
	d = r0+r1<d and r0+r1 or d
	local l = (r1*r1-r0*r0-d*d)/(2*r0*d)
	local h = (1-l*l)^0.5
	local a = atan(h/l)
	local s = ((d-tz)*d*2)^0.5
	local qx = ty/s
	local qy = -tx/s
	local qw = (d-tz)/s

	local q0w,q0x,q0y,q0z
	local trace=r00+r11+r22
	if (trace>0) then
		local s = (1+trace)^0.5
		local r = 0.5/s
		q0w,q0x,q0y,q0z = s*0.5,(r21-r12)*r,(r02-r20)*r,(r10-r01)*r
	else
		local big = r00>r11 and r00>r22 and r00 or r11>r00 and r11>r22 and r11 or r22
		if (big == r00) then
			local s = (1+r00-r11-r22)^0.5
			local r = 0.5/s
			q0w,q0x,q0y,q0z = (r21-r12)*r,0.5*s,(r10+r01)*r,(r02+r20)*r
		elseif (big == r11) then
			local s = (1-r00+r11-r22)^0.5
			local r = 0.5/s
			q0w,q0x,q0y,q0z = (r02-r20)*r,(r10+r01)*r,0.5*s,(r21+r12)*r
		elseif big == r22 then
			local s = (1-r00-r11+r22)^0.5
			local r = 0.5/s
			q0w,q0x,q0y,q0z = (r10-r01)*r,(r02+r20)*r,(r21+r12)*r,0.5*s
		end
	end
	
	local ha = a*0.5
	local rw = cos(ha)
	local rx = sin(ha)
	
	local q1x = qw*rx+qx*rw
	local q1y = qy*rw
	local q1z = -qy*rx
	local q1w = qw*rw-qx*rx
	local j0 = cf(x,y,z,q0w*q1x+q0x*q1w+q0y*q1z-q0z*q1y,q0w*q1y-q0x*q1z+q0y*q1w+q0z*q1x,q0w*q1z+q0x*q1y-q0y*q1x+q0z*q1w,q0w*q1w-q0x*q1x-q0y*q1y-q0z*q1z)
	local j1 = j0*cf(0,0,-r0,-h,0,0,l)
	return j0,j1
end