summaryrefslogtreecommitdiff
path: root/quadtree.lua
diff options
context:
space:
mode:
Diffstat (limited to 'quadtree.lua')
-rw-r--r--quadtree.lua349
1 files changed, 349 insertions, 0 deletions
diff --git a/quadtree.lua b/quadtree.lua
new file mode 100644
index 0000000..01c6018
--- /dev/null
+++ b/quadtree.lua
@@ -0,0 +1,349 @@
+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,
+}
generated by cgit v1.2.3 (git 2.39.1) at 2026-06-23 22:09:24 +0000