cb38136cb0058b1d4e67d97bb2f4c1f02733edb3
[xonotic/xonotic-data.pk3dir.git] / qcsrc / server / pathlib / main.qc
1 #include "main.qh"
2
3 #include "pathlib.qh"
4 #include "utility.qh"
5 #include "../command/common.qh"
6
7 void pathlib_deletepath(entity start)
8 {
9     entity e;
10
11     e = findchainentity(owner, start);
12     while(e)
13     {
14         setthink(e, SUB_Remove_self);
15         e.nextthink = time;
16         e = e.chain;
17     }
18 }
19
20 //#define PATHLIB_NODEEXPIRE 0.05
21 const float PATHLIB_NODEEXPIRE = 20;
22
23 void dumpnode(entity n)
24 {
25     n.is_path_node = false;
26     setthink(n, SUB_Remove_self);
27     n.nextthink    = time;
28 }
29
30 #if DEBUGPATHING
31 void pathlib_showpath(entity start);
32 void pathlib_showpath2(entity path);
33 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
34 #endif
35
36
37 entity pathlib_mknode(vector where,entity parent)
38 {
39     entity node;
40
41     node = pathlib_nodeatpoint(where);
42     if(node)
43     {
44         #ifdef TURRET_DEBUG
45         mark_error(where, 60);
46         #endif
47         return node;
48     }
49
50     node = spawn();
51
52     setthink(node, SUB_Remove_self);
53     node.nextthink    = time + PATHLIB_NODEEXPIRE;
54     node.is_path_node = true;
55     node.owner        = openlist;
56     node.path_prev    = parent;
57
58
59     setsize(node, '0 0 0', '0 0 0');
60
61     setorigin(node, where);
62     node.medium = pointcontents(where);
63 #if DEBUGPATHING
64     pathlib_showsquare(where, 1 ,15);
65 #endif
66     ++pathlib_made_cnt;
67     ++pathlib_open_cnt;
68
69     return node;
70 }
71
72 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
73 {
74     entity node;
75     float h,g,f,doedge = 0;
76     vector where;
77
78     ++pathlib_searched_cnt;
79
80     if(inwater(parent.origin))
81     {
82         LOG_TRACE("FromWater\n");
83         pathlib_expandnode = pathlib_expandnode_box;
84         pathlib_movenode   = pathlib_swimnode;
85     }
86     else
87     {
88         if(inwater(to))
89         {
90             LOG_TRACE("ToWater\n");
91             pathlib_expandnode = pathlib_expandnode_box;
92             pathlib_movenode   = pathlib_walknode;
93         }
94         else
95         {
96             LOG_TRACE("LandToLoand\n");
97             //if(edge_check(parent.origin))
98             //    return 0;
99
100             pathlib_expandnode = pathlib_expandnode_star;
101             pathlib_movenode   = pathlib_walknode;
102             doedge = 1;
103         }
104     }
105
106     node = pathlib_nodeatpoint(to);
107     if(node)
108     {
109         LOG_TRACE("NodeAtPoint\n");
110         ++pathlib_merge_cnt;
111
112         if(node.owner == openlist)
113         {
114             h = pathlib_heuristic(node.origin,goal);
115             g = pathlib_cost(parent,node.origin,cost);
116             f = g + h;
117
118             if(node.pathlib_node_g > g)
119             {
120                 node.pathlib_node_h = h;
121                 node.pathlib_node_g = g;
122                 node.pathlib_node_f = f;
123
124                 node.path_prev = parent;
125             }
126
127             if (!best_open_node)
128                 best_open_node = node;
129             else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
130                 best_open_node = node;
131         }
132
133         return 1;
134     }
135
136     where = pathlib_movenode(parent, parent.origin, to, 0);
137
138     if (!pathlib_movenode_goodnode)
139     {
140         //pathlib_showsquare(where, 0 ,30);
141         //pathlib_showsquare(parent.origin, 1 ,30);
142         LOG_TRACE("pathlib_movenode_goodnode = 0\n");
143         return 0;
144     }
145
146     //pathlib_showsquare(where, 1 ,30);
147
148     if(pathlib_nodeatpoint(where))
149     {
150         LOG_TRACE("NAP WHERE :",vtos(where),"\n");
151         LOG_TRACE("not NAP TO:",vtos(to),"\n");
152         LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");
153         return 0;
154     }
155
156
157     if(doedge)
158         if (!tile_check(parent, where))
159         {
160             LOG_TRACE("tile_check fail\n");
161 #if DEBUGPATHING
162             pathlib_showsquare(where, 0 ,30);
163 #endif
164             return 0;
165         }
166
167
168     h = pathlib_heuristic(where,goal);
169     g = pathlib_cost(parent,where,cost);
170     f = g + h;
171
172     /*
173     node = findradius(where,pathlib_gridsize * 0.5);
174     while(node)
175     {
176         if(node.is_path_node == true)
177         {
178             ++pathlib_merge_cnt;
179             if(node.owner == openlist)
180             {
181                 if(node.pathlib_node_g > g)
182                 {
183                     //pathlib_movenode(node, where,node.origin,0);
184                     //if(pathlib_movenode_goodnode)
185                     //{
186                         //mark_error(node.origin + '0 0 128',30);
187                         node.pathlib_node_h = h;
188                         node.pathlib_node_g = g;
189                         node.pathlib_node_f = f;
190                         node.path_prev = parent;
191                     //}
192                 }
193
194                 if (!best_open_node)
195                     best_open_node = node;
196                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
197                     best_open_node = node;
198             }
199
200             return 1;
201         }
202         node = node.chain;
203     }
204     */
205
206     node = pathlib_mknode(where, parent);
207     node.pathlib_node_h = h;
208     node.pathlib_node_g = g;
209     node.pathlib_node_f = f;
210
211     if (!best_open_node)
212         best_open_node = node;
213     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
214         best_open_node = node;
215
216     return 1;
217 }
218
219 entity pathlib_getbestopen()
220 {
221     entity node;
222     entity bestnode;
223
224     if(best_open_node)
225     {
226         ++pathlib_bestcash_hits;
227         pathlib_bestcash_saved += pathlib_open_cnt;
228
229         return best_open_node;
230     }
231
232     node = findchainentity(owner,openlist);
233     if(!node)
234         return world;
235
236     bestnode = node;
237     while(node)
238     {
239         ++pathlib_bestopen_seached;
240         if(node.pathlib_node_f < bestnode.pathlib_node_f)
241             bestnode = node;
242
243         node = node.chain;
244     }
245
246     return bestnode;
247 }
248
249 void pathlib_close_node(entity node,vector goal)
250 {
251
252     if(node.owner == closedlist)
253     {
254         LOG_TRACE("Pathlib: Tried to close a closed node!\n");
255         return;
256     }
257
258     if(node == best_open_node)
259         best_open_node = world;
260
261     ++pathlib_closed_cnt;
262     --pathlib_open_cnt;
263
264     node.owner = closedlist;
265
266     if(vdist(node.origin - goal, <=, pathlib_gridsize))
267     {
268         vector goalmove;
269
270         goalmove = pathlib_walknode(node, node.origin, goal, 1);
271         if(pathlib_movenode_goodnode)
272         {
273             goal_node         = node;
274             pathlib_foundgoal = true;
275         }
276     }
277 }
278
279 void pathlib_cleanup()
280 {
281     best_open_node = world;
282
283     //return;
284
285     entity node;
286
287     node = findfloat(world,is_path_node, true);
288     while(node)
289     {
290         /*
291         node.owner = openlist;
292         node.pathlib_node_g = 0;
293         node.pathlib_node_h = 0;
294         node.pathlib_node_f = 0;
295         node.path_prev = world;
296         */
297
298         dumpnode(node);
299         node = findfloat(node,is_path_node, true);
300     }
301
302     if(openlist)
303         remove(openlist);
304
305     if(closedlist)
306         remove(closedlist);
307
308     openlist       = world;
309     closedlist     = world;
310
311 }
312
313 float Cosine_Interpolate(float a, float b, float x)
314 {
315     float ft,f;
316
317         ft = x * 3.1415927;
318         f = (1 - cos(ft)) * 0.5;
319
320         return  a*(1-f) + b*f;
321 }
322
323 float buildpath_nodefilter_directional(vector n,vector c,vector p)
324 {
325     vector d1,d2;
326
327     d2 = normalize(p - c);
328     d1 = normalize(c - n);
329
330     if(vlen(d1-d2) < 0.25)
331     {
332         //mark_error(c,30);
333         return 1;
334     }
335     //mark_info(c,30);
336     return 0;
337 }
338
339 float buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
340 {
341     pathlib_walknode(this, p, n, 1);
342     if(pathlib_movenode_goodnode)
343         return 1;
344
345     return 0;
346 }
347
348 float buildpath_nodefilter_none(vector n,vector c,vector p)
349 {
350     return 0;
351 }
352
353 entity path_build(entity next, vector where, entity prev, entity start)
354 {
355     entity path;
356
357     if(prev && next)
358         if(buildpath_nodefilter)
359             if(buildpath_nodefilter(next.origin,where,prev.origin))
360                 return next;
361
362
363     path           = spawn();
364     path.owner     = start;
365     path.path_next = next;
366
367     setorigin(path,where);
368
369     if(!next)
370         path.classname = "path_end";
371     else
372     {
373         if(!prev)
374             path.classname = "path_start";
375         else
376             path.classname = "path_node";
377     }
378
379     return path;
380 }
381
382 entity pathlib_astar(vector from,vector to)
383 {SELFPARAM();
384     entity path, start, end, open, n, ln;
385     float ptime, ftime, ctime;
386
387     ptime = gettime(GETTIME_REALTIME);
388     pathlib_starttime = ptime;
389
390     pathlib_cleanup();
391
392     // Select water<->land capable node make/link
393     pathlib_makenode     = pathlib_makenode_adaptive;
394
395     // Select XYZ cost estimate
396     //pathlib_heuristic    = pathlib_h_diagonal3;
397     pathlib_heuristic    = pathlib_h_diagonal;
398
399     // Select distance + waterfactor cost
400     pathlib_cost         = pathlib_g_euclidean_water;
401
402     // Select star expander
403     pathlib_expandnode   = pathlib_expandnode_star;
404
405     // Select walk simulation movement test
406     pathlib_movenode     = pathlib_walknode;
407
408     // Filter final nodes by direction
409     buildpath_nodefilter = buildpath_nodefilter_directional;
410
411     // Filter tiles with cross pattern
412     tile_check = tile_check_cross;
413
414     // If the start is in water we need diffrent settings
415     if(inwater(from))
416     {
417         // Select volumetric node expaner
418         pathlib_expandnode = pathlib_expandnode_box;
419
420         // Water movement test
421         pathlib_movenode   = pathlib_swimnode;
422     }
423
424     if (!openlist)
425         openlist       = spawn();
426
427     if (!closedlist)
428         closedlist     = spawn();
429
430     pathlib_closed_cnt       = 0;
431     pathlib_open_cnt         = 0;
432     pathlib_made_cnt         = 0;
433     pathlib_merge_cnt        = 0;
434     pathlib_searched_cnt     = 0;
435     pathlib_bestopen_seached = 0;
436     pathlib_bestcash_hits    = 0;
437     pathlib_bestcash_saved   = 0;
438
439     pathlib_gridsize       = 128;
440     pathlib_movecost       = pathlib_gridsize;
441     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
442     pathlib_movecost_waterfactor = 25000000;
443     pathlib_foundgoal      = 0;
444
445     movenode_boxmax   = self.maxs * 1.1;
446     movenode_boxmin   = self.mins * 1.1;
447
448     movenode_stepsize = pathlib_gridsize * 0.25;
449
450     tile_check_size = pathlib_gridsize * 0.5;
451     tile_check_up   = '0 0 2' * pathlib_gridsize;
452     tile_check_up   = '0 0 128';
453     tile_check_down = '0 0 3' * pathlib_gridsize;
454     tile_check_down = '0 0 256';
455
456     //movenode_stepup   = '0 0 128';
457     movenode_stepup   = '0 0 36';
458     movenode_maxdrop  = '0 0 512';
459     //movenode_maxdrop  = '0 0 512';
460     movenode_boxup    = '0 0 72';
461
462     from.x = fsnap(from.x, pathlib_gridsize);
463     from.y = fsnap(from.y, pathlib_gridsize);
464     //from_z += 32;
465
466     to.x = fsnap(to.x, pathlib_gridsize);
467     to.y = fsnap(to.y, pathlib_gridsize);
468     //to_z += 32;
469
470     LOG_TRACE("AStar init\n");
471     path = pathlib_mknode(from, world);
472     pathlib_close_node(path, to);
473     if(pathlib_foundgoal)
474     {
475         LOG_TRACE("AStar: Goal found on first node!\n");
476
477         open           = new(path_end);
478         open.owner     = open;
479         setorigin(open,path.origin);
480
481         pathlib_cleanup();
482
483         return open;
484     }
485
486     if(pathlib_expandnode(path, from, to) <= 0)
487     {
488         LOG_TRACE("AStar path fail.\n");
489         pathlib_cleanup();
490
491         return world;
492     }
493
494     best_open_node = pathlib_getbestopen();
495     n = best_open_node;
496     pathlib_close_node(best_open_node, to);
497     if(inwater(n.origin))
498         pathlib_expandnode_box(n, from, to);
499     else
500         pathlib_expandnode_star(n, from, to);
501
502     while(pathlib_open_cnt)
503     {
504         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
505         {
506             LOG_TRACE("Path took to long to compute!\n");
507             LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
508             LOG_TRACE("Nodes -    open: ", ftos(pathlib_open_cnt),"\n");
509             LOG_TRACE("Nodes -  merged: ", ftos(pathlib_merge_cnt),"\n");
510             LOG_TRACE("Nodes -  closed: ", ftos(pathlib_closed_cnt),"\n");
511
512             pathlib_cleanup();
513             return world;
514         }
515
516         best_open_node = pathlib_getbestopen();
517         n = best_open_node;
518         pathlib_close_node(best_open_node,to);
519
520         if(inwater(n.origin))
521             pathlib_expandnode_box(n,from,to);
522         else
523             pathlib_expandnode(n,from,to);
524
525         if(pathlib_foundgoal)
526         {
527             LOG_TRACE("Target found. Rebuilding and filtering path...\n");
528             ftime = gettime(GETTIME_REALTIME);
529             ptime = ftime - ptime;
530
531             start = path_build(world,path.origin,world,world);
532             end   = path_build(world,goal_node.origin,world,start);
533             ln    = end;
534
535             open = goal_node;
536             for(open = goal_node; open.path_prev != path; open = open.path_prev)
537             {
538                 n    = path_build(ln,open.origin,open.path_prev,start);
539                 ln.path_prev = n;
540                 ln = n;
541             }
542             start.path_next = n;
543             n.path_prev = start;
544             ftime = gettime(GETTIME_REALTIME) - ftime;
545
546             ctime = gettime(GETTIME_REALTIME);
547             pathlib_cleanup();
548             ctime = gettime(GETTIME_REALTIME) - ctime;
549
550
551 #if DEBUGPATHING
552             pathlib_showpath2(start);
553
554             LOG_TRACE("Time used -      pathfinding: ", ftos(ptime),"\n");
555             LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
556             LOG_TRACE("Time used -          cleanup: ", ftos(ctime),"\n");
557             LOG_TRACE("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
558             LOG_TRACE("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
559             LOG_TRACE("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
560             LOG_TRACE("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
561             LOG_TRACE("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
562             LOG_TRACE("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
563             LOG_TRACE("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
564             LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
565             LOG_TRACE("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
566             LOG_TRACE("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
567             LOG_TRACE("AStar done.\n");
568 #endif
569             return start;
570         }
571     }
572
573     LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");
574
575     pathlib_cleanup();
576
577     return world;
578 }