Fix path_dir to actually be an angle, path item deletion and orientation of wagons

The occupation system as it is now will change. For each position, I will save the index in the train's path,
and implement a callback system. I need this because the occupation window system will not be enough to cover all use cases
(e.g. to make a train stop with it's center or back at a certain position, I need 3 different brake distances, which doesn't fit into the scheme)

7 files changed, 112 insertions, 103 deletions

diff --git a/advtrains/helpers.lua b/advtrains/helpers.lua
index 9ad5a8e..9add1b8 100644
--- a/advtrains/helpers.lua
+++ b/advtrains/helpers.lua
@@ -30,6 +30,11 @@ function advtrains.dirCoordSet(coord, dir)
 end

 advtrains.pos_add_dir = advtrains.dirCoordSet

 

+function advtrains.pos_add_angle(pos, ang)

+	-- 0 is +Z -> meaning of sin/cos swapped

+	return vector.add(pos, {x=math.sin(ang), y=0, z=math.cos(ang)})

+end

+

 function advtrains.dirToCoord(dir)

 	return advtrains.dirCoordSet({x=0, y=0, z=0}, dir)

 end

@@ -104,12 +109,11 @@ end
 

 function advtrains.dir_to_angle(dir)

 	local uvec = vector.normalize(advtrains.dirToCoord(dir))

-	return math.atan2(uvec.z, -uvec.x)

+	return math.atan2(uvec.z, uvec.x)

 end

 

-

+local pi, pi2 = math.pi, 2*math.pi

 function advtrains.minAngleDiffRad(r1, r2)

-	local pi, pi2 = math.pi, 2*math.pi

 	while r1>pi2 do

 		r1=r1-pi2

 	end

@@ -138,19 +142,19 @@ function advtrains.minAngleDiffRad(r1, r2)
 	end

 end

 

-function advtrains.dumppath(path)

-	atlog("Dumping a path:")

-	if not path then atlog("dumppath: no path(nil)") return end

-	local temp_path={}

-	for ipt, iit in pairs(path) do 

-		temp_path[#temp_path+1]={i=ipt, p=iit}

-	end

-	table.sort(temp_path, function (k1, k2) return k1.i < k2.i end)

-	for _,pit in ipairs(temp_path) do

-		atlog(pit.i.." > "..minetest.pos_to_string(pit.p))

-	end

+

+-- Takes 2 connections (0...AT_CMAX) as argument

+-- Returns the angle median of those 2 positions from the pov

+-- of standing on the cdir1 side and looking towards cdir2

+-- cdir1 - >NODE> - cdir2

+function advtrains.conn_angle_median(cdir1, cdir2)

+	local ang1 = advtrains.dir_to_angle(advtrains.oppd(cdir1))

+	local ang2 = advtrains.dir_to_angle(cdir2)

+	return ang1 + advtrains.minAngleDiffRad(ang1, ang2)/2

 end

 

+-- TODO removed dumppath, where is this used?

+

 function advtrains.merge_tables(a, ...)

 	local new={}

 	for _,t in ipairs({a,...}) do

@@ -165,22 +169,9 @@ function advtrains.save_keys(tbl, keys)
 	end

 	return new

 end

-function advtrains.yaw_from_3_positions(prev, curr, next)

-	local pts=minetest.pos_to_string

-	--atprint("p3 "..pts(prev)..pts(curr)..pts(next))

-	local prev2curr=math.atan2((curr.x-prev.x), (prev.z-curr.z))

-	local curr2next=math.atan2((next.x-curr.x), (curr.z-next.z))

-	--atprint("y3 "..(prev2curr*360/(2*math.pi)).." "..(curr2next*360/(2*math.pi)))

-	return prev2curr+(advtrains.minAngleDiffRad(prev2curr, curr2next)/2)

-end

-function advtrains.get_wagon_yaw(front, first, second, back, pct)

-	local pts=minetest.pos_to_string

-	--atprint("p "..pts(front)..pts(first)..pts(second)..pts(back))

-	local y2=advtrains.yaw_from_3_positions(second, first, front)

-	local y1=advtrains.yaw_from_3_positions(back, second, first)

-	--atprint("y "..(y1*360/(2*math.pi)).." "..(y2*360/(2*math.pi)))

-	return y1+advtrains.minAngleDiffRad(y1, y2)*pct

-end

+

+-- TODO yaw_from_3_positions and get_wagon_yaw removed

+

 function advtrains.get_real_index_position(path, index)

 	if not path or not index then return end

 	

diff --git a/advtrains/init.lua b/advtrains/init.lua
index 69b482d..93b25e5 100644
--- a/advtrains/init.lua
+++ b/advtrains/init.lua
@@ -38,16 +38,20 @@ end
 function advtrains.pcall(fun)
 	if no_action then return end
 	
-	--local succ, return1, return2, return3, return4=xpcall(fun, function(err)
-	--		atwarn("Lua Error occured: ", err)
-	--		atwarn(debug.traceback())
-	--	end)
-	--if not succ then
-	--	reload_saves()
-	--else
-	--	return return1, return2, return3, return4
-	--end
-	return fun()
+	local succ, return1, return2, return3, return4=xpcall(fun, function(err)
+			atwarn("Lua Error occured: ", err)
+			atwarn(debug.traceback())
+			if advtrains.atprint_context_tid_full then
+				advtrains.path_print(advtrains.trains[advtrains.atprint_context_tid_full], atdebug)
+			end
+		end)
+	if not succ then
+		--reload_saves()
+		no_action=true --this does also not belong here!
+		minetest.request_shutdown()
+	else
+		return return1, return2, return3, return4
+	end
 end
 
 
diff --git a/advtrains/occupation.lua b/advtrains/occupation.lua
index 7af4a3d..e72d668 100644
--- a/advtrains/occupation.lua
+++ b/advtrains/occupation.lua
@@ -227,14 +227,14 @@ function handle_chg(t, pos, train_id, old, new)
 	if #blocking > 0 then
 		-- the aware trains should brake
 		for _, ix in ipairs(aware) do
-			atc.train_set_command(t[ix], "B2")
+			advtrains.atc.train_set_command(t[ix], "B2")
 		end
 		if #blocking > 1 then
 			-- not good, 2 trains interfered with their blocking zones
 			-- make them brake too
 			local txt = {}
 			for _, ix in ipairs(blocking) do
-				atc.train_set_command(t[ix], "B2")
+				advtrains.atc.train_set_command(t[ix], "B2")
 				txt[#txt+1] = t[ix]
 			end
 			atwarn("Trains",table.concat(txt, ","), "interfered with their blocking zones, braking...")
diff --git a/advtrains/path.lua b/advtrains/path.lua
index 039085c..ad5aeb3 100644
--- a/advtrains/path.lua
+++ b/advtrains/path.lua
@@ -113,8 +113,7 @@ function advtrains.path_create(train, pos, connid, rel_index)
 	train.path_dist = {}
 	
 	train.path_dir = {
-		[1] = conns[connid].c,
-		[0] = advtrains.oppd(conns[mconnid].c)
+		[0] = advtrains.conn_angle_median(conns[mconnid].c, conns[connid].c)
 	}
 	
 	train.path_ext_f=0
@@ -123,10 +122,12 @@ function advtrains.path_create(train, pos, connid, rel_index)
 	train.path_trk_b=0
 	train.path_req_f=0
 	train.path_req_b=0
-	atdebug("path_create",train)
 	
 end
 
+-- Invalidates a path
+-- TODO: this is supposed to clear stuff from the occupation tables
+-- (note: why didn't I think of that earlier?)
 function advtrains.path_invalidate(train)
 	train.path = nil
 	train.path_dist = nil
@@ -141,6 +142,15 @@ function advtrains.path_invalidate(train)
 	train.path_req_b=0
 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("i:	CP	Position	Dir	CN		->Dist->")
+	for i = train.path_ext_b, train.path_ext_f do
+		printf(i,":	",train.path_cp[i],"	",train.path[i],"	",train.path_dir[i],"	",train.path_cn[i],"		->",train.path_dist[i],"->")
+	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)
@@ -150,58 +160,60 @@ function advtrains.path_get(train, index)
 	if index ~= atfloor(index) then
 		error("For train "..train.id..": Called path_get() but index="..index.." is not a round number")
 	end
-	while index > train.path_ext_f do
-		local pos = train.path[train.path_ext_f]
-		local connid = train.path_cn[train.path_ext_f]
+	local pef = train.path_ext_f
+	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 train.path_ext_f == train.path_trk_f then
+		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 "..train.path_ext_f.." on-track but not a valid node!") end
+			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
-		train.path_ext_f = train.path_ext_f + 1
+		pef = pef + 1
 		if adj_pos then
 			adj_pos.y = adj_pos.y + nextrail_y
-			train.path_cp[train.path_ext_f] = adj_connid
+			train.path_cp[pef] = adj_connid
 			local mconnid = advtrains.get_matching_conn(adj_connid, #next_conns)
-			train.path_cn[train.path_ext_f] = mconnid
-			train.path_dir[train.path_ext_f+1] = this_conns[mconnid].c
-			train.path_trk_f = train.path_ext_f
+			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_dir(pos, train.path_dir[train.path_ext_f-1])
-			train.path_dir[train.path_ext_f] = train.path_dir[train.path_ext_f-1]
+			adj_pos = advtrains.pos_add_angle(pos, train.path_dir[pef-1])
+			train.path_dir[pef] = train.path_dir[pef-1]
 		end
-		train.path[train.path_ext_f] = adj_pos
-		train.path_dist[train.path_ext_f - 1] = vector.distance(pos, adj_pos)
+		train.path[pef] = adj_pos
+		train.path_dist[pef - 1] = vector.distance(pos, adj_pos)
 	end
-	while index < train.path_ext_b do
-		local pos = train.path[train.path_ext_b]
-		local connid = train.path_cp[train.path_ext_b]
+	train.path_ext_f = pef
+	local peb = train.path_ext_b
+	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 train.path_ext_b == train.path_trk_b then
+		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 "..train.path_ext_f.." on-track but not a valid node!") end
+			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
-		train.path_ext_b = train.path_ext_b - 1
+		peb = peb - 1
 		if adj_pos then
 			adj_pos.y = adj_pos.y + nextrail_y
-			train.path_cn[train.path_ext_b] = adj_connid
+			train.path_cn[peb] = adj_connid
 			local mconnid = advtrains.get_matching_conn(adj_connid, #next_conns)
-			train.path_cp[train.path_ext_b] = mconnid
-			
-			train.path_dir[train.path_ext_b] = advtrains.oppd(this_conns[mconnid].c)
-			
-			train.path_trk_b = train.path_ext_b
+			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_dir(pos, train.path_dir[train.path_ext_b-1])
-			train.path_dir[train.path_ext_b] = train.path_dir[train.path_ext_b-1]
+			adj_pos = advtrains.pos_add_angle(pos, train.path_dir[peb+1])
+			train.path_dir[peb] = train.path_dir[peb+1]
 		end
-		train.path[train.path_ext_b] = adj_pos
-		train.path_dist[train.path_ext_b] = vector.distance(pos, adj_pos)
+		train.path[peb] = adj_pos
+		train.path_dist[peb] = vector.distance(pos, adj_pos)
 	end
+	train.path_ext_b = peb
 	
 	if index < train.path_req_b then
 		train.path_req_b = index
@@ -224,10 +236,8 @@ function advtrains.path_get_interpolated(train, index)
 	local p_ceil = advtrains.path_get(train, i_ceil)
 	-- Note: minimal code duplication to path_get_adjacent, for performance
 	
-	local d_floor = train.path_dir[i_floor]
-	local d_ceil = train.path_dir[i_ceil]
-	local a_floor = advtrains.dir_to_angle(d_floor)
-	local a_ceil = advtrains.dir_to_angle(d_ceil)
+	local a_floor = train.path_dir[i_floor]
+	local a_ceil = train.path_dir[i_ceil]
 	
 	local ang = advtrains.minAngleDiffRad(a_floor, a_ceil)
 	
@@ -278,23 +288,28 @@ function advtrains.path_get_index_by_offset(train, index, offset)
 	return idx + (off / train.path_dist[idx])
 end
 
-local PATH_CLEAR_KEEP = 2
+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
 		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
-	for i = train.path_req_f + PATH_CLEAR_KEEP, train.path_ext_f do
+	
+	for i = train.path_ext_f,train.path_req_f + PATH_CLEAR_KEEP,-1 do
 		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_b = i - 1
 	end
+	
 	train.path_req_f = math.ceil(train.index)
 	train.path_req_b = math.floor(train.end_index or train.index)
 end
diff --git a/advtrains/trainhud.lua b/advtrains/trainhud.lua
index 26af2a1..60ef5d1 100644
--- a/advtrains/trainhud.lua
+++ b/advtrains/trainhud.lua
@@ -60,7 +60,7 @@ function advtrains.on_control_change(pc, train, flip)
 				end
 				act=true
 			else
-				--train.movedir = -train.movedir
+				advtrains.invert_train(train.id)
 			end
 		end
 		if pc.left then
diff --git a/advtrains/trainlogic.lua b/advtrains/trainlogic.lua
index 12ec8b5..fa3ff9e 100644
--- a/advtrains/trainlogic.lua
+++ b/advtrains/trainlogic.lua
@@ -411,7 +411,7 @@ function advtrains.train_step_b(id, train, dtime)
 			train.tarvelocity = train.velocity
 		end
 	else
-		train.last_accel = 0
+		train.acceleration = 0
 	end
 	
 	--- 4. move train ---
@@ -436,6 +436,7 @@ if train.no_step or train.wait_for_path then return end
 	advtrains.path_clear_unused(train)
 	
 	-- Set our path restoration position
+	-- TODO make a common function to find a restore positionon the path, in case the wanted position is off-track
 	local fli = atfloor(train.index)
 	train.last_pos = advtrains.path_get(train, fli)
 	train.last_connid = train.path_cn[fli]
@@ -854,26 +855,21 @@ function advtrains.do_connect_trains(first_id, second_id, player)
 	return true
 end
 
--- TODO
 function advtrains.invert_train(train_id)
 	local train=advtrains.trains[train_id]
 	
-	local old_path=train.path
-	local old_path_dist=train.path_dist
-	train.path={}
-	train.path_dist={}
-	train.index, train.end_index= -train.end_index, -train.index
-	train.path_extent_min, train.path_extent_max = -train.path_extent_max, -train.path_extent_min
-	train.min_index_on_track, train.max_index_on_track = -train.max_index_on_track, -train.min_index_on_track
-	train.detector_old_index, train.detector_old_end_index = -train.detector_old_end_index, -train.detector_old_index
-	train.couple_lck_back, train.couple_lck_front = train.couple_lck_front, train.couple_lck_back 
-	
-	train.velocity=-train.velocity
-	train.tarvelocity=-train.tarvelocity
-	for k,v in pairs(old_path) do
-		train.path[-k]=v
-		train.path_dist[-k-1]=old_path_dist[k]
-	end
+	advtrains.train_ensure_clean(train_id, train, 0)
+	-- Set the path restoration position to the opposite direction
+	local fli = atfloor(train.end_index) + 1
+	train.last_pos = advtrains.path_get(train, fli)
+	train.last_connid = train.path_cp[fli]
+	train.last_frac = fli - train.end_index
+	
+	-- rotate some other stuff
+	train.couple_lck_back, train.couple_lck_front = train.couple_lck_front, train.couple_lck_back
+	
+	advtrains.path_invalidate(train)
+	
 	local old_trainparts=train.trainparts
 	train.trainparts={}
 	for k,v in ipairs(old_trainparts) do
diff --git a/advtrains/wagons.lua b/advtrains/wagons.lua
index c3217e2..bf128d9 100644
--- a/advtrains/wagons.lua
+++ b/advtrains/wagons.lua
@@ -185,7 +185,7 @@ function wagon:destroy()
 		

 		if data.train_id and self:train() then

 			table.remove(self:train().trainparts, data.pos_in_trainparts)

-			advtrains.update_trainpart_properties(self.train_id)

+			advtrains.update_trainpart_properties(data.train_id)

 			advtrains.wagons[self.id]=nil

 			if self.discouple then self.discouple.object:remove() end--will have no effect on unloaded objects

 			return true

@@ -196,6 +196,7 @@ function wagon:destroy()
 	self.object:remove()

 end

 

+local pihalf = math.pi/2

 

 function wagon:on_step(dtime)

 	return advtrains.pcall(function()

@@ -375,9 +376,11 @@ function wagon:on_step(dtime)
 		

 		-- Calculate new position, yaw and direction vector

 		local index = advtrains.path_get_index_by_offset(train, train.index, -data.pos_in_train)

-		local pos, yaw, npos, npos2 = advtrains.path_get_interpolated(train, index)

+		local pos, tyaw, npos, npos2 = advtrains.path_get_interpolated(train, index)

 		local vdir = vector.normalize(vector.subtract(npos2, npos))

 		

+		local yaw = tyaw - pihalf

+		

 		--automatic get_on

 		--needs to know index and path

 		if self.door_entry and train.door_open and train.door_open~=0 and train.velocity==0 then

@@ -478,7 +481,7 @@ function wagon:on_step(dtime)
 							self.player_yaw[name] = p:get_look_horizontal()-self.old_yaw

 						end

 						-- set player looking direction using calculated offset

-						p:set_look_horizontal((self.player_yaw[name] or 0)+yaw)

+						--TODO p:set_look_horizontal((self.player_yaw[name] or 0)+yaw)

 					end

 				end

 				self.turning = true