From be22775cdbefa54fa4047d32428ce7d80c15bc5b Mon Sep 17 00:00:00 2001 From: ywang Date: Thu, 5 Dec 2019 22:23:13 +0100 Subject: More accurate train logic, but still buggy --- advtrains/helpers.lua | 29 ++++---- advtrains/lzb.lua | 172 +++++++++++++++++++++++++---------------------- advtrains/trainlogic.lua | 78 ++++++++------------- 3 files changed, 138 insertions(+), 141 deletions(-) diff --git a/advtrains/helpers.lua b/advtrains/helpers.lua index e04991e..a2eabb5 100644 --- a/advtrains/helpers.lua +++ b/advtrains/helpers.lua @@ -79,7 +79,7 @@ function advtrains.yawToDirection(yaw, conn1, conn2) local yaw2 = advtrains.dir_to_angle(conn2) local adiff1 = advtrains.minAngleDiffRad(yaw, yaw1) local adiff2 = advtrains.minAngleDiffRad(yaw, yaw2) - + if math.abs(adiff2)=1 do conn_y = conn_y - 1 adj_pos.y = adj_pos.y + 1 end - + local nextnode_ok, nextconns, nextrail_y=advtrains.get_rail_info_at(adj_pos, drives_on) if not nextnode_ok then adj_pos.y = adj_pos.y - 1 @@ -398,6 +398,13 @@ function advtrains.decode_pos(pts) return vector.new(dec(strx), dec(stry), dec(strz)) end +function advtrains.solve_quadratic_equation(a, b, c) + if not (a and b and c) then return nil end + local delta = (b*b - 4*a*c) + if delta < 0 then return nil end + return {((-b+math.sqrt(delta))/2/a),((-b-math.sqrt(delta))/2/a)} +end + --[[ Benchmarking code local tdt = {} local tlt = {} @@ -435,8 +442,6 @@ end atdebug("pts",os.clock()-t1,"s") --Results: ---2018-11-29 16:57:08: ACTION[Main]: [advtrains]endec 1.786451 s ---2018-11-29 16:57:10: ACTION[Main]: [advtrains]pts 2.566377 s +--2018-11-29 16:57:08: ACTION[Main]: [advtrains]endec 1.786451 s +--2018-11-29 16:57:10: ACTION[Main]: [advtrains]pts 2.566377 s ]] - - diff --git a/advtrains/lzb.lua b/advtrains/lzb.lua index a34c2de..3961438 100644 --- a/advtrains/lzb.lua +++ b/advtrains/lzb.lua @@ -83,80 +83,104 @@ local function look_ahead(id, train) end --[[ -Distance needed to accelerate from v0 to v1 with constant acceleration a: - - v1 - v0 a / v1 - v0 \ 2 v1^2 - v0^2 -s = v0 * ------- + - * | ------- | = ----------- - a 2 \ a / 2*a +The .i element is the index at which LZB overrides the train control with the +lever specified by the index value. The .v element is the speed at which the +train control is taken over by LZB with the lever specified by the index. The .t +element calculates the time needed for the train to reach the point where the +control is taken over by LZB with the lever specified by the index. Unintialized +.v and .t values indicate that the train has passed the point with the +corresponding index. Note that thhe 0th item contains the data related to the +LZB point itself, and not related to the emergency brake. ]] - -local function apply_control(id, train) - local lzb = train.lzb - - local i = 1 - while i<=#lzb.oncoming do - if lzb.oncoming[i].idx < train.index then - local ent = lzb.oncoming[i] - if ent.fun then - ent.fun(ent.pos, id, train, ent.idx, ent.spd, lzb.data) - end - - table.remove(lzb.oncoming, i) +function advtrains.lzb_map_entry(train, lzb) + local ret = {[0]={},[1]={},[2]={},[3]={}} + if (not train) or (not lzb) then return ret end + local ti = train.index + local v0 = train.velocity + local v1 = lzb.spd + local a = advtrains.get_acceleration(train, train.lever) + local s = (v1*v1-v0*v0)/2/advtrains.get_acceleration(train, 1) + if v0 > 3 then s = s + params.ADD_FAST + elseif v0 <=0 then s = s + params.ADD_STAND + else s = s + params.ADD_SLOW + end + ret[0].i = lzb.idx + ret[1].i = advtrains.path_get_index_by_offset(train, ret[0].i, -s) + ret[2].i = advtrains.path_get_index_by_offset(train, ret[1].i, -params.ZONE_ROLL) + ret[3].i = advtrains.path_get_index_by_offset(train, ret[2].i, -params.ZONE_HOLD) + if a == 0 then ret[3].t = (ret[3].i)/v0 + else + ret[3].t = advtrains.solve_quadratic_equation(a/2, v0, (ti-ret[3].i)) + if not ret[3].t then ret[3].t = 0 else - i = i + 1 + if ret[3].t[1]<0 then + if ret[3].t[2]<0 then ret[3].t = ret[3].t[2] + else ret[3].t = math.abs(math.max(ret[3].t[1], ret[3].t[2])) + end + else + if ret[3].t[2]<0 then ret[3].t = ret[3].t[1] + else ret[3].t = math.min(ret[3].t[1], ret[3].t[2]) + end + end end end - - for i, it in ipairs(lzb.oncoming) do - local a = advtrains.get_acceleration(train, 1) --should be negative - local v0 = train.velocity - local v1 = it.spd - if v1 and v1 <= v0 then - local s = (v1*v1 - v0*v0) / (2*a) - - local st = s + params.ADD_SLOW - if v0 > 3 then - st = s + params.ADD_FAST - end - if v0<=0 then - st = s + params.ADD_STAND - end - - local i = advtrains.path_get_index_by_offset(train, it.idx, -st) - - --train.debug = dump({v0f=v0*f, aff=a*f*f,v0=v0, v1=v1, f=f, a=a, s=s, st=st, i=i, idx=train.index}) - if i <= train.index then - -- Gotcha! Braking... - train.ctrl.lzb = 1 - --train.debug = train.debug .. "BRAKE!!!" - return - end - - i = advtrains.path_get_index_by_offset(train, i, -params.ZONE_ROLL) - if i <= train.index and v0>1 then - -- roll control - train.ctrl.lzb = 2 - return + ret[3].v = (v0 + a*ret[3].t) + if ret[3].v <= lzb.spd then ret[3].v = lzb.spd end -- Avoid devision by zero + if ret[3].v > (train.max_speed or 10) then ret[3].v = train.max_speed or 0 end + ret[2].v = ret[3].v + ret[2].t = (ret[3].i-ret[2].i)/ret[3].v + ret[1].t = advtrains.solve_quadratic_equation(advtrains.get_acceleration(train,2),ret[2].v,(ret[2].i-ret[1].i)) + if not ret[1].t then ret[1].t = 0 + else + if ret[1].t[1]<0 then + if ret[1].t[2]<0 then ret[1].t = ret[1].t[2] + else ret[1].t = math.abs(math.max(ret[1].t[1], ret[1].t[2])) end - i = advtrains.path_get_index_by_offset(train, i, -params.ZONE_HOLD) - if i <= train.index and v0>1 then - -- hold speed - train.ctrl.lzb = 3 - return + else + if ret[1].t[2]<0 then ret[1].t = ret[1].t[1] + else ret[1].t = math.min(math.max(ret[1].t[1], ret[1].t[2])) end end end - train.ctrl.lzb = nil + ret[1].v = (ret[2].v + advtrains.get_acceleration(train,2)*ret[1].t) + if ret[1].v <= lzb.spd then ret[1].v = lzb.spd end + ret[0].v = lzb.spd + ret[0].t = (ret[0].v-ret[1].v)/advtrains.get_acceleration(train,1) + return ret +end + +--[[ +advtrains.lzb_get_limit_by_entry - get the limit +Returns a table contraining the speed and the acceleration limits +]] +function advtrains.lzb_get_limit_by_entry(train, lzb) + local ret = {} + local lzbmap = advtrains.lzb_map_entry(train, lzb) + if not (lzbmap[3].i and lzbmap[2].i and lzbmap[1].i and lzbmap[0].i) then + return {} + elseif (lzbmap[3].i > train.index) then return {} + elseif (lzbmap[2].i > train.index) then ret.lever = 3 + elseif (lzbmap[1].i > train.index) then ret.lever = 2 + else ret.lever = 1 + end + if ret.lever == 3 then ret.velocity = lzbmap[3].v + else + local s = train.index - lzbmap[ret.lever].i + local a = advtrains.get_acceleration(train, ret.lever) + local v0 = lzbmap[ret.lever].v + ret.velocity = math.sqrt(2*a*s - v0*v0) + end + if ret.velocity < train.velocity -1 then ret.lever = ret.lever - 1 end + return ret end --- Get the distance between the train and the LZB control point --- If not sure, use 3 as the parameter for lever level. -function advtrains.lzb_get_distance_until_override(id, train, lever) +-- Get next LZB restriction with the lowest speed restriction +function advtrains.lzb_get_next(train) if lever == 4 then return nil end local lzb = train.lzb local i = 1 - local ret = nil -- the value to return - local a = advtrains.get_acceleration(train, lever) -- Acceleration + local ret + local a = advtrains.get_acceleration(train, 3) -- Acceleration local v0 = train.velocity -- Remove LZB entries that are no longer valid while i <= #lzb.oncoming do @@ -170,29 +194,20 @@ function advtrains.lzb_get_distance_until_override(id, train, lever) i = i + 1 end end - -- Now run through all the LZB entries and find the one that is nearest to the train + -- Now run through all the LZB entries and find the one with the lowest + -- speed requirement for _, it in ipairs(lzb.oncoming) do local v1 = it.spd if v1 and v1 <= v0 then - local s, st - if a ~= 0 then s = (v1*v1-v0*v0)/2/a else s = 0 end - if v0 > 3 then st = s + params.ADD_FAST - elseif v0 <= 0 then st = s + params.ADD_STAND - else st = s + params.ADD_SLOW - end - i = advtrains.path_get_index_by_offset(train, it.idx, -st) - if lever == 2 then - i = advtrains.path_get_index_by_offset(train, it.idx, -params.ZONE_ROLL) - end - if lever == 3 then - i = advtrains.path_get_index_by_offset(train, id.idx, -params.ZONE_HOLD) + local curlimit = advtrains.lzb_get_limit_by_entry(train, it) + local retlimit = advtrains.lzb_get_limit_by_entry(train, ret) + if not ret then ret = it + elseif not curlimit.velocity then + elseif retlimit.velocity > curlimit.velocity then + ret = it end - if not ret then ret = i - train.index end - if (i - train.index) < ret then ret = i - train.index end end end - -- In extreme cases, there might be no LZB at all. - -- In such a case, return nil because the distance to LZB is infinite. return ret end @@ -234,5 +249,4 @@ advtrains.te_register_on_update(function(id, train) return end look_ahead(id, train) - apply_control(id, train) end, true) diff --git a/advtrains/trainlogic.lua b/advtrains/trainlogic.lua index 2b34e2a..4b76deb 100644 --- a/advtrains/trainlogic.lua +++ b/advtrains/trainlogic.lua @@ -420,61 +420,39 @@ function advtrains.train_step_b(id, train, dtime) train.lever = tmp_lever - --- 4a. Get the correct lever based on LZB --- - local lzblever = tmp_lever - tmp_lever = tmp_lever + 1 - local s, s1, s2, v0, v1, v2, t1, t2, a1, a2 - repeat - tmp_lever = lzblever - lzblever = tmp_lever - 1 - if lzblever < 0 then lzblever = 0 end - s1 = advtrains.lzb_get_distance_until_override(id, train, lzblever) - until (s1 >= 0) or (s1 == nil) -- also jump out if there is no LZB restriction - --- 4b. Calculations --- - a1 = advtrains.get_acceleration(train, tmp_lever) - a2 = advtrains.get_acceleration(train, lzblever) - v0 = train.velocity - if s1 == nil then -- No LZB limit - continue as normal - v2 = v0 + a1 * dtime - if train.tarvelocity then v2 = math.min(v2, train.tarvelocity) end - v2 = math.min(v2, (train.max_speed or 10)) - if a1 == 0 then -- if there is no acceleration, simply s = v * t - s = v0 * dtime - else - s = (v2*v2 - v0*v0)/2/a1 + --- 4a. Calculate movement --- + local lzbnxt = advtrains.lzb_get_next(train) + local lzblimit = advtrains.lzb_get_limit_by_entry(train, lzbnxt) + local lzbmap = advtrains.lzb_map_entry(train, lzbnxt) + local a = advtrains.get_acceleration(train, tmp_lever) + local v0 = train.velocity + local v1 = a*dtime+v0 + v1 = math.min(v1, (train.max_speed or 10)) + local s + if a == 0 then s = v1*dtime + else s = (v1*v1 - v0*v0)/2/a + end + if lzblimit.velocity and lzblimit.velocity < train.velocity then + tmp_lever = lzblimit.lever + while (lzbmap[tmp_lever].t > dtime) do + tmp_lever = tmp_lever - 1 end - a2 = a1 - lzblever = tmp_lever - else - if (a1 ~= 0) and ((-v0*v0)/2/a1 < s1) then -- train stops in front of LZB control - v2 = 0 - s = (-v0*v0)/2/a1 - else -- Train continues and further control seems to be taken - v1 = math.sqrt(2*s1*a1 + v0*v0) - if train.tarvelocity then v1 = math.min(v1, train.tarvelocity) end - v1 = math.min(v1, (train.max_speed or 10)) - if a1 == 0 then t1 = s1/v1 else t1 = (v1-v0)/a1 end - t2 = dtime - t1 - if t2 > 0 then -- if the train can reach s2 - v2 = a2*t2+v1 - if v2 < 0 then v2 = 0 end -- Force velocity to be at least 0 - if a2 == 0 then s2 = v2 * t2 else s2 = (v2*v2-v1*v1)/2/a2 end - s = s1 + s2 - else -- the train might not reach s2 due to some limits - v2 = v1 - if a1 == 0 then s = v2 * dtime else s = (v1*v1 - v0*v0)/2/a1 end - end + a = advtrains.get_acceleration(train, tmp_lever) + v0 = lzbmap[tmp_lever].v + t = dtime - lzbmap[tmp_lever].t + v1 = a*t+v0 + v1 = math.min(v1, (train.max_speed or 10)) + s = lzbmap[tmp_lever].i - train.index + if a == 0 then s = s + v1*t + else s = s + (v1*v1-v0*v0)/2/a end end - --- 4c. move train and change train properties --- + --- 4b. Move train and update train properties --- local pdist = train.path_dist[math.floor(train.index)] or 1 local distance = s / pdist - if train.lever > lzblever then train.ctrl.lzb = lzblever - else train.ctrl.lzb = nil - end - train.lever = lzblever - train.velocity = v2 - train.acceleration = a2 + train.lever = tmp_lever + train.velocity = v1 + train.acceleration = a --debugging code --train.debug = atdump(train.ctrl).."step_dist: "..math.floor(distance*1000) -- cgit v1.2.3