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