batteries/intersect.lua

--[[
	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