420 lines
16 KiB
Lua
420 lines
16 KiB
Lua
-- path.lua
|
|
-- Functions for pathpredicting, put in a separate file.
|
|
|
|
-- Naming conventions:
|
|
-- 'index' - An index of the train.path table.
|
|
-- 'offset' - A value in meters that determines how far on the path to walk relative to a certain index
|
|
-- 'n' - Referring or pointing towards the 'next' path item, the one with index+1
|
|
-- 'p' - Referring or pointing towards the 'prev' path item, the one with index-1
|
|
-- 'f' - Referring to the positive end of the path (the end with the higher index)
|
|
-- 'b' - Referring to the negative end of the path (the end with the lower index)
|
|
|
|
-- New path structure of trains:
|
|
--Tables:
|
|
-- path - path positions. 'indices' are relative to this. At the moment, at.round_vector_floor_y(path[i])
|
|
-- is the node this item corresponds to, however, this will change in the future.
|
|
-- path_node - (reserved)
|
|
-- path_cn - Connid of the current node that points towards path[i+1]
|
|
-- path_cp - Connid of the current node that points towards path[i-1]
|
|
-- When the day comes on that path!=node, these will only be set if this index represents a transition between rail nodes
|
|
-- path_dist - The total distance of this path element from path element 0
|
|
-- path_dir - The direction of this path item's transition to the next path item, which is the angle of conns[path_cn[i]].c
|
|
-- path_speed- Populated by the LZB system. The maximum speed (velocity) permitted in the moment this path item is passed.
|
|
-- (this saves brake distance calculations every step to determine LZB control). nil means no limit.
|
|
--Variables:
|
|
-- path_ext_f/b - how far path[i] is set
|
|
-- path_trk_f/b - how far the path extends along a track. beyond those values, paths are generated in a straight line.
|
|
-- path_req_f/b - how far path items were requested in the last step
|
|
--
|
|
--Distance and index:
|
|
-- There is an important difference between the path index and the actual distance on the track: The distance between two path items can be larger than 1,
|
|
-- but the corresponding index increment is still 1.
|
|
-- Indexes in advtrains can be fractional values. If they are, it means that the actual position is interpolated between the 2 adjacent path items.
|
|
-- If you need to proceed along the path by a specific actual distance, it does NOT work to simply add it to the index. You should use the path_get_index_by_offset() function.
|
|
|
|
-- creates the path data structure, reconstructing the train from a position and a connid
|
|
-- Important! train.drives_on must exist while calling this method
|
|
-- returns: true - successful
|
|
-- nil - node not yet available/unloaded, please wait
|
|
-- false - node definitely gone, remove train
|
|
function advtrains.path_create(train, pos, connid, rel_index)
|
|
local posr = advtrains.round_vector_floor_y(pos)
|
|
local node_ok, conns, rhe = advtrains.get_rail_info_at(pos, train.drives_on)
|
|
if not node_ok then
|
|
return node_ok
|
|
end
|
|
local mconnid = advtrains.get_matching_conn(connid, #conns)
|
|
train.index = rel_index
|
|
train.path = { [0] = { x=posr.x, y=posr.y+rhe, z=posr.z } }
|
|
train.path_cn = { [0] = connid }
|
|
train.path_cp = { [0] = mconnid }
|
|
train.path_dist = { [0] = 0 }
|
|
|
|
train.path_dir = {
|
|
[0] = advtrains.conn_angle_median(conns[mconnid].c, conns[connid].c)
|
|
}
|
|
|
|
train.path_speed = { }
|
|
|
|
train.path_ext_f=0
|
|
train.path_ext_b=0
|
|
train.path_trk_f=0
|
|
train.path_trk_b=0
|
|
train.path_req_f=0
|
|
train.path_req_b=0
|
|
|
|
advtrains.occ.set_item(train.id, posr, 0)
|
|
return true
|
|
end
|
|
|
|
-- Sets position and connid to properly restore after a crash, e.g. in order
|
|
-- to save the train or to invalidate its path
|
|
-- Assumes that the train is in clean state
|
|
-- if invert ist true, setrestore will use the end index
|
|
function advtrains.path_setrestore(train, invert)
|
|
local idx = train.index
|
|
if invert then
|
|
idx = train.end_index
|
|
end
|
|
|
|
local pos, connid, frac = advtrains.path_getrestore(train, idx, invert, true)
|
|
|
|
train.last_pos = pos
|
|
train.last_connid = connid
|
|
train.last_frac = frac
|
|
end
|
|
-- Get restore position, connid and frac (in this order) for a train that will originate at the passed index
|
|
-- If invert is set, it will return path_cp and multiply frac by -1, in order to reverse the train there.
|
|
function advtrains.path_getrestore(train, index, invert)
|
|
local idx = index
|
|
local cns = train.path_cn
|
|
|
|
if invert then
|
|
cns = train.path_cp
|
|
end
|
|
|
|
local fli = atfloor(index)
|
|
advtrains.path_get(train, fli)
|
|
if fli > train.path_trk_f then
|
|
fli = train.path_trk_f
|
|
end
|
|
if fli < train.path_trk_b then
|
|
fli = train.path_trk_b
|
|
end
|
|
return advtrains.path_get(train, fli),
|
|
cns[fli],
|
|
(idx - fli) * (invert and -1 or 1)
|
|
end
|
|
|
|
-- Invalidates a path
|
|
-- this is supposed to clear stuff from the occupation tables
|
|
-- This function throws a warning whenever any code calls it while the train steps are run, since that must not happen.
|
|
-- The ignore_lock parameter can be used to ignore this, however, it should then be accompanied by a call to train_ensure_init
|
|
-- before returning from the calling function.
|
|
function advtrains.path_invalidate(train, ignore_lock)
|
|
if advtrains.lock_path_inval and not ignore_lock then
|
|
atwarn("Train ",train.train_id,": Illegal path invalidation has occured during train step:")
|
|
atwarn(debug.traceback())
|
|
end
|
|
|
|
if train.path then
|
|
for i,p in pairs(train.path) do
|
|
advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(p))
|
|
end
|
|
end
|
|
train.path = nil
|
|
train.path_dist = nil
|
|
train.path_cp = nil
|
|
train.path_cn = nil
|
|
train.path_dir = nil
|
|
train.path_speed = nil
|
|
train.path_ext_f=0
|
|
train.path_ext_b=0
|
|
train.path_trk_f=0
|
|
train.path_trk_b=0
|
|
train.path_req_f=0
|
|
train.path_req_b=0
|
|
|
|
train.dirty = true
|
|
--atdebug(train.id, "Path invalidated")
|
|
end
|
|
|
|
-- Keeps the path intact, but invalidates all path nodes from the specified index (inclusive)
|
|
-- onwards. This has the advantage that we don't need to recalculate the whole path, and we can do it synchronously.
|
|
function advtrains.path_invalidate_ahead(train, start_idx, ignore_when_passed)
|
|
if not train.path then
|
|
-- the path wasn't even initialized. Nothing to do
|
|
return
|
|
end
|
|
|
|
local idx = atfloor(start_idx)
|
|
--atdebug("Invalidate_ahead:",train.id,"start_index",start_idx,"cur_idx",train.index)
|
|
|
|
if(idx <= train.index - 0.5) then
|
|
if ignore_when_passed then
|
|
--atdebug("ignored passed")
|
|
return
|
|
end
|
|
advtrains.path_print(train, atwarn)
|
|
error("Train "+train.id+": Cannot path_invalidate_ahead start_idx="+idx+" as train has already passed!")
|
|
end
|
|
|
|
-- leave current node in path, it won't change. What might change is the path onward from here (e.g. switch)
|
|
local i = idx + 1
|
|
while train.path[i] do
|
|
advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(train.path[i]))
|
|
i = i+1
|
|
end
|
|
train.path_ext_f=idx
|
|
train.path_trk_f=math.min(idx, train.path_trk_f)
|
|
|
|
-- callbacks called anyway for current node, because of LZB
|
|
advtrains.run_callbacks_invahead(train.id, train, idx)
|
|
end
|
|
|
|
-- Prints a path using the passed print function
|
|
-- This function should be 'atprint', 'atlog', 'atwarn' or 'atdebug', because it needs to use print_concat_table
|
|
function advtrains.path_print(train, printf)
|
|
printf("path_print: tid =",train.id," index =",train.index," end_index =",train.end_index," vel =",train.velocity)
|
|
if not train.path then
|
|
printf("path_print: Path is invalidated/inexistant.")
|
|
return
|
|
end
|
|
printf("i: CP Position Dir CN Dist Speed")
|
|
for i = train.path_ext_b, train.path_ext_f do
|
|
if i==train.path_trk_b then
|
|
printf("--Back on-track border here--")
|
|
end
|
|
printf(i,": ",train.path_cp[i]," ",train.path[i]," ",train.path_dir[i]," ",train.path_cn[i]," ",train.path_dist[i]," ",train.path_speed[i])
|
|
if i==train.path_trk_f then
|
|
printf("--Front on-track border here--")
|
|
end
|
|
end
|
|
end
|
|
|
|
-- Function to get path entry at a position. This function will automatically calculate more of the path when required.
|
|
-- returns: pos, on_track
|
|
function advtrains.path_get(train, index)
|
|
if not train.path then
|
|
error("For train "..train.id..": path_get called but there's no path set yet!")
|
|
end
|
|
if index ~= atfloor(index) then
|
|
error("For train "..train.id..": Called path_get() but index="..index.." is not a round number")
|
|
end
|
|
|
|
local pef = train.path_ext_f
|
|
-- generate forward (front of train, positive)
|
|
while index > pef do
|
|
local pos = train.path[pef]
|
|
local connid = train.path_cn[pef]
|
|
local node_ok, this_conns, adj_pos, adj_connid, conn_idx, nextrail_y, next_conns
|
|
if pef == train.path_trk_f then
|
|
node_ok, this_conns = advtrains.get_rail_info_at(pos)
|
|
if not node_ok then error("For train "..train.id..": Path item "..pef.." on-track but not a valid node!") end
|
|
adj_pos, adj_connid, conn_idx, nextrail_y, next_conns = advtrains.get_adjacent_rail(pos, this_conns, connid, train.drives_on)
|
|
end
|
|
pef = pef + 1
|
|
if adj_pos then
|
|
advtrains.occ.set_item(train.id, adj_pos, pef)
|
|
|
|
-- If we have split points, notify accordingly
|
|
local mconnid = advtrains.get_matching_conn(adj_connid, #next_conns)
|
|
if #next_conns==3 and adj_connid==1 and train.points_split and train.points_split[advtrains.encode_pos(adj_pos)] then
|
|
--atdebug(id,"has split points restored at",adj_pos)
|
|
mconnid = 3
|
|
end
|
|
|
|
adj_pos.y = adj_pos.y + nextrail_y
|
|
train.path_cp[pef] = adj_connid
|
|
train.path_cn[pef] = mconnid
|
|
train.path_dir[pef] = advtrains.conn_angle_median(next_conns[adj_connid].c, next_conns[mconnid].c)
|
|
train.path_trk_f = pef
|
|
else
|
|
-- off-track fallback behavior
|
|
adj_pos = advtrains.pos_add_angle(pos, train.path_dir[pef-1])
|
|
--atdebug("Offtrack overgenerating(front) at",adj_pos,"index",peb,"trkf",train.path_trk_f)
|
|
train.path_dir[pef] = train.path_dir[pef-1]
|
|
end
|
|
train.path[pef] = adj_pos
|
|
train.path_dist[pef] = train.path_dist[pef-1] + vector.distance(pos, adj_pos)
|
|
end
|
|
train.path_ext_f = pef
|
|
|
|
|
|
local peb = train.path_ext_b
|
|
-- generate backward (back of train, negative)
|
|
while index < peb do
|
|
local pos = train.path[peb]
|
|
local connid = train.path_cp[peb]
|
|
local node_ok, this_conns, adj_pos, adj_connid, conn_idx, nextrail_y, next_conns
|
|
if peb == train.path_trk_b then
|
|
node_ok, this_conns = advtrains.get_rail_info_at(pos)
|
|
if not node_ok then error("For train "..train.id..": Path item "..peb.." on-track but not a valid node!") end
|
|
adj_pos, adj_connid, conn_idx, nextrail_y, next_conns = advtrains.get_adjacent_rail(pos, this_conns, connid, train.drives_on)
|
|
end
|
|
peb = peb - 1
|
|
if adj_pos then
|
|
advtrains.occ.set_item(train.id, adj_pos, peb)
|
|
|
|
-- If we have split points, notify accordingly
|
|
local mconnid = advtrains.get_matching_conn(adj_connid, #next_conns)
|
|
if #next_conns==3 and adj_connid==1 and train.points_split and train.points_split[advtrains.encode_pos(adj_pos)] then
|
|
-- atdebug(id,"has split points restored at",adj_pos)
|
|
mconnid = 3
|
|
end
|
|
|
|
adj_pos.y = adj_pos.y + nextrail_y
|
|
train.path_cn[peb] = adj_connid
|
|
train.path_cp[peb] = mconnid
|
|
train.path_dir[peb] = advtrains.conn_angle_median(next_conns[mconnid].c, next_conns[adj_connid].c)
|
|
train.path_trk_b = peb
|
|
else
|
|
-- off-track fallback behavior
|
|
adj_pos = advtrains.pos_add_angle(pos, train.path_dir[peb+1] + math.pi)
|
|
--atdebug("Offtrack overgenerating(back) at",adj_pos,"index",peb,"trkb",train.path_trk_b)
|
|
train.path_dir[peb] = train.path_dir[peb+1]
|
|
end
|
|
train.path[peb] = adj_pos
|
|
train.path_dist[peb] = train.path_dist[peb+1] - vector.distance(pos, adj_pos)
|
|
end
|
|
train.path_ext_b = peb
|
|
|
|
if index < train.path_req_b then
|
|
train.path_req_b = index
|
|
end
|
|
if index > train.path_req_f then
|
|
train.path_req_f = index
|
|
end
|
|
|
|
return train.path[index], (index<=train.path_trk_f and index>=train.path_trk_b)
|
|
|
|
end
|
|
|
|
-- interpolated position to fractional index given, and angle based on path_dir
|
|
-- returns: pos, angle(yaw), p_floor, p_ceil
|
|
function advtrains.path_get_interpolated(train, index)
|
|
local i_floor = atfloor(index)
|
|
local i_ceil = i_floor + 1
|
|
local frac = index - i_floor
|
|
local p_floor = advtrains.path_get(train, i_floor)
|
|
local p_ceil = advtrains.path_get(train, i_ceil)
|
|
-- Note: minimal code duplication to path_get_adjacent, for performance
|
|
|
|
local a_floor = train.path_dir[i_floor]
|
|
local a_ceil = train.path_dir[i_ceil]
|
|
|
|
local ang = advtrains.minAngleDiffRad(a_floor, a_ceil)
|
|
|
|
return vector.add(p_floor, vector.multiply(vector.subtract(p_ceil, p_floor), frac)), (a_floor + frac * ang)%(2*math.pi), p_floor, p_ceil
|
|
end
|
|
-- returns the 2 path positions directly adjacent to index and the fraction on how to interpolate between them
|
|
-- returns: pos_floor, pos_ceil, fraction
|
|
function advtrains.path_get_adjacent(train, index)
|
|
local i_floor = atfloor(index)
|
|
local i_ceil = i_floor + 1
|
|
local frac = index - i_floor
|
|
local p_floor = advtrains.path_get(train, i_floor)
|
|
local p_ceil = advtrains.path_get(train, i_ceil)
|
|
return p_floor, p_ceil, frac
|
|
end
|
|
|
|
local function n_interpolate(s, e, f)
|
|
return s + (e-s)*f
|
|
end
|
|
|
|
-- This function determines the index resulting from moving along the path by 'offset' meters
|
|
-- starting from 'index'. See also the comment on the top of the file.
|
|
function advtrains.path_get_index_by_offset(train, index, offset)
|
|
local advtrains_path_get = advtrains.path_get
|
|
|
|
-- Step 1: determine my current absolute pos on the path
|
|
local start_index_f = atfloor(index)
|
|
local end_index_f = start_index_f + 1
|
|
local c_idx = atfloor(index + offset)
|
|
local c_idx_f = c_idx + 1
|
|
|
|
local frac = index - start_index_f
|
|
|
|
advtrains_path_get(train, math.min(start_index_f, end_index_f, c_idx, c_idx_f))
|
|
advtrains_path_get(train, math.max(start_index_f, end_index_f, c_idx, c_idx_f))
|
|
|
|
local dist1, dist2 = train.path_dist[start_index_f], train.path_dist[start_index_f+1]
|
|
local start_dist = dist1 + (dist2-dist1)*frac
|
|
|
|
-- Step 2: determine the total end distance and estimate the index we'd come out
|
|
local end_dist = start_dist + offset
|
|
|
|
local c_idx = atfloor(index + offset)
|
|
|
|
-- Step 3: move forward/backward to find real index
|
|
-- We assume here that the distance between 2 path items is never smaller than 1.
|
|
-- Our estimated index is therefore either exact or too far over, and we're going to go back
|
|
-- towards the origin. It is therefore sufficient to query path_get a single time
|
|
|
|
-- How we'll adjust c_idx
|
|
-- Desired position: -------#------
|
|
-- Path items : --|--|--|--|--
|
|
-- c_idx : ^
|
|
|
|
while train.path_dist[c_idx] < end_dist do
|
|
c_idx = c_idx + 1
|
|
end
|
|
|
|
while train.path_dist[c_idx] > end_dist do
|
|
c_idx = c_idx - 1
|
|
end
|
|
|
|
-- Step 4: now c_idx points to the place shown above. Find out the fractional part.
|
|
|
|
dist1, dist2 = train.path_dist[c_idx], train.path_dist[c_idx+1]
|
|
|
|
frac = (end_dist - dist1) / (dist2 - dist1)
|
|
|
|
assert(frac>=0 and frac<1, frac)
|
|
|
|
return c_idx + frac
|
|
end
|
|
|
|
local PATH_CLEAR_KEEP = 4
|
|
|
|
function advtrains.path_clear_unused(train)
|
|
local i
|
|
for i = train.path_ext_b, train.path_req_b - PATH_CLEAR_KEEP do
|
|
advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(train.path[i]))
|
|
train.path[i] = nil
|
|
train.path_dist[i-1] = nil
|
|
train.path_cp[i] = nil
|
|
train.path_cn[i] = nil
|
|
train.path_dir[i] = nil
|
|
train.path_ext_b = i + 1
|
|
end
|
|
|
|
--[[ Why exactly are we clearing path from the front? This doesn't make sense!
|
|
for i = train.path_ext_f,train.path_req_f + PATH_CLEAR_KEEP,-1 do
|
|
advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(train.path[i]))
|
|
train.path[i] = nil
|
|
train.path_dist[i] = nil
|
|
train.path_cp[i] = nil
|
|
train.path_cn[i] = nil
|
|
train.path_dir[i+1] = nil
|
|
train.path_ext_f = i - 1
|
|
end ]]
|
|
train.path_trk_b = math.max(train.path_trk_b, train.path_ext_b)
|
|
--train.path_trk_f = math.min(train.path_trk_f, train.path_ext_f)
|
|
|
|
train.path_req_f = math.ceil(train.index)
|
|
train.path_req_b = math.floor(train.end_index or train.index)
|
|
end
|
|
|
|
-- Scan the path of the train for position, without querying the occupation table
|
|
-- returns index, or nil if pos is not on the path
|
|
function advtrains.path_lookup(train, pos)
|
|
local cp = advtrains.round_vector_floor_y(pos)
|
|
for i = train.path_ext_b, train.path_ext_f do
|
|
if vector.equals(advtrains.round_vector_floor_y(train.path[i]), cp) then
|
|
return i
|
|
end
|
|
end
|
|
return nil
|
|
end
|