CC verlet intergration

--Created by Bloof--
--WIP--

function getMag(self)
  return math.sqrt(self.x^2 + self.y^2)
end

local monitor = peripheral.wrap("right")
local wid,hei = monitor.getSize()
local canBreak = false
local numberOfPoints = tonumber(read())
local points = {{["pos"] = vector.new(4,2,0), ["vel"] = vector.new(1,0,0), ["lastPos"] = vector.new(2,2,0), ["acc"] = vector.new(0,0,0), ["bounce"] = 1, ["anc"] = true}}
local constraints = {}

for i = 1, numberOfPoints do
    table.insert(points, {["pos"] = vector.new(4+i,1,0), ["vel"] = vector.new(0,0,0), ["lastPos"] = vector.new(4+i,1,0), ["acc"] = vector.new(0,1.5,0), ["bounce"] = 5, ["anc"] = false})
end

--table.insert(points, {["pos"] = vector.new(4 + numberOfPoints,2,0), ["vel"] = vector.new(1,0,0), ["lastPos"] = vector.new(4  + numberOfPoints,2,0), ["acc"] = vector.new(0,0,0), ["bounce"] = 1, ["anc"] = true})

for i = 1, numberOfPoints+1 do
    if i <= numberOfPoints then
        table.insert(constraints, {["p1"] = points[i], ["p2"] = points[i + 1], ["dist"] = 3})
    else
        table.insert(constraints, {["p1"] = points[i], ["p2"] = points[i - 1], ["dist"] = 3})
    end
end


function solveConstraint(cons)
    if cons["p1"] and cons["p2"] then
        local p1 = cons["p1"]
        local p2 = cons["p2"]
        local diff = (p1["pos"]-p2["pos"]) or 0
        local dist = getMag(diff) or 0
        local scal = (cons["dist"] - dist) / dist or 0
        local trans = diff * 0.5 * scal

        --print(diff, dist, scal, trans)

        if not p1["anc"] then
            p1["pos"] = p1["pos"] + trans
            --print(p1["pos"])
        end

        if not p2["anc"] then
            p2["pos"] = p2["pos"] - trans
            --print(p2["pos"])
        end

    end;
end;

--[[for ind,obj in pairs(points) do
    obj["vel"] = vector.new(math.random(-2,2),math.random(-2,2),0)
end]]

while true do
    sleep(0.25)
    monitor.clear()
    for ind, obj in pairs(points) do
        obj["vel"] = (obj["pos"] - obj["lastPos"])

        --prevent from going into wall x0
        if obj["pos"].x < 0 then
            obj["pos"].x = 0
            if obj["vel"].x < 0 then
                obj["vel"].x = (obj["vel"].x * -1) / obj["bounce"]
            end
        end

        --prevent from going into wall xW
        if obj["pos"].x > wid then
            obj["pos"].x = wid
            if obj["vel"].x > 0 then
                obj["vel"].x = (obj["vel"].x * -1) / obj["bounce"]
            end
        end

        --prevent from going into wall y0
        if obj["pos"].y < 0 then
            obj["pos"].y = 0
            if obj["vel"].y < 0 then
                obj["vel"].y = (obj["vel"].y * -1) / obj["bounce"]
            end
        end

        --prevent from going into wall yH
        if obj["pos"].y > hei then
            obj["pos"].y = hei
            if obj["vel"].y > 0 then
                obj["vel"].y = (obj["vel"].y * -1) / obj["bounce"]
            end
        end

        local nextPos = obj["pos"] + obj["vel"] * 1 + obj["acc"]
        obj["lastPos"] = obj["pos"]
        obj["pos"] = nextPos
    end

    for ind, cons in pairs(constraints) do
        if ind <= numberOfPoints then
            --print("test")
            for i = 1, 100 do
                --print(obj["pos"], obj["vel"])
                solveConstraint(constraints[ind])
            end
        end
    end

    for ind, obj in pairs(points) do
        monitor.setCursorPos(obj["pos"].x, obj["pos"].y)
        monitor.write("#")
    end
end