From 7fa1c522ebf5833675e1a5de32095d89fcc03a5d Mon Sep 17 00:00:00 2001
From: ElCeejo <christianjames8404@gmail.com>
Date: Sun, 24 Jul 2022 21:15:06 -0700
Subject: [PATCH] Add new LVM Pathfinder

---
 .luacheckrc    |   1 +
 methods.lua    |  61 ++++++++--
 mob_meta.lua   |   2 +-
 pathfinder.lua | 322 +++++++++++++++++++++++++++++++++++++++++++++----
 4 files changed, 348 insertions(+), 38 deletions(-)

diff --git a/.luacheckrc b/.luacheckrc
index 7c76332..f8c5ab3 100644
--- a/.luacheckrc
+++ b/.luacheckrc
@@ -2,6 +2,7 @@ max_line_length = 120
 
 globals = {
 	"minetest",
+	"VoxelArea",
 	"creatura",
 }
 
diff --git a/methods.lua b/methods.lua
index 39733dd..d8d6b71 100644
--- a/methods.lua
+++ b/methods.lua
@@ -23,6 +23,8 @@ local function clamp(val, min, max)
 	return val
 end
 
+local vec_normal = vector.normalize
+local vec_len = vector.length
 local vec_dist = vector.distance
 local vec_dir = vector.direction
 local vec_multi = vector.multiply
@@ -57,8 +59,8 @@ local function raycast(pos1, pos2, liquid)
 end
 
 local function get_collision(self, yaw)
-	local width = self.width
-	local height = self.height
+	local width = self.width + 0.5
+	local height = self.height + 0.5
 	local total_height = height + self.stepheight
 	local pos = self.object:get_pos()
 	if not pos then return end
@@ -107,8 +109,11 @@ local function get_avoidance_dir(self)
 	if not pos then return end
 	local collide, col_pos = get_collision(self, self.object:get_yaw())
 	if collide then
-		local avoid_dir = vec_dir(col_pos, pos)
-		return vec_multi(avoid_dir, self.width)
+		local vel = self.object:get_velocity()
+		local ahead = vec_add(pos, vec_normal(self.object:get_velocity()))
+		local avoidance_force = vector.subtract(ahead, col_pos)
+		avoidance_force = vec_normal(avoidance_force) * vec_len(vel)
+		return vec_dir(pos, vec_add(ahead, avoidance_force))
 	end
 end
 
@@ -191,31 +196,63 @@ end
 
 -- Pathfinding
 
+local function trim_path(pos, path)
+	if #path < 2 then return end
+	local trim = false
+	local closest
+	for i = #path, 1, -1 do
+		if (closest
+		and vec_dist(pos, path[i]) > vec_dist(pos, path[closest]))
+		or trim then
+			table.remove(path, i)
+			trim = true
+		else
+			closest = i
+		end
+	end
+	return path
+end
+
 creatura.register_movement_method("creatura:pathfind", function(self)
 	local path = {}
 	local box = clamp(self.width, 0.5, 1.5)
 	self:set_gravity(-9.8)
+	local trimmed = false
+	local init_path = false
+	local tick = 4
 	local function func(_self, goal, speed_factor)
 		local pos = _self.object:get_pos()
 		if not pos then return end
-		pos.y = pos.y + 0.5
 		-- Return true when goal is reached
 		if vec_dist(pos, goal) < box * 1.33 then
 			_self:halt()
 			return true
 		end
 		-- Get movement direction
-		local steer_to = get_avoidance_dir(_self, goal, speed_factor)
+		local steer_to
+		tick = tick - 1
+		if tick <= 0 then
+			steer_to = get_avoidance_dir(self, goal)
+			tick = 4
+		end
 		local goal_dir = vec_dir(pos, goal)
-		if steer_to then
+		if steer_to
+		and not init_path then
 			goal_dir = steer_to
-			if #path < 2 then
-				path = creatura.find_path(_self, pos, goal, _self.width, _self.height, 200) or {}
-			end
+			init_path = true
+		end
+		if init_path
+		and #path < 2 then
+			path = creatura.find_lvm_path(_self, pos, goal, _self.width, _self.height, 400) or {}
 		end
 		if #path > 1 then
+			if not trimmed then
+				path = trim_path(pos, path)
+				trimmed = true
+				if #path < 2 then return end
+			end
 			goal_dir = vec_dir(pos, path[2])
-			if vec_dist(pos, path[1]) < box then
+			if vec_dist(vector.round(pos), creatura.get_ground_level(path[1], 1)) < box then
 				table.remove(path, 1)
 			end
 		end
@@ -323,4 +360,4 @@ creatura.register_movement_method("creatura:obstacle_avoidance", function(self)
 		end
 	end
 	return func
-end)
+end)
\ No newline at end of file
diff --git a/mob_meta.lua b/mob_meta.lua
index 46c105c..56ea7fb 100644
--- a/mob_meta.lua
+++ b/mob_meta.lua
@@ -166,7 +166,6 @@ local function lerp_rad(a, b, w)
 end
 
 function mob:turn_to(tyaw, rate)
-	self.last_yaw = self.object:get_yaw()
 	self._tyaw = tyaw
 	rate = rate or 5
 	local yaw = self.object:get_yaw()
@@ -769,6 +768,7 @@ end
 
 function mob:on_step(dtime, moveresult)
 	if not self.hp then return end
+	self.last_yaw = self.object:get_yaw()
 	self.dtime = dtime or 0.09
 	self.moveresult = moveresult or {}
 	self.touching_ground = false
diff --git a/pathfinder.lua b/pathfinder.lua
index f7ad384..e2b466e 100644
--- a/pathfinder.lua
+++ b/pathfinder.lua
@@ -8,7 +8,7 @@ local theta_star_alloted_time = tonumber(minetest.settings:get("creatura_theta_s
 local floor = math.floor
 local abs = math.abs
 
-local vec_dist = vector.distance
+local vec_dist, vec_round = vector.distance, vector.round
 
 local moveable = creatura.is_pos_moveable
 
@@ -85,6 +85,284 @@ end
 
 -- Find a path from start to goal
 
+--[[local function debugpart(pos, time, tex)
+	minetest.add_particle({
+		pos = pos,
+		texture = tex or "creatura_particle_red.png",
+		expirationtime = time or 0.1,
+		glow = 6,
+		size = 12
+	})
+end]]
+
+local c_air = minetest.get_content_id("air")
+
+local function is_pos_moveable_vm(pos, width, height, area, data)
+	pos = vector.round(pos)
+	local pos1 = {
+		x = pos.x - math.ceil(width),
+		y = pos.y,
+		z = pos.z - math.ceil(width)
+	}
+	local pos2 = {
+		x = pos.x + math.ceil(width),
+		y = pos.y + math.ceil(height),
+		z = pos.z + math.ceil(width)
+	}
+	for z = pos1.z, pos2.z do
+	for y = pos1.y, pos2.y do
+	for x = pos1.x, pos2.x do
+		if not area:contains(x, y, z) then return false end
+		local vi = area:index(x, y, z)
+		local c = data[vi]
+		if c ~= c_air then
+			local c_name = minetest.get_name_from_content_id(c)
+			if creatura.get_node_def(c_name).walkable then
+				return false
+			end
+		end
+	end
+	end
+	end
+	return true
+end
+
+local vm_buffer = {}
+
+function creatura.find_lvm_path(self, start, goal, obj_width, obj_height, max_open, climb, fly, swim)
+	climb = climb or false
+	fly = fly or false
+	swim = swim or false
+
+	start = start
+
+	--self._path_data.start = start
+
+	local path_neighbors = {
+		{x = 1, y = 0, z = 0},
+		{x = 1, y = 0, z = 1},
+		{x = 0, y = 0, z = 1},
+		{x = -1, y = 0, z = 1},
+		{x = -1, y = 0, z = 0},
+		{x = -1, y = 0, z = -1},
+		{x = 0, y = 0, z = -1},
+		{x = 1, y = 0, z = -1}
+	}
+
+	if climb then
+		table.insert(path_neighbors, {x = 0, y = 1, z = 0})
+	end
+
+	if fly
+	or swim then
+		path_neighbors = {
+			-- Central
+			{x = 1, y = 0, z = 0},
+			{x = 0, y = 0, z = 1},
+			{x = -1, y = 0, z = 0},
+			{x = 0, y = 0, z = -1},
+			-- Directly Up or Down
+			{x = 0, y = 1, z = 0},
+			{x = 0, y = -1, z = 0}
+		}
+	end
+
+
+	local function get_neighbors(pos, width, height, tbl, open, closed, vm_area, vm_data)
+		local result = {}
+		for i = 1, #tbl do
+			local neighbor = vector.add(pos, tbl[i])
+			if not vm_area or not vm_data or not vm_area:containsp(neighbor) then return end
+			local can_move = (not swim and get_line_of_sight({x = pos.x, y = neighbor.y, z = pos.z}, neighbor)) or true
+			if open[minetest.hash_node_position(neighbor)]
+			or closed[minetest.hash_node_position(neighbor)] then
+				can_move = false
+			end
+			if can_move then
+				can_move = is_pos_moveable_vm(neighbor, width, height, vm_area, vm_data)
+				if not fly and not swim then
+					if not can_move then -- Step Up
+						local step = vec_raise(neighbor, 1)
+						can_move = is_pos_moveable_vm(vec_round(step), width, height, vm_area, vm_data)
+						neighbor = vec_round(step)
+					else
+						local step = creatura.get_ground_level(vector.new(neighbor), 1)
+						if step.y < neighbor.y
+						and is_pos_moveable_vm(vec_round(step), width, height, vm_area, vm_data) then
+							neighbor = step
+						end
+					end
+				end
+			end
+			if vector.equals(neighbor, goal) then
+				can_move = true
+			end
+			if can_move
+			and (not swim
+			or creatura.get_node_def(neighbor).drawtype == "liquid") then
+				table.insert(result, neighbor)
+			end
+		end
+		return result
+	end
+
+	local function find_path(_start, _goal)
+		local us_time = minetest.get_us_time()
+
+		_start = {
+			x = floor(_start.x + 0.5),
+			y = floor(_start.y + 0.5),
+			z = floor(_start.z + 0.5)
+		}
+
+		_goal = {
+			x = floor(_goal.x + 0.5),
+			y = floor(_goal.y + 0.5),
+			z = floor(_goal.z + 0.5)
+		}
+
+		if _goal.x == _start.x
+		and _goal.z == _start.z then -- No path can be found
+			return nil
+		end
+
+		local vm_area = self._path_data.vm_area
+		local vm_data = self._path_data.vm_data
+
+		if not vm_area
+		or not vm_data then
+			local vm_center = vector.add(_start, vector.divide(vector.subtract(_goal, _start), 2))
+			local vm_size = vec_dist(_goal, _start)
+			if vm_size < 24 then vm_size = 24 end
+			local e1 = vector.subtract(vm_center, vm_size)
+			local e2 = vector.add(vm_center, vm_size)
+			local vm = minetest.get_voxel_manip(e1, e2)
+			e1, e2 = vm:read_from_map(e1, e2)
+			vm_area = VoxelArea:new{MinEdge=e1, MaxEdge=e2}
+			vm_data = vm:get_data(vm_buffer)
+		end
+
+		local openSet = self._path_data.open or {}
+
+		local closedSet = self._path_data.closed or {}
+
+		local start_index = minetest.hash_node_position(_start)
+
+		openSet[start_index] = {
+			pos = _start,
+			parent = nil,
+			gScore = 0,
+			fScore = get_distance(_start, _goal)
+		}
+
+		local count = self._path_data.count or 1
+
+		while count > 0 do
+			-- Initialize ID and data
+			local current_id, current = next(openSet)
+
+			-- Find lowest f cost
+			for i, v in pairs(openSet) do
+				if v.fScore < current.fScore then
+					current_id = i
+					current = v
+				end
+			end
+
+			-- Add lowest fScore to closedSet and remove from openSet
+			openSet[current_id] = nil
+			closedSet[current_id] = current
+
+			self._path_data.open = openSet
+			self._path_data.closedSet = closedSet
+
+			-- Reconstruct path if end is reached
+			if ((is_on_ground(_goal)
+			or fly)
+			and current_id == minetest.hash_node_position(_goal))
+			or (not fly
+			and not is_on_ground(_goal)
+			and math.abs(_goal.x - current.pos.x) < 1.1
+			and math.abs(_goal.z - current.pos.z) < 1.1) then
+				local path = {}
+				local fail_safe = 0
+				for _ in pairs(closedSet) do
+					fail_safe = fail_safe + 1
+				end
+				repeat
+					if not closedSet[current_id] then return end
+					table.insert(path, closedSet[current_id].pos)
+					current_id = closedSet[current_id].parent
+				until current_id == start_index or #path >= fail_safe
+				if not closedSet[current_id] then self._path_data = {} return nil end
+				table.insert(path, closedSet[current_id].pos)
+				local reverse_path = {}
+				repeat table.insert(reverse_path, table.remove(path)) until #path == 0
+				self._path_data = {}
+				return reverse_path
+			end
+
+			count = count - 1
+
+			local adjacent = get_neighbors(
+				current.pos,
+				obj_width,
+				obj_height,
+				path_neighbors,
+				openSet,
+				closedSet,
+				vm_area,
+				vm_data
+			)
+
+			-- Go through neighboring nodes
+			if not adjacent or #adjacent < 1 then self._path_data = {} return {} end
+			for i = 1, #adjacent do
+				local neighbor = {
+					pos = adjacent[i],
+					parent = current_id,
+					gScore = 0,
+					fScore = 0
+				}
+				local temp_gScore = current.gScore + get_distance_to_neighbor(current.pos, neighbor.pos)
+				local new_gScore = 0
+				if openSet[minetest.hash_node_position(neighbor.pos)] then
+					new_gScore = openSet[minetest.hash_node_position(neighbor.pos)].gScore
+				end
+				if (temp_gScore < new_gScore
+				or not openSet[minetest.hash_node_position(neighbor.pos)])
+				and not closedSet[minetest.hash_node_position(neighbor.pos)] then
+					if not openSet[minetest.hash_node_position(neighbor.pos)] then
+						count = count + 1
+					end
+					local hCost = get_distance_to_neighbor(neighbor.pos, _goal)
+					neighbor.gScore = temp_gScore
+					neighbor.fScore = temp_gScore + hCost
+					openSet[minetest.hash_node_position(neighbor.pos)] = neighbor
+				end
+			end
+			if minetest.get_us_time() - us_time > a_star_alloted_time then
+				self._path_data = {
+					start = _start,
+					open = openSet,
+					closed = closedSet,
+					count = count,
+					vm_area = vm_area,
+					vm_data = vm_data
+				}
+				return {}
+			end
+			if count > (max_open or 100) then
+				self._path_data = {}
+				return
+			end
+		end
+		self._path_data = {}
+		return nil
+	end
+	return find_path(start, goal)
+end
+
 function creatura.find_path(self, start, goal, obj_width, obj_height, max_open, climb, fly, swim)
 	climb = climb or false
 	fly = fly or false
@@ -127,36 +405,31 @@ function creatura.find_path(self, start, goal, obj_width, obj_height, max_open,
 		local result = {}
 		for i = 1, #tbl do
 			local neighbor = vector.add(pos, tbl[i])
-			if neighbor.y == pos.y
-			and not fly
-			and not swim then
-				neighbor = creatura.get_ground_level(neighbor, 1)
-			end
-			local can_move = get_line_of_sight({x = pos.x, y = neighbor.y, z = pos.z}, neighbor)
-			if swim then
-				can_move = true
-			end
-			if not moveable(vec_raise(neighbor, -0.49), width, height) then
+			local can_move = (not swim and get_line_of_sight({x = pos.x, y = neighbor.y, z = pos.z}, neighbor)) or true
+			if open[minetest.hash_node_position(neighbor)]
+			or closed[minetest.hash_node_position(neighbor)] then
 				can_move = false
-				if neighbor.y == pos.y
-				and moveable(vec_raise(neighbor, 0.51), width, height) then
-					neighbor = vec_raise(neighbor, 1)
-					can_move = true
+			end
+			if can_move then
+				can_move = moveable(neighbor, width, height)
+				if not fly and not swim then
+					if not can_move then -- Step Up
+						local step = vec_raise(neighbor, 1)
+						can_move = moveable(vec_round(step), width, height)
+						neighbor = vec_round(step)
+					else
+						local step = creatura.get_ground_level(vector.new(neighbor), 1)
+						if step.y < neighbor.y
+						and moveable(vec_round(step), width, height) then
+							neighbor = step
+						end
+					end
 				end
 			end
 			if vector.equals(neighbor, goal) then
 				can_move = true
 			end
-			if open[minetest.hash_node_position(neighbor)]
-			or closed[minetest.hash_node_position(neighbor)] then
-				can_move = false
-			end
 			if can_move
-			and ((is_on_ground(neighbor)
-			or (fly or swim))
-			or (neighbor.x == pos.x
-			and neighbor.z == pos.z
-			and climb))
 			and (not swim
 			or creatura.get_node_def(neighbor).drawtype == "liquid") then
 				table.insert(result, neighbor)
@@ -294,7 +567,6 @@ function creatura.find_path(self, start, goal, obj_width, obj_height, max_open,
 	return find_path(start, goal)
 end
 
-
 ------------
 -- Theta* --
 ------------