Merge pull request #51 from Positive07/master

Performance and memory improvements in GJK

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