diff --git a/gjk.lua b/gjk.lua index ebaad05..e792f19 100644 --- a/gjk.lua +++ b/gjk.lua @@ -28,17 +28,19 @@ local _PACKAGE = (...):match("^(.+)%.[^%.]+") local vector = require(_PACKAGE .. '.vector-light') local huge, abs = math.huge, math.abs +local simplex, edge = {}, {} + local function support(shape_a, shape_b, dx, dy) local x,y = shape_a:support(dx,dy) return vector.sub(x,y, shape_b:support(-dx, -dy)) end -- returns closest edge to the origin -local function closest_edge(simplex) - local e = {dist = huge} +local function closest_edge(n) + edge.dist = huge - local i = #simplex-1 - for k = 1,#simplex-1,2 do + local i = n-1 + for k = 1,n-1,2 do local ax,ay = simplex[i], simplex[i+1] local bx,by = simplex[k], simplex[k+1] i = k @@ -47,19 +49,17 @@ local function closest_edge(simplex) local nx,ny = vector.normalize(ex,ey) local d = vector.dot(ax,ay, nx,ny) - if d < e.dist then - e.dist = d - e.nx, e.ny = nx, ny - e.i = k + if d < edge.dist then + edge.dist = d + edge.nx, edge.ny = nx, ny + edge.i = k end end - - return e end -local function EPA(shape_a, shape_b, simplex) +local function EPA(shape_a, shape_b) -- make sure simplex is oriented counter clockwise - local cx,cy, bx,by, ax,ay = unpack(simplex) + local cx,cy, bx,by, ax,ay = unpack(simplex, 1, 6) if vector.dot(ax-bx,ay-by, cx-bx,cy-by) < 0 then simplex[1],simplex[2] = ax,ay simplex[5],simplex[6] = cx,cy @@ -67,29 +67,33 @@ local function EPA(shape_a, shape_b, simplex) -- the expanding polytype algorithm local is_either_circle = shape_a._center or shape_b._center - local last_diff_dist = huge + local last_diff_dist, n = huge, 6 while true do - local e = closest_edge(simplex) - local px,py = support(shape_a, shape_b, e.nx, e.ny) - local d = vector.dot(px,py, e.nx, e.ny) + closest_edge(n) + local px,py = support(shape_a, shape_b, edge.nx, edge.ny) + local d = vector.dot(px,py, edge.nx, edge.ny) - local diff_dist = d - e.dist + local diff_dist = d - edge.dist if diff_dist < 1e-6 or (is_either_circle and abs(last_diff_dist - diff_dist) < 1e-10) then - return -d*e.nx, -d*e.ny + return -d*edge.nx, -d*edge.ny end last_diff_dist = diff_dist - -- simplex = {..., simplex[e.i-1], px, py, simplex[e.i] - table.insert(simplex, e.i, py) - table.insert(simplex, e.i, px) + -- simplex = {..., simplex[edge.i-1], px, py, simplex[edge.i] + for i = n, edge.i, -1 do + simplex[i+2] = simplex[i] + end + simplex[edge.i+0] = px + simplex[edge.i+1] = py + n = n + 2 end end -- : : origin must be in plane between A and B -- B o------o A since A is the furthest point on the MD -- : : in direction of the origin. -local function do_line(simplex) - local bx,by, ax,ay = unpack(simplex) +local function do_line() + local bx,by, ax,ay = unpack(simplex, 1, 4) local abx,aby = bx-ax, by-ay @@ -98,7 +102,7 @@ local function do_line(simplex) if vector.dot(dx,dy, -ax,-ay) < 0 then dx,dy = -dx,-dy end - return simplex, dx,dy + return dx,dy end -- B .' @@ -108,8 +112,8 @@ end -- | _.-' '. from left of BC. -- o-' 3 -- C '. -local function do_triangle(simplex) - local cx,cy, bx,by, ax,ay = unpack(simplex) +local function do_triangle() + local cx,cy, bx,by, ax,ay = unpack(simplex, 1, 6) local aox,aoy = -ax,-ay local abx,aby = bx-ax, by-ay local acx,acy = cx-ax, cy-ay @@ -123,8 +127,7 @@ local function do_triangle(simplex) -- simplex = {bx,by, ax,ay} simplex[1], simplex[2] = bx,by simplex[3], simplex[4] = ax,ay - simplex[5], simplex[6] = nil, nil - return simplex, dx,dy + return 4, dx,dy end -- test region 3 @@ -135,12 +138,11 @@ local function do_triangle(simplex) if vector.dot(dx,dy, aox, aoy) > 0 then -- simplex = {cx,cy, ax,ay} simplex[3], simplex[4] = ax,ay - simplex[5], simplex[6] = nil, nil - return simplex, dx,dy + return 4, dx,dy end -- must be in region 4 - return simplex + return 6 end local function GJK(shape_a, shape_b) @@ -158,8 +160,7 @@ local function GJK(shape_a, shape_b) return false end - local simplex = {ax,ay} - local n = 2 + simplex[1], simplex[2] = ax, ay local dx,dy = -ax,-ay -- first iteration: line case @@ -168,9 +169,10 @@ local function GJK(shape_a, shape_b) return false end - simplex[n+1], simplex[n+2] = ax,ay - simplex, dx, dy = do_line(simplex, dx, dy) - n = 4 + simplex[3], simplex[4] = ax,ay + dx, dy = do_line() + + local n -- all other iterations must be the triangle case while true do @@ -180,12 +182,11 @@ local function GJK(shape_a, shape_b) return false end - simplex[n+1], simplex[n+2] = ax,ay - simplex, dx, dy = do_triangle(simplex, dx,dy) - n = #simplex + simplex[5], simplex[6] = ax,ay + n, dx, dy = do_triangle() if n == 6 then - return true, EPA(shape_a, shape_b, simplex) + return true, EPA(shape_a, shape_b) end end end