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