diff --git a/lua/d3bot/sv_benchmark.lua b/lua/d3bot/sv_benchmark.lua
index 8de69f75..0557c45e 100644
--- a/lua/d3bot/sv_benchmark.lua
+++ b/lua/d3bot/sv_benchmark.lua
@@ -19,6 +19,33 @@ local function benchmarkGetNearestNodeOrNil()
 	return (endTime - startTime) / iterations, displayName
 end
 
+---This will benchmark GetBestMeshPathOrNil on the current navmesh.
+---Only works correctly if zs_infected_square_v1 is loaded.
+---@return number runtime
+---@return string displayName
+local function benchmarkGetBestMeshPathOrNil()
+	local startTime = SysTime()
+	local iterations = 1
+	local displayName = "GetBestMeshPathOrNil"
+
+	local pathCostFunction = function(node, linkedNode, link)
+		local linkMetadata = D3bot.LinkMetadata[link]
+		local linkPenalty = linkMetadata and linkMetadata.ZombieDeathCost or 0
+		return linkPenalty * 1
+	end
+
+	local abilities = {Walk = true}
+
+	local navMesh = D3bot.MapNavMesh
+	local startNode = navMesh.NodeById[32]
+	local endNode = navMesh.NodeById[866]
+
+	local path = D3bot.GetBestMeshPathOrNil(startNode, endNode, pathCostFunction, nil, abilities)
+
+	local endTime = SysTime()
+	return (endTime - startTime) / iterations, displayName
+end
+
 ---Runs a benchmark and prints the results.
 ---@param func function
 local function test(func)
@@ -29,3 +56,4 @@ end
 
 -- Run tests.
 --test(benchmarkGetNearestNodeOrNil)
+--test(benchmarkGetBestMeshPathOrNil)
diff --git a/lua/d3bot/sv_path.lua b/lua/d3bot/sv_path.lua
index 04cf0d7a..5f1fa438 100644
--- a/lua/d3bot/sv_path.lua
+++ b/lua/d3bot/sv_path.lua
@@ -1,60 +1,84 @@
+local mathMax = math.max
+local mathHuge = math.huge
+local tableInsert = table.insert
+
+---Returns the best path (or nil if there is no possible path) between startNode and endNode.
+---@param startNode any
+---@param endNode any
+---@param pathCostFunction function|nil
+---@param heuristicCostFunction function|nil
+---@param abilities any|nil
+---@return any|nil
 function D3bot.GetBestMeshPathOrNil(startNode, endNode, pathCostFunction, heuristicCostFunction, abilities)
-	-- See https://en.wikipedia.org/wiki/A*_search_algorithm
-	
+	-- See: https://en.wikipedia.org/wiki/A*_search_algorithm
+
+	-- Benchmarks:
+	-- Navmesh: zs_infected_square_v1
+	-- CPU: Intel(R) Core(TM) i5-10600K CPU @ 4.10GHz
+	-- 2020-06-23 (769f186): ~4.95 ms per call.
+	-- 2022-09-24 (       ): ~4.45 ms per call.
+
 	local minimalTotalPathCostByNode = {}
 	local minimalPathCostByNode = { [startNode] = 0 }
-	
+
 	local entranceByNode = {}
-	
-	local evaluationNodeQueue = D3bot.NewSortedQueue(function(nodeA, nodeB) return (minimalTotalPathCostByNode[nodeA] or math.huge) > (minimalTotalPathCostByNode[nodeB] or math.huge) end)
+
+	local evaluationNodeQueue = D3bot.NewSortedQueue(function(nodeA, nodeB)
+		return (minimalTotalPathCostByNode[nodeA] or mathHuge) > (minimalTotalPathCostByNode[nodeB] or mathHuge)
+	end)
 	evaluationNodeQueue:Enqueue(startNode)
-	
+
 	while true do
 		local node = evaluationNodeQueue:Dequeue()
 		if not node then return end
-		
+
 		if node == endNode then
 			local path = { node }
 			while true do
 				node = entranceByNode[node]
 				if not node then break end
-				table.insert(path, 1, node)
+				tableInsert(path, 1, node)
 			end
 			return path
 		end
-		
+
 		for linkedNode, link in pairs(node.LinkByLinkedNode) do
-			
+			local linkedNodeParams = linkedNode.Params
+			local linkParams = link.Params
+
 			local blocked = false
-			if linkedNode.Params.Condition == "Unblocked" or linkedNode.Params.Condition == "Blocked" then
+			if linkedNodeParams.Condition == "Unblocked" or linkedNodeParams.Condition == "Blocked" then
 				local ents = ents.FindInBox(linkedNode.Pos + D3bot.NodeBlocking.mins, linkedNode.Pos + D3bot.NodeBlocking.maxs)
 				for _, ent in ipairs(ents) do
 					if D3bot.NodeBlocking.classes[ent:GetClass()] then blocked = true; break end
 				end
-				if linkedNode.Params.Condition == "Blocked" then blocked = not blocked end
+				if linkedNodeParams.Condition == "Blocked" then blocked = not blocked end
 			end
 
-			-- Block pathing if the wave is outside of the interval [BlockBeforeWave, BlockAfterWave]
-			if linkedNode.Params.BlockBeforeWave and tonumber(linkedNode.Params.BlockBeforeWave) then
-				if GAMEMODE:GetWave() < tonumber(linkedNode.Params.BlockBeforeWave) then blocked = true end
+			-- Block pathing if the wave is outside of the interval [BlockBeforeWave, BlockAfterWave].
+			if linkedNodeParams.BlockBeforeWave and tonumber(linkedNodeParams.BlockBeforeWave) then
+				if GAMEMODE:GetWave() < tonumber(linkedNodeParams.BlockBeforeWave) then blocked = true end
 			end
-			if linkedNode.Params.BlockAfterWave and tonumber(linkedNode.Params.BlockAfterWave) then
-				if GAMEMODE:GetWave() > tonumber(linkedNode.Params.BlockAfterWave) then blocked = true end
+			if linkedNodeParams.BlockAfterWave and tonumber(linkedNodeParams.BlockAfterWave) then
+				if GAMEMODE:GetWave() > tonumber(linkedNodeParams.BlockAfterWave) then blocked = true end
 				-- TODO: Invert logic when BlockBeforeWave > BlockAfterWave. This way it's possible to describe a interval of blocked waves, instead of unblocked waves
 			end
-			
+
 			local able = true
-			if link.Params.Walking == "Needed" and abilities and not abilities.Walk then able = false end
-			if link.Params.Pouncing == "Needed" and abilities and not abilities.Pounce then able = false end
-			if linkedNode.Params.Climbing == "Needed" and abilities and not abilities.Climb then able = false end
+			if abilities then
+				if linkParams.Walking == "Needed" and not abilities.Walk then able = false end
+				if linkParams.Pouncing == "Needed" and not abilities.Pounce then able = false end
+				if linkedNodeParams.Climbing == "Needed" and not abilities.Climb then able = false end
+			end
 
-			if able and not blocked and not (link.Params.Direction == "Forward" and link.Nodes[2] == node) and not (link.Params.Direction == "Backward" and link.Nodes[1] == node) then
-				local linkedNodePathCost = minimalPathCostByNode[node] + math.max(node.Pos:Distance(linkedNode.Pos) + (linkedNode.Params.Cost or 0) + (link.Params.Cost or 0) + (pathCostFunction and pathCostFunction(node, linkedNode, link) or 0), 0) -- Prevent negative change of the link costs, otherwise it will get stuck decreasing forever
-				if linkedNodePathCost < (minimalPathCostByNode[linkedNode] or math.huge) then
+			if able and not blocked and not (linkParams.Direction == "Forward" and link.Nodes[2] == node) and
+				not (linkParams.Direction == "Backward" and link.Nodes[1] == node) then
+				local linkedNodePathCost = minimalPathCostByNode[node] + mathMax(node.Pos:Distance(linkedNode.Pos) + (linkedNodeParams.Cost or 0) + (linkParams.Cost or 0) + (pathCostFunction and pathCostFunction(node, linkedNode, link) or 0), 0) -- Prevent negative change of the link costs, otherwise it will get stuck decreasing forever.
+				if linkedNodePathCost < (minimalPathCostByNode[linkedNode] or mathHuge) then
 					entranceByNode[linkedNode] = node
 					minimalPathCostByNode[linkedNode] = linkedNodePathCost
 					local heuristic = (heuristicCostFunction and heuristicCostFunction(linkedNode) or 0)
-					minimalTotalPathCostByNode[linkedNode] = linkedNodePathCost + heuristic + linkedNode.Pos:Distance(endNode.Pos) -- Negative costs are allowed here
+					minimalTotalPathCostByNode[linkedNode] = linkedNodePathCost + heuristic + linkedNode.Pos:Distance(endNode.Pos) -- Negative costs are allowed here.
 					evaluationNodeQueue:Enqueue(linkedNode)
 				end
 			end