Increase the maximum number of mutators
[xonotic/xonotic-data.pk3dir.git] / qcsrc / server / bot / lib / pathlib / main.qc
1 #include "pathlib.qh"
2 #include "utility.qh"
3
4 void pathlib_deletepath(entity start)
5 {
6     entity e;
7
8     e = findchainentity(owner, start);
9     while(e)
10     {
11         e.think = SUB_Remove;
12         e.nextthink = time;
13         e = e.chain;
14     }
15 }
16
17 //#define PATHLIB_NODEEXPIRE 0.05
18 const float PATHLIB_NODEEXPIRE = 20;
19
20 void dumpnode(entity n)
21 {
22     n.is_path_node = false;
23     n.think        = SUB_Remove;
24     n.nextthink    = time;
25 }
26
27 entity pathlib_mknode(vector where,entity parent)
28 {
29     entity node;
30
31     node = pathlib_nodeatpoint(where);
32     if(node)
33     {
34         #ifdef TURRET_DEBUG
35         mark_error(where, 60);
36         #endif
37         return node;
38     }
39
40     node = spawn();
41
42     node.think        = SUB_Remove;
43     node.nextthink    = time + PATHLIB_NODEEXPIRE;
44     node.is_path_node = true;
45     node.owner        = openlist;
46     node.path_prev    = parent;
47
48
49     setsize(node, '0 0 0', '0 0 0');
50
51     setorigin(node, where);
52     node.medium = pointcontents(where);
53     pathlib_showsquare(where, 1 ,15);
54
55     ++pathlib_made_cnt;
56     ++pathlib_open_cnt;
57
58     return node;
59 }
60
61 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
62 {
63     entity node;
64     float h,g,f,doedge = 0;
65     vector where;
66
67     ++pathlib_searched_cnt;
68
69     if(inwater(parent.origin))
70     {
71         LOG_TRACE("FromWater\n");
72         pathlib_expandnode = pathlib_expandnode_box;
73         pathlib_movenode   = pathlib_swimnode;
74     }
75     else
76     {
77         if(inwater(to))
78         {
79             LOG_TRACE("ToWater\n");
80             pathlib_expandnode = pathlib_expandnode_box;
81             pathlib_movenode   = pathlib_walknode;
82         }
83         else
84         {
85             LOG_TRACE("LandToLoand\n");
86             //if(edge_check(parent.origin))
87             //    return 0;
88
89             pathlib_expandnode = pathlib_expandnode_star;
90             pathlib_movenode   = pathlib_walknode;
91             doedge = 1;
92         }
93     }
94
95     node = pathlib_nodeatpoint(to);
96     if(node)
97     {
98         LOG_TRACE("NodeAtPoint\n");
99         ++pathlib_merge_cnt;
100
101         if(node.owner == openlist)
102         {
103             h = pathlib_heuristic(node.origin,goal);
104             g = pathlib_cost(parent,node.origin,cost);
105             f = g + h;
106
107             if(node.pathlib_node_g > g)
108             {
109                 node.pathlib_node_h = h;
110                 node.pathlib_node_g = g;
111                 node.pathlib_node_f = f;
112
113                 node.path_prev = parent;
114             }
115
116             if (!best_open_node)
117                 best_open_node = node;
118             else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
119                 best_open_node = node;
120         }
121
122         return 1;
123     }
124
125     where = pathlib_movenode(parent.origin, to, 0);
126
127     if (!pathlib_movenode_goodnode)
128     {
129         //pathlib_showsquare(where, 0 ,30);
130         //pathlib_showsquare(parent.origin, 1 ,30);
131         LOG_TRACE("pathlib_movenode_goodnode = 0\n");
132         return 0;
133     }
134
135     //pathlib_showsquare(where, 1 ,30);
136
137     if(pathlib_nodeatpoint(where))
138     {
139         LOG_TRACE("NAP WHERE :",vtos(where),"\n");
140         LOG_TRACE("not NAP TO:",vtos(to),"\n");
141         LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");
142         return 0;
143     }
144
145
146     if(doedge)
147         if (!tile_check(where))
148         {
149             LOG_TRACE("tile_check fail\n");
150             pathlib_showsquare(where, 0 ,30);
151             return 0;
152         }
153
154
155     h = pathlib_heuristic(where,goal);
156     g = pathlib_cost(parent,where,cost);
157     f = g + h;
158
159     /*
160     node = findradius(where,pathlib_gridsize * 0.5);
161     while(node)
162     {
163         if(node.is_path_node == true)
164         {
165             ++pathlib_merge_cnt;
166             if(node.owner == openlist)
167             {
168                 if(node.pathlib_node_g > g)
169                 {
170                     //pathlib_movenode(where,node.origin,0);
171                     //if(pathlib_movenode_goodnode)
172                     //{
173                         //mark_error(node.origin + '0 0 128',30);
174                         node.pathlib_node_h = h;
175                         node.pathlib_node_g = g;
176                         node.pathlib_node_f = f;
177                         node.path_prev = parent;
178                     //}
179                 }
180
181                 if (!best_open_node)
182                     best_open_node = node;
183                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
184                     best_open_node = node;
185             }
186
187             return 1;
188         }
189         node = node.chain;
190     }
191     */
192
193     node = pathlib_mknode(where, parent);
194     node.pathlib_node_h = h;
195     node.pathlib_node_g = g;
196     node.pathlib_node_f = f;
197
198     if (!best_open_node)
199         best_open_node = node;
200     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
201         best_open_node = node;
202
203     return 1;
204 }
205
206 entity pathlib_getbestopen()
207 {
208     entity node;
209     entity bestnode;
210
211     if(best_open_node)
212     {
213         ++pathlib_bestcash_hits;
214         pathlib_bestcash_saved += pathlib_open_cnt;
215
216         return best_open_node;
217     }
218
219     node = findchainentity(owner,openlist);
220     if(!node)
221         return world;
222
223     bestnode = node;
224     while(node)
225     {
226         ++pathlib_bestopen_seached;
227         if(node.pathlib_node_f < bestnode.pathlib_node_f)
228             bestnode = node;
229
230         node = node.chain;
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!\n");
242         return;
243     }
244
245     if(node == best_open_node)
246         best_open_node = world;
247
248     ++pathlib_closed_cnt;
249     --pathlib_open_cnt;
250
251     node.owner = closedlist;
252
253     if(vlen(node.origin - goal) <= pathlib_gridsize)
254     {
255         vector goalmove;
256
257         goalmove = pathlib_walknode(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 = world;
269
270     //return;
271
272     entity node;
273
274     node = findfloat(world,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 = world;
283         */
284
285         dumpnode(node);
286         node = findfloat(node,is_path_node, true);
287     }
288
289     if(openlist)
290         remove(openlist);
291
292     if(closedlist)
293         remove(closedlist);
294
295     openlist       = world;
296     closedlist     = world;
297
298 }
299
300 float Cosine_Interpolate(float a, float b, float x)
301 {
302     float ft,f;
303
304         ft = x * 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(vlen(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(vector n,vector c,vector p)
327 {
328     pathlib_walknode(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(vector from,vector to)
370 {SELFPARAM();
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_seached = 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   = self.maxs * 1.1;
433     movenode_boxmin   = self.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\n");
458     path = pathlib_mknode(from, world);
459     pathlib_close_node(path, to);
460     if(pathlib_foundgoal)
461     {
462         LOG_TRACE("AStar: Goal found on first node!\n");
463
464         open           = spawn();
465         open.owner     = open;
466         open.classname = "path_end";
467         setorigin(open,path.origin);
468
469         pathlib_cleanup();
470
471         return open;
472     }
473
474     if(pathlib_expandnode(path, from, to) <= 0)
475     {
476         LOG_TRACE("AStar path fail.\n");
477         pathlib_cleanup();
478
479         return world;
480     }
481
482     best_open_node = pathlib_getbestopen();
483     n = best_open_node;
484     pathlib_close_node(best_open_node, to);
485     if(inwater(n.origin))
486         pathlib_expandnode_box(n, from, to);
487     else
488         pathlib_expandnode_star(n, from, to);
489
490     while(pathlib_open_cnt)
491     {
492         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
493         {
494             LOG_TRACE("Path took to long to compute!\n");
495             LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
496             LOG_TRACE("Nodes -    open: ", ftos(pathlib_open_cnt),"\n");
497             LOG_TRACE("Nodes -  merged: ", ftos(pathlib_merge_cnt),"\n");
498             LOG_TRACE("Nodes -  closed: ", ftos(pathlib_closed_cnt),"\n");
499
500             pathlib_cleanup();
501             return world;
502         }
503
504         best_open_node = pathlib_getbestopen();
505         n = best_open_node;
506         pathlib_close_node(best_open_node,to);
507
508         if(inwater(n.origin))
509             pathlib_expandnode_box(n,from,to);
510         else
511             pathlib_expandnode(n,from,to);
512
513         if(pathlib_foundgoal)
514         {
515             LOG_TRACE("Target found. Rebuilding and filtering path...\n");
516             ftime = gettime(GETTIME_REALTIME);
517             ptime = ftime - ptime;
518
519             start = path_build(world,path.origin,world,world);
520             end   = path_build(world,goal_node.origin,world,start);
521             ln    = end;
522
523             open = goal_node;
524             for(open = goal_node; open.path_prev != path; open = open.path_prev)
525             {
526                 n    = path_build(ln,open.origin,open.path_prev,start);
527                 ln.path_prev = n;
528                 ln = n;
529             }
530             start.path_next = n;
531             n.path_prev = start;
532             ftime = gettime(GETTIME_REALTIME) - ftime;
533
534             ctime = gettime(GETTIME_REALTIME);
535             pathlib_cleanup();
536             ctime = gettime(GETTIME_REALTIME) - ctime;
537
538
539 #ifdef DEBUGPATHING
540             pathlib_showpath2(start);
541
542             LOG_TRACE("Time used -      pathfinding: ", ftos(ptime),"\n");
543             LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
544             LOG_TRACE("Time used -          cleanup: ", ftos(ctime),"\n");
545             LOG_TRACE("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
546             LOG_TRACE("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
547             LOG_TRACE("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
548             LOG_TRACE("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
549             LOG_TRACE("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
550             LOG_TRACE("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
551             LOG_TRACE("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
552             LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
553             LOG_TRACE("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
554             LOG_TRACE("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
555             LOG_TRACE("AStar done.\n");
556 #endif
557             return start;
558         }
559     }
560
561     LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");
562
563     pathlib_cleanup();
564
565     return world;
566 }