Number rounding error failing previously working inverse kinematics solver

So I have attempted to make my FABRIK function more robust by using storing the chain data in a table and using a For Loop to automatically calculate the chain (For future use if I wanted to make an octopus or something).

This has backfired greatly. The Rig’s feet no longer reach the goal being dragged as seen in the below gifs.
(Note: Every heartbeat frame the method iterates so I believe its not a amount of time iterations issue)

Old Manual Method Working (The Objective)

NewTouroIK

The newer method leg not reaching the goal (The Problem)

RobustMethodFailing

So I have resorted to print debugging with a set of predefined vectors and I find out the new vectors outputted while similar are not exactly the same.

Old manual code
local v1 =Vector3.new(1,0,0)
local v2 =Vector3.new(0,1,0)
local v3 =Vector3.new(0,0,1)
local targetPos = Vector3.new(5,5,5)
local originCF = CFrame.new()

local function backwards(originCF, targetPos, v1, v2, v3)
	local pointThree = originCF.p + v1 + v2

	--Construct the v3 into the new form closer to the goal
	--vecNew is temporary storage for the new direction vector
	--direction is targetPos to pointthree
	local vecNew = pointThree - targetPos

	local v3New = vecNew.Unit * v3.Magnitude

	--reconstruct chain from targetPos
	local vecFromPos = targetPos + v3New

	local pointTwo = originCF.p + v1

	--Construct the v2 with new direction
		vecNew = pointTwo - vecFromPos

	local v2New = vecNew.Unit * v2.Magnitude

	--Rereconstruct the chain
	local vecTwoFromPos = vecFromPos + v2New

	vecNew = originCF.p - vecTwoFromPos

	local v1New = vecNew.Unit * v1.Magnitude

	return originCF, targetPos, v1New, v2New, v3New
	--new  vector chain goes from goal towards the start position
end

_,_,v1,v2,v3 = backwards(originCF,targetPos,v1,v2,v3)
print("Old manual method:")
print("new v1: ",v1)
print("new v2: ",v2)
print("new v3: ",v3)
New Iterative method
local v1 =Vector3.new(1,0,0)
local v2 =Vector3.new(0,1,0)
local v3 =Vector3.new(0,0,1)
local targetPos = Vector3.new(5,5,5)
local originCF = CFrame.new()
local limbVectable = {v1,v2,v3}

local function Backwards(originCF, targetPos, limbVecTable)
	for i = #limbVecTable, 1,-1 do
		
		--initialize zero vector
		local vecSum = Vector3.new(0,0,0)

		for v = 1, i-1, 1 do
			vecSum = vecSum + limbVecTable[v]
		end
		
		--Gets the new direction of the new vector along the chain
		--direction is Target Pos to the next point on the chain
		local pointTo = originCF.Position+vecSum-targetPos
		
		--constructs the new vector Table
		limbVecTable[i] = pointTo.Unit*limbVecTable[i].Magnitude
			
	end
	return originCF, targetPos, limbVecTable
end

_,_,limbVectable = Backwards(originCF,targetPos,limbVectable)

print("New Iterative method:   ")
for i=1,#limbVectable do
	
	print("vectorLimb: ",i," Vectable: ",limbVectable[i])

end
Output in terminal
 New Iterative method:   
  vectorLimb:  1  Vectable:  -0.577350199, -0.577350199, -0.577350199
  vectorLimb:  2  Vectable:  -0.492365956, -0.615457416, -0.615457416
  vectorLimb:  3  Vectable:  -0.529812932, -0.529812932, -0.662266135
  Old manual method:
  new v1:  -0.59683609, -0.575823307, -0.558761418
  new v2:  -0.486684084, -0.62693131, -0.608355045
  new v3:  -0.529812932, -0.529812932, -0.662266135

Now I suspect the rounding caused here must have caused the issue and now I’m mentally stuck. Any ideas why and how to resolve it, please?

1 Like

NVM I forgot to move the target position along the chain as it solved in the new iterative method. Solved by further print debugging.

  new algorithm v 3 Direction:  -4, -4, -5
  new algorithm v 2 Direction:  -4, -5, -5
  new algorithm v 1 Direction:  -5, -5, -5
The correct version:
  v3 direction -4, -4, -5
  v2 direction -3.47018719, -4.47018719, -4.33773375
  v1 direction -3.9835031, -3.843256, -3.7293787

I was missing this in the new iteration

local function Backwards(originCF, targetPos, limbVecTable,limbLengthTable)

       -- I was missing this one store variable which moves the target position along the chain
	local store = Vector3.new()

	for i = #limbVecTable, 1,-1 do

		local vecSum = Vector3.new(0,0,0)
		--print("Index: ",i," Vectable: ",limbVectable[i])

		for v = 1, i-1, 1 do
			vecSum = vecSum + limbVecTable[v]
		end
		--print("vec sum: ",vecSum)
		
		--Gets the new direction of the new vector along the chain
		--direction is Target Pos to the next point on the chain
		local pointTo = originCF.Position+vecSum-targetPos-store
	--	print(pointTo)
		--constructs the new vectable
		limbVecTable[i] = pointTo.Unit*limbLengthTable[i]
		store = store + limbVecTable[i]
	end
	return originCF, targetPos, limbVecTable,limbLengthTable
end
1 Like