bit of everything

sys/apis/point.lua

@@ -4,13 +4,19 @@ function Point.copy(pt)
   return { x = pt.x, y = pt.y, z = pt.z }
 end
 
+function Point.same(pta, ptb)
+  return pta.x == ptb.x and
+         pta.y == ptb.y and
+         pta.z == ptb.z
+end
+
 function Point.subtract(a, b)
   a.x = a.x - b.x
   a.y = a.y - b.y
   a.z = a.z - b.z
 end
 
--- real distance
+-- Euclidian distance
 function Point.pythagoreanDistance(a, b)
   return math.sqrt(
            math.pow(a.x - b.x, 2) +
@@ -18,7 +24,7 @@ function Point.pythagoreanDistance(a, b)
            math.pow(a.z - b.z, 2))
 end
 
--- turtle distance
+-- turtle distance (manhattan)
 function Point.turtleDistance(a, b)
   if a.y and b.y then
     return math.abs(a.x - b.x) + 
@@ -40,6 +46,35 @@ function Point.calculateTurns(ih, oh)
   return 1
 end
 
+function Point.calculateHeading(pta, ptb)
+
+  local heading
+
+  if (pta.heading % 2) == 0 and pta.z ~= ptb.z then
+    if ptb.z > pta.z then
+      heading = 1
+    else
+      heading = 3
+    end
+  elseif (pta.heading % 2) == 1 and pta.x ~= ptb.x then
+    if ptb.x > pta.x then
+      heading = 0
+    else
+      heading = 2
+    end
+  elseif pta.heading == 0 and pta.x > ptb.x then
+    heading = 2
+  elseif pta.heading == 2 and pta.x < ptb.x then
+    heading = 0
+  elseif pta.heading == 1 and pta.z > ptb.z then
+    heading = 3
+  elseif pta.heading == 3 and pta.z < ptb.z then
+    heading = 1
+  end
+
+  return heading or pta.heading
+end
+
 -- Calculate distance to location including turns
 -- also returns the resulting heading
 function Point.calculateMoves(pta, ptb, distance)
@@ -88,6 +123,32 @@ function Point.closest(reference, pts)
   return lpt
 end
 
+-- find the closest block
+-- * favor same plane
+-- * going backwards only if the dest is above or below
+function Point.closest2(reference, pts)
+  local lpt, lm -- lowest
+  for _,pt in pairs(pts) do
+    local m = Point.turtleDistance(reference, pt)
+    local h = Point.calculateHeading(reference, pt)
+    local t = Point.calculateTurns(reference.heading, h)
+    if pt.y ~= reference.y then -- try and stay on same plane
+      m = m + .01
+    end
+    if t ~= 2 or pt.y == reference.y then
+      m = m + t
+      if t > 0 then
+        m = m + .01
+      end
+    end
+    if not lm or m < lm then
+      lpt = pt
+      lm = m
+    end
+  end
+  return lpt
+end
+
 function Point.adjacentPoints(pt)
   local pts = { }