Add new LVM Pathfinder
This commit is contained in:
ElCeejo
parent
021bd7f5a0
commit
7fa1c522eb
|
|
@ -2,6 +2,7 @@ max_line_length = 120
|
|||
|
||||
globals = {
|
||||
"minetest",
|
||||
"VoxelArea",
|
||||
"creatura",
|
||||
}
|
||||
|
||||
|
|
|
|||
61
methods.lua
61
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)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
322
pathfinder.lua
322
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* --
|
||||
------------
|
||||
|
|
|
|||
Loading…
Reference in New Issue