aboutsummaryrefslogtreecommitdiff
path: root/advtrains/path.lua
blob: 23f5e78c8a7f1990328f3b3b311d42d0d06b6f64 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
-- 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
--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_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_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
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")
	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],"")
		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)
	-- Step 1: determine my current absolute pos on the path
	local start_index_f = math.floor(index)
	local _, _, frac = advtrains.path_get_adjacent(train, index)
	local dist1, dist2 = train.path_dist[start_index_f], train.path_dist[start_index_f+1]
	local start_dist = n_interpolate(dist1, dist2, 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 = math.floor(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           :       ^
	
	advtrains.path_get_adjacent(train, 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
	
	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

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