Pushed Theta Astar implementation

Added support for clearance

jumper/pathfinder.lua

@@ -37,6 +37,7 @@ if (...) then
   local Finders = {
     ['ASTAR']     = require (_PATH .. '.search.astar'),
     ['DIJKSTRA']  = require (_PATH .. '.search.dijkstra'),
+    ['THETASTAR'] = require (_PATH .. '.search.thetastar'),
     ['BFS']       = require (_PATH .. '.search.bfs'),
     ['DFS']       = require (_PATH .. '.search.dfs'),
     ['JPS']       = require (_PATH .. '.search.jps')

jumper/search/astar.lua

@@ -14,7 +14,7 @@ if (...) then
 	local Heap = require (_PATH.. '.core.bheap')
 
 	-- Updates G-cost
-	local function computeCost(node, neighbour, finder)
+	local function computeCost(node, neighbour, finder, clearance)
 		local mCost = Heuristics.EUCLIDIAN(neighbour._x - node._x, neighbour._y - node._y)
 		if node._g + mCost < neighbour._g then
 			neighbour._parent = node
@@ -26,7 +26,7 @@ if (...) then
 	local function updateVertex(finder, openList, node, neighbour, endNode, clearance, heuristic, overrideCostEval)
 		local oldG = neighbour._g
 		local cmpCost = overrideCostEval or computeCost
-		cmpCost(node, neighbour, finder)
+		cmpCost(node, neighbour, finder, clearance)
 		if neighbour._g < oldG then
 			local nClearance = neighbour._clearance[finder._walkable] or finder._grid:evalClearance(neighbour, finder._walkable)
 			local pushThisNode = clearance and nClearance and (nClearance >= clearance)

jumper/search/thetastar.lua

@@ -0,0 +1,71 @@
+-- ThetaStar implementation
+-- See: http://aigamedev.com/open/tutorials/theta-star-any-angle-paths for reference
+
+if (...) then
+
+	local _PATH = (...):gsub('%.search.thetastar$','')
+
+	-- Depandancies
+	local Heuristics   = require (_PATH .. '.core.heuristics')
+	local astar_search = require (_PATH .. '.search.astar')
+
+	-- Internalization
+	local ipairs = ipairs
+	local huge, abs = math._huge, math.abs
+
+	-- Line Of Sight (Bresenham's line marching algorithm)
+	-- http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+	local lineOfSight = function (node, neighbour, finder, clearance)
+		local x0, y0 = node._x, node._y
+		local x1, y1 = neighbour._x, neighbour._y
+		local dx = abs(x1-x0)
+		local dy = abs(y1-y0)
+		local err = dx - dy
+		local sx = (x0 < x1) and 1 or -1
+		local sy = (y0 < y1) and 1 or -1		
+
+		while true do
+			if not finder._grid:isWalkableAt(x0, y0, finder._walkable, finder._tunnel, clearance) then 
+				return false 
+			end
+			if x0 == x1 and y0 == y1 then
+				break
+			end
+			local e2 = 2*err
+			if e2 > -dy then
+				err = err - dy
+				x0 = x0 + sx
+			end
+			if e2 < dx then
+				err = err + dx
+				y0 = y0 + sy
+			end
+		end
+		return true
+	end
+
+	-- Theta star cost evaluation
+	local function computeCost(node, neighbour, finder, clearance)
+		local parent = node._parent or node
+		local mpCost = Heuristics.EUCLIDIAN(neighbour._x - parent._x, neighbour._y - parent._y)
+		if lineOfSight(parent, neighbour, finder, clearance) then
+			if parent._g + mpCost < neighbour._g then
+				neighbour._parent = parent
+				neighbour._g = parent._g + mpCost
+			end
+		else
+			local mCost = Heuristics.EUCLIDIAN(neighbour._x - node._x, neighbour._y - node._y)
+			if node._g + mCost < neighbour._g then
+				neighbour._parent = node
+				neighbour._g = node._g + mCost
+			end
+		end
+	end
+
+  -- Calculates a path.
+  -- Returns the path from location `<startX, startY>` to location `<endX, endY>`.
+  return function (finder, startNode, endNode, clearance, toClear, overrideHeuristic)
+    return astar_search(finder, startNode, endNode, clearance, toClear, overrideHeuristic, computeCost)
+	end
+
+end