Inverse Kinematics

local chain = {};
local plane = {};

-- table sum function
function sum(t)
    local s = 0;
    for _, value in ipairs(t) do
        s = s + value;
    end;
    return s;
end;

local part = Instance.new("Part");
part.Material = Enum.Material.Plastic;
part.Anchored = true;
part.CanCollide = false;
part.BrickColor = BrickColor.Blue();

local parts = {};
local model = Instance.new("Model", game.Workspace);
function drawline(key, a, v)
    if not parts[key] then parts[key] = part:Clone(); parts[key].Parent = model; end;
    parts[key].Size = Vector3.new(.2, .2, v.magnitude);
    parts[key].CFrame = CFrame.new(a + v/2, a + v);
    return parts[key];
end;

function chain.new(joints, target)
    local self = setmetatable({}, {__index = chain});

    local lengths = {};
    for i = 1, #joints - 1 do
        lengths[i] = (joints[i] - joints[i+1]).magnitude;
    end;

    self.n = #joints;
    self.tolerance = 0.1;
    self.target = target;
    self.joints = joints;
    self.lengths = lengths;
    self.origin = CFrame.new(joints[1]);
    self.totallength = sum(lengths);

    -- rotation constraints
    self.constrained = false;
    self.left = math.rad(89);
    self.right = math.rad(89);
    self.up = math.rad(89);
    self.down = math.rad(89);

    return self;
end;

-- this is the hardest part of the code so I super commented it!
function chain:constrain(calc, line, cf)
    local scalar = calc:Dot(line) / line.magnitude;
    local proj = scalar * line.unit;

    -- get axis that are closest
    local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))};
    local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)),  cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))};
    table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);
    table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);

    local upvec = ups[1];
    local rightvec = rights[1];

    -- get the vector from the projection to the calculated vector
    local adjust = calc - proj;
    if scalar < 0 then
        -- if we're below the cone flip the projection vector
        proj = -proj;
    end;

    -- get the 2D components
    local xaspect = adjust:Dot(rightvec);
    local yaspect = adjust:Dot(upvec);

    -- get the cross section of the cone
    local left = -(proj.magnitude * math.tan(self.left));
    local right = proj.magnitude * math.tan(self.right);
    local up = proj.magnitude * math.tan(self.up);
    local down = -(proj.magnitude * math.tan(self.down));

    -- find the quadrant
    local xbound = xaspect >= 0 and right or left;
    local ybound = yaspect >= 0 and up or down;

    local f = calc;
    -- check if in 2D point lies in the ellipse
    local ellipse = xaspect^2/xbound^2 + yaspect^2/ybound^2;
    local inbounds = ellipse <= 1 and scalar >= 0;

    if not inbounds then
        -- get the angle of our out of ellipse point
        local a = math.atan2(yaspect, xaspect);
        -- find nearest point
        local x = xbound * math.cos(a);
        local y = ybound * math.sin(a);
        -- convert back to 3D
        f = (proj + rightvec * x + upvec * y).unit * calc.magnitude;
    end;

    -- return our final vector
    return f;
end;

function chain:backward()
    -- backward reaching; set end effector as target
    self.joints[self.n] = self.target;
    for i = self.n - 1, 1, -1 do
        local r = (self.joints[i+1] - self.joints[i]);
        local l = self.lengths[i] / r.magnitude;
        -- find new joint position
        local pos = (1 - l) * self.joints[i+1] + l * self.joints[i];
        self.joints[i] = pos;
    end;
end;

function chain:forward()
    -- forward reaching; set root at initial position
    self.joints[1] = self.origin.p;
    local coneVec = (self.joints[2] - self.joints[1]).unit;
    for i = 1, self.n - 1 do
        local r = (self.joints[i+1] - self.joints[i]);
        local l = self.lengths[i] / r.magnitude;
        -- setup matrix
        local cf = CFrame.new(self.joints[i], self.joints[i] + coneVec);
        -- find new joint position
        local pos = (1 - l) * self.joints[i] + l * self.joints[i+1];
        local t = self:constrain(pos - self.joints[i], coneVec, cf);
        self.joints[i+1] = self.constrained and self.joints[i] + t or pos;
        coneVec = self.joints[i+1] - self.joints[i];
    end;
end;

function chain:solve()
    local distance = (self.joints[1] - self.target).magnitude;
    if distance > self.totallength then
        -- target is out of reach
        for i = 1, self.n - 1 do
            local r = (self.target - self.joints[i]).magnitude;
            local l = self.lengths[i] / r;
            -- find new joint position
            self.joints[i+1] = (1 - l) * self.joints[i] + l * self.target;
        end;
    else
        -- target is in reach
        local bcount = 0;
        local dif = (self.joints[self.n] - self.target).magnitude;
        while dif > self.tolerance do
            self:backward();
            self:forward();
            dif = (self.joints[self.n] - self.target).magnitude;
            -- break if it's taking too long so the game doesn't freeze
            bcount = bcount + 1;
            if bcount > 5 then break; end;
        end;
    end;
end;

return chain;