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