batteries/intersect.lua

542 lines
16 KiB
Lua

--[[
geometric intersection routines
from simple point tests to shape vs shape tests
optimised pretty well in most places
tests provided:
overlap
boolean "is overlapping"
collide
nil for no collision
minimum separating vector on collision
provided in the direction of the first object
optional output parameters to avoid garbage generation
]]
local path = (...):gsub("intersect", "")
local vec2 = require(path .. "vec2")
--module storage
local intersect = {}
--epsilon for collisions
local COLLIDE_EPS = 1e-6
------------------------------------------------------------------------------
-- circles
function intersect.circle_point_overlap(pos, rad, v)
return pos:distance_squared(v) < rad * rad
end
function intersect.circle_circle_overlap(a_pos, a_rad, b_pos, b_rad)
local rad = a_rad + b_rad
return a_pos:distance_squared(b_pos) < rad * rad
end
function intersect.circle_circle_collide(a_pos, a_rad, b_pos, b_rad, into)
--get delta
local delta = a_pos
:pooled_copy()
:vector_sub_inplace(b_pos)
--squared threshold
local rad = a_rad + b_rad
local dist = delta:length_squared()
local res = false
if dist < rad * rad then
if dist == 0 then
--singular case; just resolve vertically
dist = 1
delta:scalar_set(0, 1)
else
--get actual distance
dist = math.sqrt(dist)
end
--allocate if needed
if into == nil then
into = vec2(0)
end
--normalise, scale to separating distance
res = into:set(delta)
:scalar_div_inplace(dist)
:scalar_mul_inplace(rad - dist)
end
delta:release()
return res
end
function intersect.circle_point_collide(a_pos, a_rad, b, into)
return intersect.circle_circle_collide(a_pos, a_rad, b, 0, into)
end
------------------------------------------------------------------------------
-- line segments
-- todo: separate double-sided, one-sided, and pull-through (along normal) collisions?
--get the nearest point on the line segment a from point b
function intersect.nearest_point_on_line(a_start, a_end, b_pos, into)
if into == nil then into = vec2(0) end
--direction of segment
local segment = a_end:pooled_copy()
:vector_sub_inplace(a_start)
--detect degenerate case
local lensq = segment:length_squared()
if lensq <= COLLIDE_EPS then
into:set(a_start)
else
--solve for factor along segment
local point_to_start = b_pos:pooled_copy()
:vector_sub_inplace(a_start)
local factor = math.clamp01(point_to_start:dot(segment) / lensq)
point_to_start:release()
into:set(segment)
:scalar_mul_inplace(factor)
:vector_add_inplace(a_start)
end
segment:release()
return into
end
--internal
--vector from line seg origin to point
function intersect._line_to_point(a_start, a_end, b_pos, into)
return intersect.nearest_point_on_line(a_start, a_end, b_pos, into)
:vector_sub_inplace(b_pos)
end
--internal
--line displacement vector from separation vector
function intersect._line_displacement_to_sep(a_start, a_end, separation, total_rad)
local distance = separation:normalise_len_inplace()
local sep = distance - total_rad
if sep <= 0 then
if distance <= COLLIDE_EPS then
--point intersecting the line; push out along normal
separation:set(a_end)
:vector_sub_inplace(a_start)
:normalise_inplace()
:rot90l_inplace()
else
separation:scalar_mul_inplace(-sep)
end
return separation
end
return false
end
--overlap a line segment with a circle
function intersect.line_circle_overlap(a_start, a_end, a_rad, b_pos, b_rad)
local nearest = intersect.nearest_point_on_line(a_start, a_end, b_pos, vec2:pooled())
local overlapped = intersect.circle_point_overlap(b_pos, a_rad + b_rad, nearest)
nearest:release()
return overlapped
end
--collide a line segment with a circle
function intersect.line_circle_collide(a_start, a_end, a_rad, b_pos, b_rad, into)
into = intersect._line_to_point(a_start, a_end, b_pos, into)
return intersect._line_displacement_to_sep(a_start, a_end, into, a_rad + b_rad)
end
--collide 2 line segments
function intersect.line_line_collide(a_start, a_end, a_rad, b_start, b_end, b_rad, into)
--segment directions from start points
local a_dir = a_end
:pooled_copy()
:vector_sub_inplace(a_start)
local b_dir = b_end
:pooled_copy()
:vector_sub_inplace(b_start)
--detect degenerate cases
local a_degen = a_dir:length_squared() <= COLLIDE_EPS
local b_degen = a_dir:length_squared() <= COLLIDE_EPS
if a_degen or b_degen then
vec2.release(a_dir, b_dir)
if a_degen and b_degen then
--actually just circles
return intersect.circle_circle_collide(a_start, a_rad, b_start, b_rad, into)
elseif a_degen then
--a is just circle
return intersect.circle_line_collide(a_start, a_rad, b_start, b_end, b_rad, into)
elseif b_degen then
--b is just circle
return intersect.line_circle_collide(a_start, a_end, a_rad, b_start, b_rad, into)
end
end
--otherwise we're _actually_ 2 line segs :)
if into == nil then into = vec2(0) end
--first, check intersection
--(c to lua translation of paul bourke's
-- line intersection algorithm)
local dx1 = (a_end.x - a_start.x)
local dx2 = (b_end.x - b_start.x)
local dy1 = (a_end.y - a_start.y)
local dy2 = (b_end.y - b_start.y)
local dxab = (a_start.x - b_start.x)
local dyab = (a_start.y - b_start.y)
local denom = dy2 * dx1 - dx2 * dy1
local numera = dx2 * dyab - dy2 * dxab
local numerb = dx1 * dyab - dy1 * dxab
--check coincident lines
local intersected = "none"
if
math.abs(numera) < COLLIDE_EPS and
math.abs(numerb) < COLLIDE_EPS and
math.abs(denom) < COLLIDE_EPS
then
intersected = "both"
else
--check parallel, non-coincident lines
if math.abs(denom) < COLLIDE_EPS then
intersected = "none"
else
--get interpolants along segments
local mua = numera / denom
local mub = numerb / denom
--intersection outside segment bounds?
local outside_a = mua < 0 or mua > 1
local outside_b = mub < 0 or mub > 1
if outside_a and outside_b then
intersected = "none"
elseif outside_a then
intersected = "b"
elseif outside_b then
intersected = "a"
else
intersected = "both"
end
end
end
if intersected == "both" then
--simply displace along A normal
into:set(a_dir)
vec2.release(a_dir, b_dir)
return into
:normalise_inplace()
:scalar_mul_inplace(a_rad + b_rad)
:rot90l_inplace()
end
vec2.release(a_dir, b_dir)
--dumb as a rock check-corners approach
--todo: pool storage
--todo proper calculus from http://geomalgorithms.com/a07-_distance.html
local search_tab = {}
--only insert corners from the non-intersected line
--since intersected line is potentially the apex
if intersected ~= "a" then
--a endpoints
table.insert(search_tab, {intersect._line_to_point(b_start, b_end, a_start), 1})
table.insert(search_tab, {intersect._line_to_point(b_start, b_end, a_end), 1})
end
if intersected ~= "b" then
--b endpoints
table.insert(search_tab, {intersect._line_to_point(a_start, a_end, b_start), -1})
table.insert(search_tab, {intersect._line_to_point(a_start, a_end, b_end), -1})
end
local best = nil
local best_len = nil
for _, v in ipairs(search_tab) do
local len = v[1]:length_squared()
if not best_len or len < best_len then
best = v
end
end
--fix direction
into:set(best[1])
:scalar_mul_inplace(best[2])
return intersect._line_displacement_to_sep(a_start, a_end, into, a_rad + b_rad)
end
------------------------------------------------------------------------------
-- axis aligned bounding boxes
--return true on overlap, false otherwise
function intersect.aabb_point_overlap(pos, hs, v)
local delta = pos
:pooled_copy()
:vector_sub_inplace(v)
:abs_inplace()
local overlap = delta.x < hs.x and delta.y < hs.y
delta:release()
return overlap
end
-- discrete displacement
-- return msv to push point to closest edge of aabb
function intersect.aabb_point_collide(pos, hs, v, into)
--separation between centres
local delta_c = v
:pooled_copy()
:vector_sub_inplace(pos)
--absolute separation
local delta_c_abs = delta_c
:pooled_copy()
:abs_inplace()
local res = false
if delta_c_abs.x < hs.x and delta_c_abs.y < hs.y then
res = (into or vec2(0))
--separating offset in both directions
:set(hs)
:vector_sub_inplace(delta_c_abs)
--minimum separating distance
:minor_inplace()
--in the right direction
:vector_mul_inplace(delta_c:sign_inplace())
--from the aabb's point of view
:inverse_inplace()
end
vec2.release(delta_c, delta_c_abs)
return res
end
--return true on overlap, false otherwise
function intersect.aabb_aabb_overlap(a_pos, a_hs, b_pos, b_hs)
local delta = a_pos
:pooled_copy()
:vector_sub_inplace(b_pos)
:abs_inplace()
local total_size = a_hs
:pooled_copy()
:vector_add_inplace(b_hs)
local overlap = delta.x < total_size.x and delta.y < total_size.y
vec2.release(delta, total_size)
return overlap
end
--discrete displacement
--return msv on collision, false otherwise
function intersect.aabb_aabb_collide(a_pos, a_hs, b_pos, b_hs, into)
local delta = a_pos
:pooled_copy()
:vector_sub_inplace(b_pos)
local abs_delta = delta
:pooled_copy()
:abs_inplace()
local size = a_hs
:pooled_copy()
:vector_add_inplace(b_hs)
local abs_amount = size
:pooled_copy()
:vector_sub_inplace(abs_delta)
local res = false
if abs_amount.x > COLLIDE_EPS and abs_amount.y > COLLIDE_EPS then
if not into then into = vec2(0) end
--actually collided
if abs_amount.x <= abs_amount.y then
--x min
res = into:scalar_set(abs_amount.x * math.sign(delta.x), 0)
else
--y min
res = into:scalar_set(0, abs_amount.y * math.sign(delta.y))
end
end
return res
end
-- helper function to clamp point to aabb
function intersect.aabb_point_clamp(pos, hs, v, into)
local v_min = pos
:pooled_copy()
:vector_sub_inplace(hs)
local v_max = pos
:pooled_copy()
:vector_add_inplace(hs)
into = into or vec2(0)
into:set(v)
:clamp_inplace(v_min, v_max)
vec2.release(v_min, v_max)
return into
end
-- return true on overlap, false otherwise
function intersect.aabb_circle_overlap(a_pos, a_hs, b_pos, b_rad)
local clamped = intersect.aabb_point_clamp(a_pos, a_hs, b_pos, vec2:pooled())
local edge_distance_squared = clamped:distance_squared(b_pos)
clamped:release()
return edge_distance_squared < (b_rad * b_rad)
end
-- return msv on collision, false otherwise
function intersect.aabb_circle_collide(a_pos, a_hs, b_pos, b_rad, into)
local abs_delta = a_pos
:pooled_copy()
:vector_sub_inplace(b_pos)
:abs_inplace()
--circle centre within aabb-like bounds, collide as an aabb
local like_aabb = abs_delta.x < a_hs.x or abs_delta.y < a_hs.y
--(clean up)
abs_delta:release()
--
local result
if like_aabb then
local pretend_hs = vec2:pooled(0, 0)
result = intersect.aabb_aabb_collide(a_pos, a_hs, b_pos, pretend_hs, into)
pretend_hs:release()
else
--outside aabb-like bounds so we need to collide with the nearest clamped corner point
local clamped = intersect.aabb_point_clamp(a_pos, a_hs, b_pos, vec2:pooled())
result = intersect.circle_circle_collide(clamped, 0, b_pos, b_rad, into)
clamped:release()
end
return result
end
--check if a point is in a polygon
--point is the point to test
--poly is a list of points in order
--based on winding number, so re-intersecting areas are counted as solid rather than inverting
function intersect.point_in_poly(point, poly)
local wn = 0
for i, a in ipairs(poly) do
local b = poly[i + 1] or poly[1]
if a.y <= point.y then
if
b.y > point.y
and vec2.winding_side(a, b, point) > 0
then
wn = wn + 1
end
else
if
b.y <= point.y
and vec2.winding_side(a, b, point) < 0
then
wn = wn - 1
end
end
end
return wn ~= 0
end
--reversed versions
--it's annoying to need to flip the order of operands depending on what
--shapes you're working with
--so these functions provide the
--todo: ensure this is all of them
--(helper for reversing only if there's actually a vector, preserving false)
function intersect.reverse_msv(result)
if result then
result:inverse_inplace()
end
return result
end
function intersect.point_circle_overlap(a, b_pos, b_rad)
return intersect.circle_point_overlap(b_pos, b_rad, a)
end
function intersect.point_circle_collide(a, b_pos, b_rad, into)
return intersect.reverse_msv(intersect.circle_circle_collide(b_pos, b_rad, a, 0, into))
end
function intersect.point_aabb_overlap(a, b_pos, b_hs)
return intersect.aabb_point_overlap(b_pos, b_hs, a)
end
function intersect.point_aabb_collide(a, b_pos, b_hs, into)
return intersect.reverse_msv(intersect.aabb_point_collide(b_pos, b_rad, a, into))
end
function intersect.circle_aabb_overlap(a, a_rad, b_pos, b_hs)
return intersect.aabb_circle_overlap(b_pos, b_rad, a, a_rad)
end
function intersect.circle_aabb_collide(a, a_rad, b_pos, b_hs, into)
return intersect.reverse_msv(intersect.aabb_circle_collide(b_pos, b_rad, a, a_rad, into))
end
function intersect.circle_line_collide(a, a_rad, b_start, b_end, b_rad)
return intersect.reverse_msv(intersect.line_circle_collide(b_start, b_end, b_rad, a, a_rad, into))
end
--resolution helpers
--resolve a collision between two bodies, given a (minimum) separating vector
-- from a's frame of reference, like the result of any of the _collide functions
--requires the two positions of the bodies, the msv, and a balance factor
--balance should be between 1 and 0;
-- 1 is only a_pos moving to resolve
-- 0 is only b_pos moving to resolve
-- 0.5 is balanced between both (default)
--note: this wont work as-is for line segments, which have two separate position coordinates
-- you will need to understand what is going on and move the both coordinates yourself
function intersect.resolve_msv(a_pos, b_pos, msv, balance)
balance = balance or 0.5
a_pos:fused_multiply_add_inplace(msv, balance)
b_pos:fused_multiply_add_inplace(msv, -(1 - balance))
end
-- gets a normalised balance factor from two mass inputs, and treats <=0 or infinite or nil masses as static bodies
-- returns false if we're colliding two static bodies, as that's invalid
function intersect.balance_from_mass(a_mass, b_mass)
--static cases
local a_static = a_mass <= 0 or a_mass == math.huge or not a_mass
local b_static = b_mass <= 0 or b_mass == math.huge or not a_mass
if a_static and b_static then
return false --colliding two static bodies
elseif a_static then
return 0.0
elseif b_static then
return 1.0
end
--get balance factor
local total = a_mass + b_mass
return a_mass / total
end
--bounce a velocity off of a normal (modifying velocity)
--essentially flips the part of the velocity in the direction of the normal
function intersect.bounce_off(velocity, normal, conservation)
--(default)
conservation = conservation or 1
--take a copy, we need it
local old_vel = velocity:pooled_copy()
--reject on the normal (keep velocity tangential to the normal)
velocity:vector_rejection_inplace(normal)
--add back the complement of the difference;
--basically "flip" the velocity in line with the normal.
velocity:fused_multiply_add_inplace(old_vel:vector_sub_inplace(velocity), -conservation)
--clean up
old_vel:release()
return velocity
end
--mutual bounce; two similar bodies bounce off each other, transferring energy
function intersect.mutual_bounce(velocity_a, velocity_b, normal, conservation)
--(default)
conservation = conservation or 1
--take copies, we need them
local old_a_vel = velocity_a:pooled_copy()
local old_b_vel = velocity_b:pooled_copy()
--reject on the normal
velocity_a:vector_rejection_inplace(normal)
velocity_b:vector_rejection_inplace(normal)
--calculate the amount remaining from the old velocity
--(transfer pool ownership)
local a_remaining = old_a_vel:vector_sub_inplace(velocity_a)
local b_remaining = old_b_vel:vector_sub_inplace(velocity_b)
--transfer it to the other body
velocity_a:fused_multiply_add_inplace(b_remaining, conservation)
velocity_b:fused_multiply_add_inplace(a_remaining, conservation)
--clean up
vec2.release(a_remaining, b_remaining)
end
return intersect