path: root/quadtree.lua

local ok, bit = pcall(require, "bit")
if not ok then
    ok, bit = pcall(require, "bit32")
    if not ok then error("no bit library") end
end

local function clamp(x, a, b)
    if x < a then
        return a
    elseif x > b then
        return b
    else
        return x
    end
end

local function dot(x1, y1, x2, y2)
    return x1*x2 + y1*y2
end
local function sq_len(x, y)
    return x*x + y*y
end
local function sq_dist(x1, y1, x2, y2)
    return sq_len(x2-x1, y2-y1)
end

-- distance

local function closest_point_aabb(x, y, x1, y1, x2, y2)
    return clamp(x, x1, x2), clamp(y, y1, y2)
end
local function sq_dist_point_aabb(x, y, x1, y1, x2, y2)
    return sq_dist(x, y, closest_point_aabb(x, y, x1, y1, x2, y2))
end

local function closest_point_edge(x, y, x1, y1, x2, y2)
    local l = sq_dist(x1, y1, x2, y2)
    if l == 0 then
        return x1, y1
    end

    local t = clamp(dot(x-x1, y-y1, x2-x1, y2-y1) / l, 0, 1)
    return x1 + t * (x2 - x1), y1 + t * (y2 - y1)
end
local function sq_dist_point_edge(x, y, x1, y1, x2, y2)
    return sq_dist(x, y, closest_point_edge(x, y, x1, y1, x2, y2))
end

local function signed_triangle_area(x1, y1, x2, y2, x3, y3)
    return (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1)
end
local function point_edge_left(x, y, x1, y1, x2, y2)
    return signed_triangle_area(x1, y1, x2, y2, x, y) > 0
end

local function edge_intersect(ax, ay, bx, by, cx, cy, dx, dy)
    local oa = signed_triangle_area(cx, cy, dx, dy, ax, ay)
    local ob = signed_triangle_area(cx, cy, dx, dy, bx, by)
    local oc = signed_triangle_area(ax, ay, bx, by, cx, cy)
    local od = signed_triangle_area(ax, ay, bx, by, dx, dy)
    return oa*ob < 0 and oc*od < 0
end

local function point_in_triangle(x, y, x1, y1, x2, y2, x3, y3)
    return
        not point_edge_left(x, y, x1, y1, x2, y2) and
        not point_edge_left(x, y, x2, y2, x3, y3) and
        not point_edge_left(x, y, x3, y3, x1, y1)
end

local function signed_poly_area(poly)
    local area = 0
    for i, a in ipairs(poly) do
        local b = poly[i % #poly + 1]
        area = area + (b.x - a.x) * (b.y - a.y)
    end
    return area
end

local function point_in_poly(x, y, poly)
    for i, a in ipairs(poly) do
        local b = poly[i % #poly + 1]
        if not point_edge_left(x, y, a.x, a.y, b.x, b.y) then
            return false
        end
    end
    return true
end

local function dist_point_poly_lt(sq_v, poly, x, y)
    for i, a in ipairs(poly) do
        local b = poly[i % #poly + 1]
        if sq_dist_point_edge(x, y, a.x, a.y, b.x, b.y) < sq_v then
            return true
        end
    end
    return point_in_poly(x, y, poly)
end

-- triangulate

local function orient_poly(poly)
    if signed_poly_area(poly) > 0 then
        return poly
    end
    local rev = {}
    for i = #poly, 1, -1 do
        table.insert(rev, poly[i])
    end
    return rev
end

local function triangulate(poly)
    --[[
    poly = orient_poly(poly)
    local tri = {}
    local i, stop = 1, 1
    while #poly > 3 do
        local a, b, c = poly[i], poly[i % #poly + 1], poly[(i+1) % #poly + 1]
        local is_ear
        if not point_edge_left(c.x, c.y, a.x, a.y, b.x, b.y) then
            is_ear = true
            for j = 3, #poly-1 do
                local pa, pb, pc = poly[(i+j-2)%#poly+1], poly[(i+j-1)%#poly+1], poly[(i+j)%#poly+1]
                -- point edge left check???
                if point_in_triangle(pb.x, pb.y, a.x, a.y, b.x, b.y, c.x, c.y) and
                    point_edge_left(pa.x, pa.y, pb.x, pb.y, pc.x, pc.y) then
                    is_ear = false
                    break
                end
            end
        end
        if is_ear then
            table.insert(tri, { a, b, c })
            table.remove(poly, i % #poly + 1)
            i = i % #poly + 1
            stop = i
        else
            i = i % #poly + 1
            if i == stop then
                error("triangulation fail")
            end
        end
    end
    table.insert(tri, poly)
    return tri]]
    local verts = {}
    for _, p in ipairs(poly) do
        table.insert(verts, p.x)
        table.insert(verts, p.y)
    end
    local tris = {}
    for _, t in ipairs(love.math.triangulate(verts)) do
        table.insert(tris, orient_poly({
            { x = t[1], y = t[2] },
            { x = t[3], y = t[4] },
            { x = t[5], y = t[6] },
        }))
    end
    return tris
end

local function poly_simple(poly)
    for i, a in ipairs(orient_poly(poly)) do
        local b = poly[i+1]
        for j = i+2, #poly do
            local c = poly[j]
            local d = poly[j % #poly + 1]
            if edge_intersect(a.x, a.y, b.x, b.y, c.x, c.y, d.x, d.y) then
                return false
            end
        end
    end
    return true
end

-- intersect

local function project_aabb(x, y, x1, y1, x2, y2)
    local d1 = dot(x, y, x1, y1)
    local d2 = dot(x, y, x1, y2)
    local d3 = dot(x, y, x2, y1)
    local d4 = dot(x, y, x2, y2)
    return math.min(d1, d2, d3, d4), math.max(d1, d2, d3, d4)
end

local function project_poly(x, y, poly)
    local p1, p2
    for _, p in ipairs(poly) do
        local d = dot(x, y, p.x, p.y)
        if not p1 then
            p1 = d
            p2 = d
        else
            p1 = math.min(p1, d)
            p2 = math.max(p2, d)
        end
    end
    return p1, p2
end

local function collect_axes(poly)
    local axes = { { x = 0, y = 1 }, { x = 1, y = 0 } }
    for i, a in ipairs(poly) do
        local b = poly[i % #poly + 1]
        local x, y = (a.y - b.y), -(a.x - b.x)
        local l = math.sqrt(sq_len(x, y))
        table.insert(axes, { x = x/l, y = y/l })
    end
    for _, ax in ipairs(axes) do
        ax.p1, ax.p2 = project_poly(ax.x, ax.y, poly)
    end
    return axes
end

local function overlap(a1, a2, b1, b2)
    return a2 >= b1 and b2 >= a1
end

local function intersect_aabb_poly(x1, y1, x2, y2, axes)
    for _, ax in ipairs(axes) do
        if not overlap(ax.p1, ax.p2, project_aabb(ax.x, ax.y, x1, y1, x2, y2)) then
            return false
        end
    end
    return true
end

local debug = {}

local function to_float(map, x, y)
    return x*map.width/0x8000, y*map.height/0x8000
end

local function cached_dist_point_poly_lt(map, query, x, y)
    local key = bit.bor(x, bit.lshift(y, 16))
    if query.cache[key] == nil then
        query.cache[key] = dist_point_poly_lt(query.sq_dist, query.poly, to_float(map, x, y))
    end
    return query.cache[key]
end

local function classify(map, query, g, x, y)
    local p1 = cached_dist_point_poly_lt(map, query, x,   y)
    local p2 = cached_dist_point_poly_lt(map, query, x,   y+g)
    local p3 = cached_dist_point_poly_lt(map, query, x+g, y)
    local p4 = cached_dist_point_poly_lt(map, query, x+g, y+g)

    -- all corners are too close to wall (<=> the entire square is)
    if p1 and p2 and p3 and p4 then
        table.insert(debug, { msg = "full cover", p1 = { to_float(map, x, y) }, p2 = { to_float(map, x+g, y+g) }, bits = { x, y }, g = g })
        return true
    end

    -- at least one corner is too close to wall
    if p1 or p2 or p3 or p4 then
        table.insert(debug, { msg = "close to corner", p1 = { to_float(map, x, y) }, p2 = { to_float(map, x+g, y+g) }, bits = { x, y }, g = g })
        return
    end

    local x1, y1 = to_float(map, x, y)
    local x2, y2 = to_float(map, x+g, y+g)

    -- cell intersects wall
    if intersect_aabb_poly(x1, y1, x2, y2, query.axes) then
        table.insert(debug, { msg = "intersect", p1 = { to_float(map, x, y) }, p2 = { to_float(map, x+g, y+g) }, bits = { x, y }, g = g })
        return
    end

    -- wall is too close to an edge of the cell
    for _, p in ipairs(query.poly) do
        if sq_dist_point_aabb(p.x, p.y, x1, y1, x2, y2) < query.sq_dist then
            table.insert(debug, { msg = "close to edge", p1 = { to_float(map, x, y) }, p2 = { to_float(map, x+g, y+g) }, bits = { x, y }, g = g })
            return
        end
    end

    table.insert(debug, { msg = "free", p1 = { to_float(map, x, y) }, p2 = { to_float(map, x+g, y+g) }, bits = { x, y }, g = g })
    return false
end

local function node_insert(map, query, node, g, x, y)
    if node == true then
        return node
    end

    local class = classify(map, query, g, x, y)
    if class == false then
        return node
    elseif class == true then
        return true
    end

    g = bit.rshift(g, 1)
    if g < map.limit then
        return true
    end

    local node = node or { false, false, false, false }
    node[1] = node_insert(map, query, node[1], g, x,   y)
    node[2] = node_insert(map, query, node[2], g, x,   y+g)
    node[3] = node_insert(map, query, node[3], g, x+g, y)
    node[4] = node_insert(map, query, node[4], g, x+g, y+g)
    if node[1] == true and node[2] == true and node[3] == true and node[4] == true then
        return true
    end
    return node
end

local function node_merge(a, b)
    if a == true or b == true then
        return true
    end
    if a == false then
        return b
    end
    if b == false then
        return a
    end
    local x = {}
    for i = 1, 4 do
        x[i] = merge(a[i], b[i])
    end
    if x[1] == true and x[2] == true and x[3] == true and x[4] == true then
        return true
    end
    return x
end

local function insert(map, poly, dist)
    map.root = node_insert(map, { cache = {}, poly = poly, axes = collect_axes(poly), sq_dist = dist*dist }, map.root, 0x8000, 0, 0)
end

local function new(width, height, precision)
    return {
        width = width,
        height = height,
        limit = bit.lshift(1, math.max(0, 15-math.ceil(math.log(math.max(width, height)/precision, 2)))),
        root = false,
    }
end

return {
    new = new,
    insert = insert,
    triangulate = triangulate,
    poly_simple = poly_simple,
    debug = debug,
}