More accurate train logic, but still buggy

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)<math.abs(adiff1) then

 		return conn2

 	else

@@ -129,7 +129,7 @@ function advtrains.minAngleDiffRad(r1, r2)
 	local try1=r2-r1

 	local try2=r2+pi2-r1

 	local try3=r2-pi2-r1

-	

+

 	local minabs = math.min(math.abs(try1), math.abs(try2), math.abs(try3))

 	if minabs==math.abs(try1) then

 		return try1

@@ -170,12 +170,12 @@ end
 

 function advtrains.get_real_index_position(path, index)

 	if not path or not index then return end

-	

+

 	local first_pos=path[math.floor(index)]

 	local second_pos=path[math.floor(index)+1]

-	

+

 	if not first_pos or not second_pos then return nil end

-	

+

 	local factor=index-math.floor(index)

 	local actual_pos={x=first_pos.x-(first_pos.x-second_pos.x)*factor, y=first_pos.y-(first_pos.y-second_pos.y)*factor, z=first_pos.z-(first_pos.z-second_pos.z)*factor,}

 	return actual_pos

@@ -306,16 +306,16 @@ function advtrains.get_adjacent_rail(this_posnr, this_conns_p, conn_idx, drives_
 		end

 		return nil

 	end

-	

+

 	local conn = this_conns[conn_idx]

 	local conn_y = conn.y or 0

 	local adj_pos = advtrains.dirCoordSet(this_pos, conn.c);

-	

+

 	while conn_y>=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 b7a4528..30d2d6a 100644
--- a/advtrains/trainlogic.lua
+++ b/advtrains/trainlogic.lua
@@ -419,61 +419,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)