refactor + cleanup

sys/apis/turtle/pathfind.lua

@@ -1,7 +1,7 @@
 _G.requireInjector()
 
-local Grid       = require ("jumper.grid")
-local Pathfinder = require ("jumper.pathfinder")
+local Grid       = require('jumper.grid')
+local Pathfinder = require('jumper.pathfinder')
 local Point      = require('point')
 local Util       = require('util')
 
@@ -19,74 +19,46 @@ end
 -- map shrinks/grows depending upon blocks encountered
 -- the map will encompass any blocks encountered, the turtle position, and the destination
 local function mapDimensions(dest, blocks, boundingBox, dests)
-	local sx, sz, sy = turtle.point.x, turtle.point.z, turtle.point.y
-	local ex, ez, ey = turtle.point.x, turtle.point.z, turtle.point.y
+	local box = Point.makeBox(turtle.point, turtle.point)
 
-	local function adjust(pt)
-		if pt.x < sx then
-			sx = pt.x
-		elseif pt.x > ex then
-			ex = pt.x
-		end
-		if pt.y < sy then
-			sy = pt.y
-		elseif pt.y > ey then
-			ey = pt.y
-		end
-		if pt.z < sz then
-			sz = pt.z
-		elseif pt.z > ez then
-			ez = pt.z
-		end
-	end
-
-	adjust(dest)
+	Point.expandBox(box, dest)
 
 	for _,d in pairs(dests) do
-		adjust(d)
+		Point.expandBox(box, d)
 	end
 
 	for _,b in pairs(blocks) do
-		adjust(b)
+		Point.expandBox(box, b)
 	end
 
 	-- expand one block out in all directions
 	if boundingBox then
-		sx = math.max(sx - 1, boundingBox.x)
-		sz = math.max(sz - 1, boundingBox.z)
-		sy = math.max(sy - 1, boundingBox.y)
-		ex = math.min(ex + 1, boundingBox.ex)
-		ez = math.min(ez + 1, boundingBox.ez)
-		ey = math.min(ey + 1, boundingBox.ey)
+		box.x = math.max(box.x - 1, boundingBox.x)
+		box.z = math.max(box.z - 1, boundingBox.z)
+		box.y = math.max(box.y - 1, boundingBox.y)
+		box.ex = math.min(box.ex + 1, boundingBox.ex)
+		box.ez = math.min(box.ez + 1, boundingBox.ez)
+		box.ey = math.min(box.ey + 1, boundingBox.ey)
 	else
-		sx = sx - 1
-		sz = sz - 1
-		sy = sy - 1
-		ex = ex + 1
-		ez = ez + 1
-		ey = ey + 1
+		box.x = box.x - 1
+		box.z = box.z - 1
+		box.y = box.y - 1
+		box.ex = box.ex + 1
+		box.ez = box.ez + 1
+		box.ey = box.ey + 1
 	end
 
-	return {
-		ex = ex,
-		ez = ez,
-		ey = ey,
-		x = sx,
-		z = sz,
-		y = sy
-	}
+	return box
 end
 
 local function nodeToPoint(node)
-	return { x = node:getX(), z = node:getZ(), y = node:getY() }
+	return { x = node.x, y = node.y, z = node.z, heading = node.heading }
 end
 
-local heuristic = function(n, node)
-	local m, h = Point.calculateMoves(
-			{ x = node._x, y = node._y, z = node._z, heading = node._heading },
-			{ x = n._x, y = n._y, z = n._z, heading = n._heading })
-
-	return m, h
+local function heuristic(n, node)
+	return Point.calculateMoves(node, n)
+--			{ x = node.x, y = node.y, z = node.z, heading = node.heading },
+--			{ x = n.x, y = n.y, z = n.z, heading = n.heading })
 end
 
 local function dimsAreEqual(d1, d2)
@@ -123,9 +95,6 @@ local function addSensorBlocks(blocks, sblocks)
 end
 
 local function selectDestination(pts, box, grid)
-	if #pts == 1 then
-		return pts[1]
-	end
 	while #pts > 0 do
 		local pt = Point.closest(turtle.point, pts)
 		if box and not Point.inBox(pt, box) then
@@ -143,16 +112,15 @@ local function pathTo(dest, options)
 	local blocks = options.blocks or turtle.getState().blocks or { }
 	local dests  = options.dest   or { dest }  -- support alternative destinations
 	local box    = options.box    or turtle.getState().box
-
-	local lastDim = nil
-	local grid = nil
+	local lastDim
+	local grid
 
 	if box then
 		box = Point.normalizeBox(box)
 	end
 
 	-- Creates a pathfinder object
-	local myFinder = Pathfinder(heuristic)
+	local finder = Pathfinder(heuristic)
 
 	while turtle.point.x ~= dest.x or turtle.point.z ~= dest.z or turtle.point.y ~= dest.y do
 
@@ -163,7 +131,7 @@ local function pathTo(dest, options)
 		if not lastDim or not dimsAreEqual(dim, lastDim) then
 			-- Creates a grid object
 			grid = Grid(dim)
-			myFinder:setGrid(grid)
+			finder:setGrid(grid)
 
 			lastDim = dim
 		end
@@ -173,7 +141,6 @@ local function pathTo(dest, options)
 
 		dest = selectDestination(dests, box, grid)
 		if not dest then
---			error('failed to reach destination')
 			return false, 'failed to reach destination'
 		end
 		if turtle.point.x == dest.x and turtle.point.z == dest.z and turtle.point.y == dest.y then
@@ -185,24 +152,44 @@ local function pathTo(dest, options)
 		local endPt = dest
 
 		-- Calculates the path, and its length
-		local path = myFinder:getPath(
+		local path = finder:getPath(
 			startPt.x, startPt.y, startPt.z, turtle.point.heading,
 			endPt.x, endPt.y, endPt.z, dest.heading)
 
 		if not path then
 	    Util.removeByValue(dests, dest)
 		else
+			path:filter()
+
 			for node in path:nodes() do
 				local pt = nodeToPoint(node)
 
-				if turtle.abort then
+				if turtle.isAborted() then
 					return false, 'aborted'
 				end
 
+--if this is the next to last node
+--and we are traveling up or down, then the
+--heading for this node should be the heading of the last node
+--or, maybe..
+--if last node is up or down (or either?)
+
 				-- use single turn method so the turtle doesn't turn around
-				-- when encountering obstacles -- IS THIS RIGHT ??
-				if not turtle.gotoSingleTurn(pt.x, pt.z, pt.y) then
-					table.insert(blocks, pt)
+				-- when encountering obstacles
+				if not turtle.gotoSingleTurn(pt.x, pt.z, pt.y, pt.heading) then
+					local bpt = Point.nearestTo(turtle.point, pt)
+
+					table.insert(blocks, bpt)
+					-- really need to check if the block we ran into was a turtle.
+					-- if so, this block should be temporary (1-2 secs)
+
+					--local side = turtle.getSide(turtle.point, pt)
+					--if turtle.isTurtleAtSide(side) then
+					--	pt.timestamp = os.clock() + ?
+					--end
+					-- if dim has not changed, then need to update grid with
+					-- walkable = nil (after time has elapsed)
+
 					--if device.turtlesensorenvironment then
 					--	addSensorBlocks(blocks, device.turtlesensorenvironment.sonicScan())
 					--end