batteries/intersect.lua

389 lines
11 KiB
Lua
Raw Normal View History

2020-01-29 03:26:28 +00:00
--[[
geometric intersection routines
from simple point tests to shape vs shape tests
options for boolean or minimum separating vector results
continuous sweeps (where provided) also return the
time-domain position of first intersection
TODO: refactor storage to be pooled rather than fully local
so these functions can be reentrant
]]
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
local _ccc_delta = vec2:zero()
function intersect.circle_circle_collide(a_pos, a_rad, b_pos, b_rad, into)
--get delta
_ccc_delta:vset(a_pos):vsubi(b_pos)
--squared threshold
local rad = a_rad + b_rad
local dist = _ccc_delta:length_squared()
if dist < rad * rad then
if dist == 0 then
--singular case; just resolve vertically
dist = 1
_ccc_delta:sset(0,1)
else
--get actual distance
dist = math.sqrt(dist)
end
--allocate if needed
if into == nil then
into = vec2:zero()
end
--normalise, scale to separating distance
into:vset(_ccc_delta):sdiv(dist):smuli(rad - dist)
return into
end
return false
end
------------------------------------------------------------------------------
-- line segments
-- todo: separate double-sided, one-sided, and pull-through (along normal) collisions?
--vector from line seg to point
function intersect._line_to_point(a_start, a_end, b_pos, into)
if into == nil then into = vec2:zero() end
--direction of line
into:vset(a_end):vsub(a_start)
--detect degenerate case
if into:length_squared() <= COLLIDE_EPS then
return intersect.circle_circle_collide(a_start, a_rad, b_pos, b_rad)
end
--solve for factor along line
local dx = (b_pos.x - a_start.x) * (a_end.x - a_start.x)
local dy = (b_pos.y - a_start.y) * (a_end.y - a_start.y)
local u = (dx + dy) / into:length_squared()
--clamp onto segment
u = math.clamp01(u)
--get the displacement to the nearest point (invalidate direction)
into:smuli(u):vaddi(a_start):vsubi(b_pos)
return into
end
--line displacement vector from separation vector
function intersect._line_displacement_to_sep(a_start, a_end, separation, total_rad)
local distance = separation:normalisei_len()
local sep = distance - total_rad
if sep <= 0 then
if distance <= COLLIDE_EPS then
--point intersecting the line; push out along normal
separation:vset(a_end):vsub(a_start):normalisei():rot90li()
else
separation:smuli(-sep)
end
return separation
end
return false
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:vsub(a_start)
local b_dir = b_end:vsub(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 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; annoying, need reversed msv
local collided = intersect.line_circle_collide(b_start, b_end, b_rad, a_start, a_rad, into)
if collided then
collided:smuli(-1)
end
return collided
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
--otherwise we're _actually_ 2 line segs :)
if into == nil then into = vec2:zero() 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"
--collision point =
--[[vec2:xy(
a_start.x + mua * dx1,
a_start.y + mua * dy1,
)]]
end
end
end
if intersected == "both" then
--simply displace along A normal
return into:vset(a_dir):normalisei():smuli(a_rad + b_rad):rot90li()
else
--dumb as a rock check-corners approach
--todo: pool storage
--todo 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 = table.find_best(search_tab, function(v)
return -(v[1]:length_squared())
end)
--fix direction
into:vset(best[1]):smuli(best[2])
return intersect._line_displacement_to_sep(a_start, a_end, into, a_rad + b_rad)
end
return false
end
------------------------------------------------------------------------------
-- axis aligned bounding boxes
--return true on overlap, false otherwise
local _apo_delta = vec2:zero()
function intersect.aabb_point_overlap(pos, hs, v)
_apo_delta:vset(pos):vsubi(v):absi()
return _apo_delta.x < hs.x and _apo_delta.y < hs.y
end
--return true on overlap, false otherwise
local _aao_abs_delta = vec2:zero()
local _aao_total_size = vec2:zero()
function intersect.aabb_aabb_overlap(pos, hs, opos, ohs)
_aao_abs_delta:vset(pos):vsubi(opos):absi()
_aao_total_size:vset(hs):vaddi(ohs)
return _aao_abs_delta.x < _aao_total_size.x and _aao_abs_delta.y < _aao_total_size.y
end
--discrete displacement
--return msv on collision, false otherwise
local _aac_delta = vec2:zero()
local _aac_abs_delta = vec2:zero()
local _aac_size = vec2:zero()
local _aac_abs_amount = vec2:zero()
function intersect.aabb_aabb_collide(apos, ahs, bpos, bhs, into)
if not into then into = vec2:zero() end
_aac_delta:vset(apos):vsubi(bpos)
_aac_abs_delta:vset(_aac_delta):absi()
_aac_size:vset(ahs):vaddi(bhs)
_aac_abs_amount:vset(_aac_size):vsubi(_aac_abs_delta)
if _aac_abs_amount.x > COLLIDE_EPS and _aac_abs_amount.y > COLLIDE_EPS then
--actually collided
if _aac_abs_amount.x <= _aac_abs_amount.y then
--x min
if _aac_delta.x < 0 then
return into:sset(-_aac_abs_amount.x, 0)
else
return into:sset(_aac_abs_amount.x, 0)
end
else
--y min
if _aac_delta.y < 0 then
return into:sset(0, -_aac_abs_amount.y)
else
return into:sset(0, _aac_abs_amount.y)
end
end
end
return false
end
--return normal and fraction of dt encountered on collision, false otherwise
--TODO: re-pool storage here
function intersect.aabb_aabb_collide_continuous(
a_startpos, a_endpos, ahs,
b_startpos, b_endpos, bhs,
into
)
if not into then into = vec2:zero() end
--compute delta motion
local _self_delta_motion = a_endpos:vsub(a_startpos)
local _other_delta_motion = b_endpos:vsub(b_startpos)
--cheap "is this even possible" early-out
do
local _self_half_delta = _self_delta_motion:smul(0.5)
local _self_bounds_pos = _self_half_delta:vadd(a_endpos)
local _self_bounds_hs = _self_half_delta:vadd(ahs)
local _other_half_delta = _other_delta_motion:smul(0.5)
local _other_bounds_pos = _other_half_delta:vadd(b_endpos)
local _other_bounds_hs = _other_half_delta:vadd(bhs)
if not body._overlap_raw(
_self_bounds_pos, _self_bounds_hs,
_other_bounds_pos, _other_bounds_hs
) then
return false
end
end
--get ccd minkowski box
--this is a relative-space box
local _relative_delta_motion = _self_delta_motion:vsub(_other_delta_motion)
local _minkowski_halfsize = ahs:vadd(bhs)
local _minkowski_pos = b_startpos:vsub(a_startpos)
--if a line seg from our relative motion hits the minkowski box, we're in luck
--slab raycast is speedy
--alias
local _rmx = _relative_delta_motion.x
local _rmy = _relative_delta_motion.y
local _inv_x = math.huge
if _rmx ~= 0 then _inv_x = 1 / _rmx end
local _inv_y = math.huge
if _rmy ~= 0 then _inv_y = 1 / _rmy end
local _minkowski_tl = _minkowski_pos:vsub(_minkowski_halfsize)
local _minkowski_br = _minkowski_pos:vadd(_minkowski_halfsize)
--clip x
--get edge t along line
local tx1 = (_minkowski_tl.x) * _inv_x
local tx2 = (_minkowski_br.x) * _inv_x
--clip to existing clip space
local txmin = math.min(tx1, tx2)
local txmax = math.max(tx1, tx2)
--clip y
--get edge t along line
local ty1 = (_minkowski_tl.y) * _inv_y
local ty2 = (_minkowski_br.y) * _inv_y
--clip to existing clip space
local tymin = math.min(ty1, ty2)
local tymax = math.max(ty1, ty2)
--clip space
local tmin = math.max(0, txmin, tymin)
local tmax = math.min(1, txmax, tymax)
--still some unclipped? collision!
if tmin <= tmax then
--"was colliding at start"
if tmin == 0 then
--todo: maybe collide at old pos, not new pos
local msv = self:collide(other, into)
if msv then
return msv, tmin
else
return false
end
end
--delta before colliding
local _self_collide_pre = _self_delta_motion:smul(tmin)
--delta after colliding (to be discarded or projected or whatever)
local _self_collide_post = _self_delta_motion:smul(1.0 - tmin)
--get the collision normal
--(whichever boundary crossed _last_ -> normal)
local _self_collide_normal = vec2:zero()
if txmin > tymin then
_self_collide_normal.x = -math.sign(_self_delta_motion.x)
else
_self_collide_normal.y = -math.sign(_self_delta_motion.y)
end
--travelling away from normal?
if _self_collide_normal:dot(_self_delta_motion) >= 0 then
return false
end
--just "slide" projection for now
_self_collide_post:vreji(_self_collide_normal)
--combine
local _final_delta = _self_collide_pre:vadd(_self_collide_post)
--construct the target position
local _target_pos = a_startpos:vadd(_final_delta)
--return delta to target pos
local msv = _target_pos:vsub(a_endpos)
if math.abs(msv.x) > COLLIDE_EPS or math.abs(msv.y) > COLLIDE_EPS then
into:vset(msv)
return into, tmin
end
end
return false
end
return intersect