Merge pull request 'Villagers and pathfinding improvements' (#3083) from village_town_bell_pathfix into master

Reviewed-on: MineClone2/MineClone2#3083
Reviewed-by: Michieal <michieal@noreply.git.minetest.land>

mods/ENTITIES/mcl_mobs/pathfinding.lua

@@ -1,11 +1,24 @@
 local math, vector, minetest, mcl_mobs = math, vector, minetest, mcl_mobs
 local mob_class = mcl_mobs.mob_class
 
-local LOGGING_ON = minetest.settings:get_bool("mcl_logging_mobs_villager",false)
+local PATHFINDING_FAIL_THRESHOLD = 100 -- no. of ticks to fail before giving up. 20p/s. 5s helps them get through door
-local PATHFINDING = "gowp"
+local PATHFINDING_FAIL_WAIT = 30 -- how long to wait before trying to path again
-local enable_pathfinding = true
 
-local LOG_MODULE = "[Mobs]"
+local PATHFINDING = "gowp"
+
+local one_down = vector.new(0,-1,0)
+local one_up = vector.new(0,1,0)
+
+local plane_adjacents = {
+	vector.new(1,0,0),
+	vector.new(-1,0,0),
+	vector.new(0,0,1),
+	vector.new(0,0,-1),
+}
+
+local LOGGING_ON = minetest.settings:get_bool("mcl_logging_mobs_pathfinding",false)
+
+local LOG_MODULE = "[Mobs Pathfinding]"
 local function mcl_log (message)
 	if LOGGING_ON and message then
 		minetest.log(LOG_MODULE .. " " .. message)
@@ -21,7 +34,7 @@ function output_table (wp)
 end
 
 function append_paths (wp1, wp2)
-	mcl_log("Start append")
+	--mcl_log("Start append")
 	if not wp1 or not wp2 then
 		mcl_log("Cannot append wp's")
 		return
@@ -31,7 +44,7 @@ function append_paths (wp1, wp2)
 	for _,a in pairs (wp2) do
 		table.insert(wp1, a)
 	end
-	mcl_log("End append")
+	--mcl_log("End append")
 end
 
 local function output_enriched (wp_out)
@@ -43,11 +56,12 @@ local function output_enriched (wp_out)
 
 		local action =  outy["action"]
 		if action then
+			--mcl_log("Pos ".. i ..":" .. minetest.pos_to_string(outy["pos"]))
 			mcl_log("type: " .. action["type"])
 			mcl_log("action: " .. action["action"])
 			mcl_log("target: " .. minetest.pos_to_string(action["target"]))
 		end
-		mcl_log("failed attempts: " .. outy["failed_attempts"])
+		--mcl_log("failed attempts: " .. outy["failed_attempts"])
 	end
 end
 
@@ -56,34 +70,26 @@ end
 -- an action, such as to open or close a door where we know that pos requires that action
 local function generate_enriched_path(wp_in, door_open_pos, door_close_pos, cur_door_pos)
 	local wp_out = {}
+
+	-- TODO  Just pass in door position and the index before is open, the index after is close
+	local current_door_index = -1
+
 	for i, cur_pos in pairs(wp_in) do
 		local action = nil
 
-		local one_down = vector.new(0,-1,0)
 		local cur_pos_to_add = vector.add(cur_pos, one_down)
 		if door_open_pos and vector.equals (cur_pos, door_open_pos) then
 			mcl_log ("Door open match")
-			--action = {type = "door", action = "open"}
+			action = {type = "door", action = "open", target = cur_door_pos}
-			action = {}
-			action["type"] = "door"
-			action["action"] = "open"
-			action["target"] = cur_door_pos
 			cur_pos_to_add = vector.add(cur_pos, one_down)
 		elseif door_close_pos and vector.equals(cur_pos, door_close_pos) then
 			mcl_log ("Door close match")
-			--action = {type = "door", action = "closed"}
+			action = {type = "door", action = "close", target = cur_door_pos}
-			action = {}
-			action["type"] = "door"
-			action["action"] = "close"
-			action["target"] = cur_door_pos
 			cur_pos_to_add = vector.add(cur_pos, one_down)
 		elseif cur_door_pos and vector.equals(cur_pos, cur_door_pos) then
 			mcl_log("Current door pos")
+			action = {type = "door", action = "open", target = cur_door_pos}
 			cur_pos_to_add = vector.add(cur_pos, one_down)
-			action = {}
-			action["type"] = "door"
-			action["action"] = "open"
-			action["target"] = cur_door_pos
 		else
 			cur_pos_to_add = cur_pos
 			--mcl_log ("Pos doesn't match")
@@ -101,106 +107,156 @@ local function generate_enriched_path(wp_in, door_open_pos, door_close_pos, cur_
 	return wp_out
 end
 
-local plane_adjacents = {
+function mob_class:ready_to_path()
-	vector.new(1,0,0),
+	mcl_log("Check ready to path")
-	vector.new(-1,0,0),
+	if self._pf_last_failed and (os.time() - self._pf_last_failed) < PATHFINDING_FAIL_WAIT then
-	vector.new(0,0,1),
+		mcl_log("Not ready to path as last fail is less than threshold: " .. (os.time() - self._pf_last_failed))
-	vector.new(0,0,-1),
+		return false
-}
+	else
+		mcl_log("We are ready to pathfind, no previous fail or we are past threshold")
+		return true
+	end
+end
 
 -- This function is used to see if we can path. We could use to check a route, rather than making people move.
-local function calculate_path_through_door (p, t, target)
+local function calculate_path_through_door (p, cur_door_pos, t)
-	-- target is the same as t, just 1 square difference. Maybe we don't need target
+	if t then
-	mcl_log("Plot route from mob: " .. minetest.pos_to_string(p) .. ", to target: " .. minetest.pos_to_string(t))
+		mcl_log("Plot route through door from pos: " .. minetest.pos_to_string(p) .. ", to target: " .. minetest.pos_to_string(t))
+	else
+		mcl_log("Plot route through door from pos: " .. minetest.pos_to_string(p))
+	end
 
 	local enriched_path = nil
+	local wp, prospective_wp
 
-	local cur_door_pos = nil
 	local pos_closest_to_door = nil
 	local other_side_of_door = nil
 
-	--Path to door first
-	local wp = minetest.find_path(p,t,150,1,4)
-	if not wp then
-		mcl_log("No direct path. Path through door")
-
-		-- This could improve. There could be multiple doors. Check you can path from door to target first.
-		local cur_door_pos = minetest.find_node_near(target,16,{"group:door"})
 	if cur_door_pos then
 		mcl_log("Found a door near: " .. minetest.pos_to_string(cur_door_pos))
+
 		for _,v in pairs(plane_adjacents) do
 			pos_closest_to_door = vector.add(cur_door_pos,v)
+			other_side_of_door = vector.add(cur_door_pos,-v)
 
 			local n = minetest.get_node(pos_closest_to_door)
+
 			if n.name == "air" then
-					wp = minetest.find_path(p,pos_closest_to_door,150,1,4)
+				mcl_log("We have air space next to door at: " .. minetest.pos_to_string(pos_closest_to_door))
-					if wp then
+
+				prospective_wp = minetest.find_path(p,pos_closest_to_door,150,1,4)
+
+				if prospective_wp then
 					mcl_log("Found a path to next to door".. minetest.pos_to_string(pos_closest_to_door))
-						other_side_of_door = vector.add(cur_door_pos,-v)
 					mcl_log("Opposite is: ".. minetest.pos_to_string(other_side_of_door))
 
+					table.insert(prospective_wp, cur_door_pos)
+
+					if t then
+						mcl_log("We have t, lets go from door to target")
 						local wp_otherside_door_to_target = minetest.find_path(other_side_of_door,t,150,1,4)
+
 						if wp_otherside_door_to_target and #wp_otherside_door_to_target > 0 then
-							table.insert(wp, cur_door_pos)
+							append_paths (prospective_wp, wp_otherside_door_to_target)
-							append_paths (wp, wp_otherside_door_to_target)
+
-							enriched_path = generate_enriched_path(wp, pos_closest_to_door, other_side_of_door, cur_door_pos)
+							wp = prospective_wp
 							mcl_log("We have a path from outside door to target")
 						else
 							mcl_log("We cannot path from outside door to target")
 						end
-						break
 					else
-						mcl_log("This block next to door doesn't work.")
+						mcl_log("No t, just add other side of door")
-					end
+						table.insert(prospective_wp, other_side_of_door)
-				else
+						wp = prospective_wp
-					mcl_log("Block is not air, it is: ".. n.name)
 					end
 
+					if wp then
+						enriched_path = generate_enriched_path(wp, pos_closest_to_door, other_side_of_door, cur_door_pos)
+						break
+					end
+				else
+					mcl_log("Cannot path to this air block next to door.")
+				end
+			end
 		end
 	else
 		mcl_log("No door found")
 	end
-	else
-		mcl_log("We have a direct route")
-	end
 
 	if wp and not enriched_path then
+		mcl_log("Wp but not enriched")
 		enriched_path = generate_enriched_path(wp)
 	end
 	return enriched_path
 end
 
-local gopath_last = os.time()
 function mob_class:gopath(target,callback_arrived)
 	if self.state == PATHFINDING then mcl_log("Already pathfinding, don't set another until done.") return end
-
+	if not self:ready_to_path() then return end
-	if self._pf_last_failed and (os.time() - self._pf_last_failed) < 30 then
-		mcl_log("We are not ready to path as last fail is less than threshold: " .. (os.time() - self._pf_last_failed))
-		return
-	else
-		mcl_log("We are ready to pathfind, no previous fail or we are past threshold")
-	end
-
-	--if os.time() - gopath_last < 5 then
-	--	mcl_log("Not ready to path yet")
-	--	return
-	--end
-	--gopath_last = os.time()
 
 	self.order = nil
 
 	local p = self.object:get_pos()
 	local t = vector.offset(target,0,1,0)
 
-	local wp = calculate_path_through_door(p, t, target)
+	--Check direct route
+	local wp = minetest.find_path(p,t,150,1,4)
+
+	if not wp then
+		mcl_log("### No direct path. Path through door closest to target.")
+		local door_near_target = minetest.find_node_near(target, 16, {"group:door"})
+		wp = calculate_path_through_door(p, door_near_target, t)
+
+		if not wp then
+			mcl_log("### No path though door closest to target. Try door closest to origin.")
+			local door_closest = minetest.find_node_near(p, 16, {"group:door"})
+			wp = calculate_path_through_door(p, door_closest, t)
+
+			-- Path through 2 doors
+			if not wp then
+				mcl_log("### Still not wp. Need to path through 2 doors.")
+				local path_through_closest_door = calculate_path_through_door(p, door_closest)
+
+				if path_through_closest_door and #path_through_closest_door > 0 then
+					mcl_log("We have path through first door")
+					mcl_log("Number of pos in path through door: " .. tostring(#path_through_closest_door))
+
+					local pos_after_door_entry = path_through_closest_door[#path_through_closest_door]
+					if pos_after_door_entry then
+						local pos_after_door = vector.add(pos_after_door_entry["pos"], one_up)
+						mcl_log("pos_after_door: " .. minetest.pos_to_string(pos_after_door))
+						local path_after_door = calculate_path_through_door(pos_after_door, door_near_target, t)
+						if path_after_door and #path_after_door > 1 then
+							mcl_log("We have path after first door")
+							table.remove(path_after_door, 1) -- Remove duplicate
+							wp = path_through_closest_door
+							append_paths (wp, path_after_door)
+						else
+							mcl_log("Path after door is not good")
+						end
+					else
+						mcl_log("No pos after door")
+					end
+				else
+					mcl_log("Path through closest door empty or null")
+				end
+			else
+				mcl_log("ok, we have a path through 1 door")
+			end
+		end
+	else
+		wp = generate_enriched_path(wp)
+		mcl_log("We have a direct route")
+	end
+
 	if not wp then
 		mcl_log("Could not calculate path")
 		self._pf_last_failed = os.time()
-		-- Cover for a flaw in pathfind where it chooses the wrong door and gets stuck. Take a break, allow others.
+		-- If cannot path, don't immediately try again
 	end
-	--output_table(wp)
 
 	if wp and #wp > 0 then
+		--output_table(wp)
 		self._target = t
 		self.callback_arrived = callback_arrived
 		local current_location = table.remove(wp,1)
@@ -269,17 +325,7 @@ function mob_class:do_pathfind_action(action)
 	end
 end
 
-local gowp_etime = 0
-
 function mob_class:check_gowp(dtime)
-	gowp_etime = gowp_etime + dtime
-
-	-- 0.1 is optimal.
-	--less frequently = villager will get sent back after passing a point.
-	--more frequently = villager will fail points they shouldn't they just didn't get there yet
-
-	--if gowp_etime < 0.05 then return end
-	--gowp_etime = 0
 	local p = self.object:get_pos()
 
 	-- no destination
@@ -292,7 +338,7 @@ function mob_class:check_gowp(dtime)
 	-- arrived at location, finish gowp
 	local distance_to_targ = vector.distance(p,self._target)
 	--mcl_log("Distance to targ: ".. tostring(distance_to_targ))
-	if distance_to_targ < 2 then
+	if distance_to_targ < 1.8 then
 		mcl_log("Arrived at _target")
 		self.waypoints = nil
 		self._target = nil
@@ -303,6 +349,8 @@ function mob_class:check_gowp(dtime)
 		self.object:set_acceleration({x = 0, y = 0, z = 0})
 		if self.callback_arrived then return self.callback_arrived(self) end
 		return true
+	elseif not self.current_target then
+		mcl_log("Not close enough to targ: ".. tostring(distance_to_targ))
 	end
 
 	-- More pathing to be done
@@ -315,7 +363,7 @@ function mob_class:check_gowp(dtime)
 	-- 0.8 is optimal for 0.025 frequency checks and also 1... Actually. 0.8 is winning
 	-- 0.9 and 1.0 is also good. Stick with unless door open or closing issues
 	if self.waypoints and #self.waypoints > 0 and ( not self.current_target or not self.current_target["pos"] or distance_to_current_target < 0.9 ) then
-		-- We have waypoints, and no current target, or we're at it. We need a new current_target.
+		-- We have waypoints, and are at current_target or have no current target. We need a new current_target.
 		self:do_pathfind_action (self.current_target["action"])
 
 		local failed_attempts = self.current_target["failed_attempts"]
@@ -325,10 +373,11 @@ function mob_class:check_gowp(dtime)
 		self:go_to_pos(self.current_target["pos"])
 		return
 	elseif self.current_target and self.current_target["pos"] then
-		-- No waypoints left, but have current target. Potentially last waypoint to go to.
+		-- No waypoints left, but have current target and not close enough. Potentially last waypoint to go to.
+
 		self.current_target["failed_attempts"] = self.current_target["failed_attempts"] + 1
 		local failed_attempts = self.current_target["failed_attempts"]
-		if failed_attempts >= 50 then
+		if failed_attempts >= PATHFINDING_FAIL_THRESHOLD then
 			mcl_log("Failed to reach position (" .. minetest.pos_to_string(self.current_target["pos"]) .. ") too many times. Abandon route. Times tried: " .. failed_attempts)
 			self.state = "stand"
 			self.current_target = nil
@@ -348,9 +397,22 @@ function mob_class:check_gowp(dtime)
 		-- Is a little sensitive and could take 1 - 7 times. A 10 fail count might be a good exit condition.
 
 		mcl_log("We don't have waypoints or a current target. Let's try to path to target")
+
+		if self.waypoints then
+			mcl_log("WP: " .. tostring(self.waypoints))
+			mcl_log("WP num: " .. tostring(#self.waypoints))
+		else
+			mcl_log("No wp set")
+		end
+		if self.current_target then
+			mcl_log("Current target: " .. tostring(self.current_target))
+		else
+			mcl_log("No current target")
+		end
+
 		local final_wp = minetest.find_path(p,self._target,150,1,4)
 		if final_wp then
-			mcl_log("We might be able to get to target here.")
+			mcl_log("We can get to target here.")
 		--	self.waypoints = final_wp
 			self:go_to_pos(self._target)
 		else
@@ -374,9 +436,9 @@ function mob_class:check_gowp(dtime)
 			self:go_to_pos(self._current_target)
 		else
 			mcl_log("close to current target: ".. minetest.pos_to_string(self.current_target["pos"]))
+			mcl_log("target is: ".. minetest.pos_to_string(self._target))
 			self.current_target = nil
 		end
-
 		return
 	end
 end

mods/ENTITIES/mobs_mc/villager.lua

@@ -641,7 +641,7 @@ function get_activity(tod)
 	else
 		activity = "chill"
 	end
-	mcl_log("Time is " .. tod ..". Activity is: ".. activity)
+	--mcl_log("Time is " .. tod ..". Activity is: ".. activity)
 	return activity
 
 end
@@ -770,7 +770,7 @@ local function check_bed (entity)
 	local n = minetest.get_node(b)
 
 	local is_bed_bottom = string.find(n.name,"_bottom")
-	mcl_log("" .. tostring(is_bed_bottom))
+	--mcl_log("is bed bottom: " .. tostring(is_bed_bottom))
 	if n and not is_bed_bottom then
 		mcl_log("Where did my bed go?!")
 		entity._bed = nil --the stormtroopers have killed uncle owen
@@ -836,6 +836,7 @@ end
 
 local function take_bed (entity)
 	if not entity then return end
+	if not entity:ready_to_path() then return end
 
 	local p = entity.object:get_pos()
 
@@ -1059,9 +1060,9 @@ local function look_for_job(self, requested_jobsites)
 end
 
 
-
 local function get_a_job(self)
 	if self.order == WORK then self.order = nil end
+	if not self:ready_to_path() then return end
 
 	mcl_log("I'm unemployed or lost my job block and have traded. Can I get a job?")
 
@@ -1135,22 +1136,19 @@ local function validate_jobsite(self)
 end
 
 local function do_work (self)
-	--debug_trades(self)
+
-	if self.child then
+	if not self or self.child then
-		mcl_log("A child so don't send to work")
+		mcl_log("No self, or a child so don't work")
 		return
 	end
+
 	--mcl_log("Time for work")
+	local jobsite_node = retrieve_my_jobsite (self)
 
-	-- Don't try if looking_for_work, or gowp possibly
+	if jobsite_node then
-	if validate_jobsite(self) then
-		--mcl_log("My jobsite is valid. Do i need to travel?")
-
-		local jobsite2 = retrieve_my_jobsite (self)
 		local jobsite = self._jobsite
 
-		if self and jobsite2 and self._jobsite then
+		local distance_to_jobsite = vector.distance(self.object:get_pos(), jobsite)
-			local distance_to_jobsite = vector.distance(self.object:get_pos(),self._jobsite)
 		--mcl_log("Villager: ".. minetest.pos_to_string(self.object:get_pos()) ..  ", jobsite: " .. minetest.pos_to_string(self._jobsite) .. ", distance to jobsite: ".. distance_to_jobsite)
 
 		if distance_to_jobsite < 2 then
@@ -1167,7 +1165,7 @@ local function do_work (self)
 				self.order = nil
 				return
 			end
-				self:gopath(jobsite, function(self,jobsite)
+			self:gopath(jobsite, function(self, jobsite)
 				if not self then
 					--mcl_log("missing self. not good")
 					return false
@@ -1185,18 +1183,38 @@ local function do_work (self)
 			end)
 		end
 	end
-	elseif self._profession == "unemployed" or has_traded(self) then
+
-		get_a_job(self)
+end
+
+local below_vec = vector.new(0, -1, 0)
+
+local function get_ground_below_floating_object (float_pos)
+	local pos = float_pos
+	repeat
+		mcl_log("Current pos: " .. minetest.pos_to_string(pos))
+		pos = vector.add(pos, below_vec)
+		local node = minetest.get_node(pos)
+		mcl_log("First non air materials: ".. tostring(node.name))
+	until node.name ~= "air"
+
+	-- If pos is 1 below float_pos, then just return float_pos as there is no air below it
+	if pos.y == float_pos.y - 1 then
+		--mcl_log("pos is only 1 lower than float pos so no air below")
+		return float_pos
+	else
+		--mcl_log("pos is more than 1 lower than float pos so air is below")
+		return pos
 	end
+
+	return pos
 end
 
 local function go_to_town_bell(self)
-	if self.order == GATHERING then
+	if self.order == GATHERING then return
-		mcl_log("Already gathering")
+	else mcl_log("Current order" .. self.order) end
-		return
+
-	else
+	if not self:ready_to_path() then return end
-		mcl_log("Current order" .. self.order)
+
-	end
 	mcl_log("Go to town bell")
 
 	local looking_for_type={}
@@ -1208,8 +1226,9 @@ local function go_to_town_bell(self)
 	--Ideally should check for closest available. It'll make pathing easier.
 	for _,n in pairs(nn) do
 		mcl_log("Found bell")
+		local target_point = get_ground_below_floating_object(n)
 
-		local gp = self:gopath(n,function(self)
+		local gp = self:gopath(target_point,function(self)
 			if self then
 				self.order = GATHERING
 				mcl_log("Callback has a self")
@@ -1277,22 +1296,45 @@ local function validate_bed(self)
 end
 
 local function do_activity (self)
-	-- Maybe just check we're pathfinding first?
+
 	if self.following then
 		mcl_log("Following, so do not do activity.")
 		return
 	end
+	if self.state == PATHFINDING then
+		mcl_log("Pathfinding, so do not do activity.")
+		return
+	end
 
-	if not validate_bed(self) and self.state ~= PATHFINDING then
+	local jobsite_valid = false
+
+	if not mcl_beds.is_night() then
 		if self.order == SLEEP then self.order = nil end
-		mcl_log("Villager has no bed. Currently at location: "..minetest.pos_to_string(self.object:get_pos()))
+
+		if not validate_jobsite(self) then
+			--debug_trades(self)
+			if self._profession == "unemployed" or has_traded(self) then
+				get_a_job(self)
+				return
+			end
+		else
+			jobsite_valid = true
+			--mcl_log("My jobsite is valid. Do i need to travel?")
+		end
+	else
+		if self.order == WORK then self.order = nil end
+
+		if not validate_bed(self) then
+			if self.order == SLEEP then self.order = nil end
+			mcl_log("Villager at this location has no bed: " .. minetest.pos_to_string(self.object:get_pos()))
 			take_bed (self)
 		end
+	end
 
 	-- Only check in day or during thunderstorm but wandered_too_far code won't work
 	local wandered_too_far = false
 	if check_bed (self) then
-		wandered_too_far = ( self.state ~= PATHFINDING ) and (vector.distance(self.object:get_pos(),self._bed) > 50 )
+		wandered_too_far = vector.distance(self.object:get_pos(),self._bed) > 50
 	end
 
 	if wandered_too_far  then
@@ -1300,7 +1342,7 @@ local function do_activity (self)
 		go_home(self, false)
 	elseif get_activity() == SLEEP then
 		go_home(self, true)
-	elseif get_activity() == WORK then
+	elseif get_activity() == WORK and jobsite_valid then
 		do_work(self)
 	elseif get_activity() == GATHERING then
 		go_to_town_bell(self)
@@ -1309,13 +1351,6 @@ local function do_activity (self)
 		self.order = nil
 	end
 
-	-- Daytime is work and play time
-	if not mcl_beds.is_night() then
-		if self.order == SLEEP then self.order = nil end
-	else
-		if self.order == WORK then self.order = nil end
-	end
-
 end
 
 local function update_max_tradenum(self)