]> git.xonotic.org Git - xonotic/xonotic-data.pk3dir.git/blob - qcsrc/server/pathlib.qc
Merge branch 'master' into Mario/ctf_updates
[xonotic/xonotic-data.pk3dir.git] / qcsrc / server / pathlib.qc
1 //#define PATHLIB_RDFIELDS
2 #ifdef PATHLIB_RDFIELDS
3     #define path_next swampslug
4     #define path_prev lasertarget
5 #else
6     .entity path_next;
7     .entity path_prev;
8 #endif
9
10 #define medium spawnshieldtime
11
12 //#define DEBUGPATHING
13
14 entity openlist;
15 entity closedlist;
16 entity scraplist;
17
18 .float pathlib_node_g;
19 .float pathlib_node_h;
20 .float pathlib_node_f;
21
22 float pathlib_open_cnt;
23 float pathlib_closed_cnt;
24 float pathlib_made_cnt;
25 float pathlib_merge_cnt;
26 float pathlib_recycle_cnt;
27 float pathlib_searched_cnt;
28
29 #ifdef DEBUGPATHING
30
31 #endif
32
33 float pathlib_bestopen_seached;
34 float pathlib_bestcash_hits;
35 float pathlib_bestcash_saved;
36
37 float pathlib_gridsize;
38
39 float pathlib_movecost;
40 float pathlib_movecost_diag;
41 float pathlib_movecost_waterfactor;
42
43 float pathlib_edge_check_size;
44
45 float pathlib_foundgoal;
46 entity goal_node;
47
48 entity best_open_node;
49 .float is_path_node;
50
51
52 #ifdef DEBUGPATHING
53 float edge_show(vector point,float fsize);
54 void mark_error(vector where,float lifetime);
55 void mark_info(vector where,float lifetime);
56 entity mark_misc(vector where,float lifetime);
57
58 void pathlib_showpath(entity start)
59 {
60     entity e;
61     e = start;
62     while(e.path_next)
63     {
64         te_lightning1(e,e.origin,e.path_next.origin);
65         e = e.path_next;
66     }
67 }
68
69 void path_dbg_think()
70 {
71     pathlib_showpath(self);
72     self.nextthink = time + 1;
73 }
74
75 void __showpath2_think()
76 {
77     mark_info(self.origin,1);
78     if(self.path_next)
79     {
80         self.path_next.think     = __showpath2_think;
81         self.path_next.nextthink = time + 0.15;
82     }
83     else
84     {
85         self.owner.think     = __showpath2_think;
86         self.owner.nextthink = time + 0.15;
87     }
88 }
89
90 void pathlib_showpath2(entity path)
91 {
92     path.think     = __showpath2_think;
93     path.nextthink = time;
94 }
95
96 #endif
97
98 void pathlib_deletepath(entity start)
99 {
100     entity e;
101
102     e = findchainentity(owner, start);
103     while(e)
104     {
105         e.think = SUB_Remove;
106         e.nextthink = time;
107         e = e.chain;
108     }
109 }
110
111 float fsnap(float val,float fsize)
112 {
113     return rint(val / fsize) * fsize;
114 }
115
116 vector vsnap(vector point,float fsize)
117 {
118     vector vret;
119
120     vret_x = rint(point_x / fsize) * fsize;
121     vret_y = rint(point_y / fsize) * fsize;
122     vret_z = ceil(point_z / fsize) * fsize;
123
124     return vret;
125 }
126
127 float  walknode_stepsize;
128 vector walknode_stepup;
129 vector walknode_maxdrop;
130 vector walknode_boxup;
131 vector walknode_boxmax;
132 vector walknode_boxmin;
133 float  pathlib_movenode_goodnode;
134
135 float floor_ok(vector point)
136 {
137     float pc;
138
139     if(trace_dphitq3surfaceflags & Q3SURFACEFLAG_SKY)
140         return 0;
141
142     pc = pointcontents(point);
143
144     switch(pc)
145     {
146         case CONTENT_SOLID:
147         case CONTENT_SLIME:
148         case CONTENT_LAVA:
149         case CONTENT_SKY:
150             return 0;
151         case CONTENT_EMPTY:
152             if(pointcontents(point - '0 0 1') != CONTENT_SOLID)
153                 return 0;
154             break;
155         case CONTENT_WATER:
156             return 1;
157     }
158     if(pointcontents(point - '0 0 1') == CONTENT_SOLID)
159         return 1;
160
161     return 0;
162 }
163
164 #define inwater(point) (pointcontents(point) == CONTENT_WATER)
165 /*
166 float inwater(vector point)
167 {
168     if(pointcontents(point) == CONTENT_WATER)
169         return 1;
170     return 0;
171 }
172 */
173
174 #define _pcheck(p) traceline(p+z_up,p-z_down,MOVE_WORLDONLY,self); if (!floor_ok(trace_endpos)) return 1
175 float edge_check(vector point,float fsize)
176 {
177     vector z_up,z_down;
178
179     z_up   = '0 0 1' * fsize;
180     z_down = '0 0 1' * fsize;
181
182     _pcheck(point + ('1 1 0'  * fsize));
183     _pcheck(point + ('1 -1 0'  * fsize));
184     _pcheck(point + ('1 0 0' * fsize));
185
186     _pcheck(point + ('0 1 0'  * fsize));
187     _pcheck(point + ('0 -1 0' * fsize));
188
189     _pcheck(point + ('-1 0 0'  * fsize));
190     _pcheck(point + ('-1 1 0'  * fsize));
191     _pcheck(point + ('-1 -1 0' * fsize));
192
193     return 0;
194 }
195
196 #ifdef DEBUGPATHING
197 #define _pshow(p) mark_error(p,10)
198 float edge_show(vector point,float fsize)
199 {
200
201     _pshow(point + ('1 1 0'  * fsize));
202     _pshow(point + ('1 -1 0' * fsize));
203     _pshow(point + ('1 0 0'  * fsize));
204
205     _pshow(point + ('0 1 0'  * fsize));
206     _pshow(point + ('0 -1 0' * fsize));
207
208     _pshow(point + ('-1 0 0'  * fsize));
209     _pshow(point + ('-1 1 0'  * fsize));
210     _pshow(point + ('-1 -1 0' * fsize));
211
212     return 0;
213 }
214 #endif
215
216 var vector pathlib_movenode(vector start,vector end,float doedge);
217 vector pathlib_wateroutnode(vector start,vector end,float doedge)
218 {
219     vector surface;
220
221     pathlib_movenode_goodnode = 0;
222
223     end_x = fsnap(end_x, pathlib_gridsize);
224     end_y = fsnap(end_y, pathlib_gridsize);
225
226     traceline(end + ('0 0 0.25' * pathlib_gridsize),end - ('0 0 1' * pathlib_gridsize),MOVE_WORLDONLY,self);
227     end = trace_endpos;
228
229     if(pointcontents(end - '0 0 1') != CONTENT_SOLID)
230         return end;
231
232     for(surface = start ; surface_z < (end_z + 32); ++surface_z)
233     {
234         if(pointcontents(surface) == CONTENT_EMPTY)
235             break;
236     }
237
238     if(pointcontents(surface + '0 0 1') != CONTENT_EMPTY)
239         return end;
240
241     tracebox(start + '0 0 64', walknode_boxmin,walknode_boxmax, end + '0 0 64', MOVE_WORLDONLY, self);
242     if(trace_fraction == 1)
243         pathlib_movenode_goodnode = 1;
244
245     if(fabs(surface_z - end_z) > 32)
246         pathlib_movenode_goodnode = 0;
247
248     return end;
249 }
250
251 vector pathlib_swimnode(vector start,vector end,float doedge)
252 {
253     pathlib_movenode_goodnode = 0;
254
255     if(pointcontents(start) != CONTENT_WATER)
256         return end;
257
258     end_x = fsnap(end_x, pathlib_gridsize);
259     end_y = fsnap(end_y, pathlib_gridsize);
260
261     if(pointcontents(end) == CONTENT_EMPTY)
262         return pathlib_wateroutnode( start, end);
263
264     tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
265     if(trace_fraction == 1)
266         pathlib_movenode_goodnode = 1;
267
268     return end;
269 }
270
271 vector pathlib_flynode(vector start,vector end)
272 {
273     pathlib_movenode_goodnode = 0;
274
275     end_x = fsnap(end_x, pathlib_gridsize);
276     end_y = fsnap(end_y, pathlib_gridsize);
277
278     tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
279     if(trace_fraction == 1)
280         pathlib_movenode_goodnode = 1;
281
282     return end;
283 }
284
285 vector pathlib_walknode(vector start,vector end,float doedge)
286 {
287     vector direction,point,last_point,s,e;
288     float steps, distance, i,laststep;
289
290     pathlib_movenode_goodnode = 0;
291
292     s   = start;
293     e   = end;
294     e_z = 0;
295     s_z = 0;
296     direction  = normalize(s - e);
297
298     distance    = vlen(start - end);
299     laststep    = distance / walknode_stepsize;
300     steps       = floor(laststep);
301     laststep    = laststep - steps;
302
303     point = start;
304     s     = point + walknode_stepup;
305     e     = point - walknode_maxdrop;
306
307     traceline(s, e,MOVE_WORLDONLY,self);
308     if(trace_fraction == 1.0)
309         return trace_endpos;
310
311     if (floor_ok(trace_endpos) == 0)
312         return trace_endpos;
313
314     last_point = trace_endpos;
315
316     for(i = 0; i < steps; ++i)
317     {
318         point = last_point + direction * walknode_stepsize;
319
320         s = point + walknode_stepup;
321         e = point - walknode_maxdrop;
322         traceline(s, e,MOVE_WORLDONLY,self);
323         if(trace_fraction == 1.0)
324             return trace_endpos;
325
326         point = trace_endpos;
327         if (!floor_ok(trace_endpos))
328             return trace_endpos;
329
330         tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
331         if(trace_fraction != 1.0)
332             return trace_endpos;
333
334         if(doedge)
335         if(edge_check(point,pathlib_edge_check_size))
336             return trace_endpos;
337
338         last_point = point;
339     }
340
341     point = last_point + direction * walknode_stepsize * laststep;
342
343     point_x = fsnap(point_x, pathlib_gridsize);
344     point_y = fsnap(point_y, pathlib_gridsize);
345
346     s = point + walknode_stepup;
347     e = point - walknode_maxdrop;
348     traceline(s, e,MOVE_WORLDONLY,self);
349
350     if(trace_fraction == 1.0)
351         return trace_endpos;
352
353     point = trace_endpos;
354
355     if (!floor_ok(trace_endpos))
356         return trace_endpos;
357
358     tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
359     if(trace_fraction != 1.0)
360         return trace_endpos;
361
362     pathlib_movenode_goodnode = 1;
363     return point;
364 }
365
366 var float pathlib_cost(entity parent,vector to, float static_cost);
367 float pathlib_g_static(entity parent,vector to, float static_cost)
368 {
369     if(inwater(to))
370         return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
371     else
372         return parent.pathlib_node_g + static_cost;
373 }
374
375 float pathlib_g_static_water(entity parent,vector to, float static_cost)
376 {
377     if(inwater(to))
378         return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
379     else
380         return parent.pathlib_node_g + static_cost;
381 }
382
383 float pathlib_g_euclidean(entity parent,vector to, float static_cost)
384 {
385     return parent.pathlib_node_g + vlen(parent.origin - to);
386 }
387 float pathlib_g_euclidean_water(entity parent,vector to, float static_cost)
388 {
389     if(inwater(to))
390         return parent.pathlib_node_g + vlen(parent.origin - to) * pathlib_movecost_waterfactor;
391     else
392         return parent.pathlib_node_g + vlen(parent.origin - to);
393 }
394
395 var float(vector from,vector to) pathlib_heuristic;
396
397 /**
398     Manhattan Menas we expect to move up,down left or right
399     No diagonal moves espected. (like moving bewteen city blocks)
400 **/
401 float pathlib_h_manhattan(vector a,vector b)
402 {
403     //h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))
404
405     float h;
406     h  = fabs(a_x - b_x);
407     h += fabs(a_y - b_y);
408     h *= pathlib_gridsize;
409
410     return h;
411 }
412
413 /**
414     This heuristic consider both stright and disagonal moves
415     to have teh same cost.
416 **/
417 float pathlib_h_diagonal(vector a,vector b)
418 {
419     //h(n) = D * max(abs(n.x-goal.x), abs(n.y-goal.y))
420     float h,x,y;
421
422     x = fabs(a_x - b_x);
423     y = fabs(a_y - b_y);
424     h = pathlib_movecost * max(x,y);
425
426     return h;
427 }
428
429 /**
430     This heuristic only considers the stright line distance.
431     Will usualy mean a lower H then G meaning A* Will speand more
432     and run slower.
433 **/
434 float pathlib_h_euclidean(vector a,vector b)
435 {
436     return vlen(a - b);
437 }
438
439 /**
440     This heuristic consider both stright and disagonal moves,
441     But has a separate cost for diagonal moves.
442 **/
443 float pathlib_h_diagonal2(vector a,vector b)
444 {
445     float h_diag,h_str,h,x,y;
446
447     /*
448     h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
449     h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
450     h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
451     */
452
453     x = fabs(a_x - b_x);
454     y = fabs(a_y - b_y);
455
456     h_diag = min(x,y);
457     h_str = x + y;
458
459     h =  pathlib_movecost_diag * h_diag;
460     h += pathlib_movecost * (h_str - 2 * h_diag);
461
462     return h;
463 }
464
465 /**
466     This heuristic consider both stright and disagonal moves,
467     But has a separate cost for diagonal moves.
468
469
470 **/
471 float pathlib_h_diagonal2sdp(vector preprev,vector prev,vector point,vector end)
472 {
473     float h_diag,h_str,h,x,y,z;
474
475     //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
476     //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
477     //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
478
479     x = fabs(point_x - end_x);
480     y = fabs(point_y - end_y);
481     z = fabs(point_z - end_z);
482
483     h_diag = min3(x,y,z);
484     h_str = x + y + z;
485
486     h =  pathlib_movecost_diag * h_diag;
487     h += pathlib_movecost * (h_str - 2 * h_diag);
488
489     float m;
490     vector d1,d2;
491
492     d1 = normalize(preprev - point);
493     d2 = normalize(prev    - point);
494     m = vlen(d1-d2);
495     //bprint("pathlib_h_diagonal2sdp-M = ",ftos(m),"\n");
496
497     return h * m;
498 }
499
500
501 float pathlib_h_diagonal3(vector a,vector b)
502 {
503     float h_diag,h_str,h,x,y,z;
504
505     //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
506     //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
507     //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
508
509     x = fabs(a_x - b_x);
510     y = fabs(a_y - b_y);
511     z = fabs(a_z - b_z);
512
513     h_diag = min3(x,y,z);
514     h_str = x + y + z;
515
516     h =  pathlib_movecost_diag * h_diag;
517     h += pathlib_movecost * (h_str - 2 * h_diag);
518
519     return h;
520 }
521
522 //#define PATHLIB_USE_NODESCRAP
523 #define PATHLIB_NODEEXPIRE 0.05
524 float pathlib_scraplist_cnt;
525 entity newnode()
526 {
527     entity n;
528 #ifdef PATHLIB_USE_NODESCRAP
529     if(pathlib_scraplist_cnt)
530     {
531         n = findentity(world,owner,scraplist);
532         if(n)
533         {
534             --pathlib_scraplist_cnt;
535             ++pathlib_recycle_cnt;
536             return n;
537         }
538         else
539             pathlib_scraplist_cnt = 0;
540     }
541 #endif
542     ++pathlib_made_cnt;
543     n = spawn();
544 #ifdef PATHLIB_NODEEXPIRE
545     n.think      = SUB_Remove;
546     n.nextthink  = time + PATHLIB_NODEEXPIRE;
547     return n;
548 }
549
550 void dumpnode(entity n)
551 {
552 #ifdef PATHLIB_USE_NODESCRAP
553     ++pathlib_scraplist_cnt;
554
555     n.path_next    = world;
556     n.path_prev    = world;
557     n.is_path_node = FALSE;
558     n.owner        = scraplist;
559 #else
560     //n.is_path_node = FALSE;
561     n.think        = SUB_Remove;
562     n.nextthink    = time;
563 #endif
564 }
565
566 entity pathlib_mknode(vector where,entity parent)
567 {
568     entity node;
569
570     node              = newnode();
571     node.is_path_node = TRUE;
572     node.owner        = openlist;
573     node.path_prev    = parent;
574
575     setorigin(node, where);
576
577     ++pathlib_open_cnt;
578
579     node.medium = pointcontents(where);
580
581     return node;
582 }
583
584 var float pathlib_expandnode(entity node, vector start, vector goal);
585 float pathlib_expandnode_star(entity node, vector start, vector goal);
586 float pathlib_expandnode_box(entity node, vector start, vector goal);
587
588 var float pathlib_makenode(entity parent,vector start, vector to, vector goal,float cost);
589 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
590 {
591     entity node;
592     float h,g,f,doedge;
593     vector where;
594
595     ++pathlib_searched_cnt;
596
597     if(inwater(parent.origin))
598     {
599         pathlib_expandnode = pathlib_expandnode_box;
600         pathlib_movenode   = pathlib_swimnode;
601         doedge = 0;
602     }
603     else
604     {
605         if(inwater(to))
606         {
607             pathlib_expandnode = pathlib_expandnode_box;
608             pathlib_movenode   = pathlib_swimnode;
609             doedge = 0;
610         }
611         else
612         {
613
614             pathlib_expandnode = pathlib_expandnode_star;
615             pathlib_movenode   = pathlib_walknode;
616             doedge = 1;
617         }
618     }
619
620     where = pathlib_movenode(parent.origin,to,0);
621     if (!pathlib_movenode_goodnode)
622         return 0;
623
624     if(doedge)
625     if(edge_check(where,pathlib_edge_check_size))
626         return 0;
627
628     if(parent.path_prev)
629         pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
630
631     h = pathlib_heuristic(where,goal);
632     g = pathlib_cost(parent,where,cost);
633     f = g + h;
634
635     node = findradius(where,pathlib_gridsize * 0.75);
636     while(node)
637     {
638         if(node.is_path_node == TRUE)
639         {
640             ++pathlib_merge_cnt;
641             if(node.owner == openlist)
642             {
643                 if(node.pathlib_node_g > g)
644                 {
645                     node.pathlib_node_h = h;
646                     node.pathlib_node_g = g;
647                     node.pathlib_node_f = f;
648                     node.path_prev = parent;
649                 }
650
651                 if (!best_open_node)
652                     best_open_node = node;
653                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
654                     best_open_node = node;
655             }
656
657             return 1;
658         }
659         node = node.chain;
660     }
661
662     node = pathlib_mknode(where,parent);
663     node.pathlib_node_h = h;
664     node.pathlib_node_g = g;
665     node.pathlib_node_f = f;
666
667     if (!best_open_node)
668         best_open_node = node;
669     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
670         best_open_node = node;
671
672     return 1;
673 }
674
675 entity pathlib_getbestopen()
676 {
677     entity node;
678     entity bestnode;
679
680     if(best_open_node)
681     {
682         ++pathlib_bestcash_hits;
683         pathlib_bestcash_saved += pathlib_open_cnt;
684
685         return best_open_node;
686     }
687
688     node = findchainentity(owner,openlist);
689     if(!node)
690         return world;
691
692     bestnode = node;
693     while(node)
694     {
695         ++pathlib_bestopen_seached;
696         if(node.pathlib_node_f < bestnode.pathlib_node_f)
697             bestnode = node;
698
699         node = node.chain;
700     }
701
702     return bestnode;
703 }
704
705 void pathlib_close_node(entity node,vector goal)
706 {
707
708     if(node.owner == closedlist)
709     {
710         dprint("Pathlib: Tried to close a closed node!\n");
711         return;
712     }
713
714     if(node == best_open_node)
715         best_open_node = world;
716
717     ++pathlib_closed_cnt;
718     --pathlib_open_cnt;
719
720     node.owner = closedlist;
721
722     if(vlen(node.origin - goal) <= pathlib_gridsize)
723     {
724         vector goalmove;
725
726         goalmove = pathlib_walknode(node.origin,goal,1);
727         if(pathlib_movenode_goodnode)
728         {
729             goal_node         = node;
730             pathlib_foundgoal = TRUE;
731         }
732     }
733 }
734
735 float pathlib_expandnode_star(entity node, vector start, vector goal)
736 {
737     vector point;
738     vector where;
739     float nodecnt;
740
741     where = node.origin;
742
743     v_forward = '1 0 0';
744     v_right   = '0 1 0';
745
746     // Forward
747     point = where + v_forward * pathlib_gridsize;
748     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
749
750     // Back
751     point = where - v_forward * pathlib_gridsize;
752     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
753
754     // Right
755     point = where + v_right * pathlib_gridsize;
756     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
757
758     // Left
759     point = where - v_right * pathlib_gridsize;
760     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
761
762     // Forward-right
763     point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
764     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
765
766     // Forward-left
767     point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
768     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
769
770     // Back-right
771     point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
772     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
773
774     // Back-left
775     point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
776     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
777
778     return pathlib_open_cnt;
779 }
780
781 float pathlib_expandnode_box(entity node, vector start, vector goal)
782 {
783     vector v;
784
785     for(v_z = node.origin_z - pathlib_gridsize; v_z <= node.origin_z + pathlib_gridsize; v_z += pathlib_gridsize)
786     for(v_y = node.origin_y - pathlib_gridsize; v_y <= node.origin_y + pathlib_gridsize; v_y += pathlib_gridsize)
787     for(v_x = node.origin_x - pathlib_gridsize; v_x <= node.origin_x + pathlib_gridsize; v_x += pathlib_gridsize)
788     {
789         if(vlen(v - node.origin))
790             pathlib_makenode(node,start,v,goal,pathlib_movecost);
791     }
792
793     return pathlib_open_cnt;
794 }
795
796 void pathlib_cleanup()
797 {
798     entity node;
799
800     node = findfloat(world,is_path_node, TRUE);
801     while(node)
802     {
803         dumpnode(node);
804         node = findfloat(node,is_path_node, TRUE);
805     }
806
807     if(openlist)
808         remove(openlist);
809
810     if(closedlist)
811         remove(closedlist);
812
813     best_open_node = world;
814     openlist       = world;
815     closedlist     = world;
816 }
817
818 var float buildpath_nodefilter(vector n,vector c,vector p);
819 float buildpath_nodefilter_directional(vector n,vector c,vector p)
820 {
821     vector d1,d2;
822
823     d2 = normalize(p - c);
824     d1 = normalize(c - n);
825
826     if(vlen(d1-d2) < 0.25)
827         return 1;
828
829     return 0;
830 }
831
832 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
833 {
834     pathlib_walknode(p,n,1);
835     if(pathlib_movenode_goodnode)
836         return 1;
837
838     return 0;
839 }
840
841 entity path_build(entity next, vector where, entity prev, entity start)
842 {
843     entity path;
844
845     if(prev && next)
846         if(buildpath_nodefilter)
847             if(buildpath_nodefilter(next.origin,where,prev.origin))
848                 return next;
849
850
851     path           = spawn();
852     path.owner     = start;
853     path.path_next = next;
854
855     setorigin(path,where);
856
857     if(!next)
858         path.classname = "path_end";
859     else
860     {
861         if(!prev)
862             path.classname = "path_start";
863         else
864             path.classname = "path_node";
865     }
866
867     return path;
868 }
869
870 entity pathlib_astar(vector from,vector to)
871 {
872     entity path, start, end, open, n, ln;
873     float ptime, ftime, ctime;
874
875     ptime = gettime(GETTIME_REALTIME);
876
877     pathlib_cleanup();
878
879     // Select water<->land capable node make/link
880     pathlib_makenode     = pathlib_makenode_adaptive;
881     // Select XYZ cost estimate
882     pathlib_heuristic    = pathlib_h_diagonal3;
883     // Select distance + waterfactor cost
884     pathlib_cost         = pathlib_g_euclidean_water;
885     // Select star expander
886     pathlib_expandnode   = pathlib_expandnode_star;
887     // Select walk simulation movement test
888     pathlib_movenode     = pathlib_walknode;
889     // Filter final nodes by direction
890     buildpath_nodefilter = buildpath_nodefilter_directional;
891
892     // If the start is in water we need diffrent settings
893     if(inwater(from))
894     {
895         // Select volumetric node expaner
896         pathlib_expandnode = pathlib_expandnode_box;
897
898         // Water movement test
899         pathlib_movenode   = pathlib_swimnode;
900     }
901
902     if (!openlist)
903         openlist       = spawn();
904
905     if (!closedlist)
906         closedlist     = spawn();
907
908     if (!scraplist)
909         scraplist      = spawn();
910
911     pathlib_closed_cnt       = 0;
912     pathlib_open_cnt         = 0;
913     pathlib_made_cnt         = 0;
914     pathlib_merge_cnt        = 0;
915     pathlib_searched_cnt     = 0;
916     pathlib_bestopen_seached = 0;
917     pathlib_bestcash_hits    = 0;
918     pathlib_bestcash_saved   = 0;
919     pathlib_recycle_cnt      = 0;
920
921     pathlib_gridsize       = 128;
922     pathlib_movecost       = pathlib_gridsize;
923     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
924     pathlib_movecost_waterfactor = 1.1;
925     pathlib_foundgoal      = 0;
926
927     walknode_boxmax   = self.maxs * 1.5;
928     walknode_boxmin   = self.mins * 1.5;
929
930     pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
931
932     walknode_boxup    = '0 0 2' * self.maxs_z;
933     walknode_stepsize = 32;
934     walknode_stepup   = '0 0 1' * walknode_stepsize;
935     walknode_maxdrop  = '0 0 3' * walknode_stepsize;
936
937     from_x = fsnap(from_x,pathlib_gridsize);
938     from_y = fsnap(from_y,pathlib_gridsize);
939
940     to_x = fsnap(to_x,pathlib_gridsize);
941     to_y = fsnap(to_y,pathlib_gridsize);
942
943     dprint("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
944     path = pathlib_mknode(from,world);
945     pathlib_close_node(path,to);
946     if(pathlib_foundgoal)
947     {
948         dprint("AStar: Goal found on first node!\n");
949
950         open           = spawn();
951         open.owner     = open;
952         open.classname = "path_end";
953         setorigin(open,path.origin);
954
955         pathlib_cleanup();
956
957         return open;
958     }
959
960     if(pathlib_expandnode(path,from,to) <= 0)
961     {
962         dprint("AStar path fail.\n");
963         pathlib_cleanup();
964
965         return world;
966     }
967
968     best_open_node = pathlib_getbestopen();
969     n = best_open_node;
970     pathlib_close_node(best_open_node,to);
971     if(inwater(n.origin))
972         pathlib_expandnode_box(n,from,to);
973     else
974         pathlib_expandnode_star(n,from,to);
975
976     while(pathlib_open_cnt)
977     {
978         best_open_node = pathlib_getbestopen();
979         n = best_open_node;
980         pathlib_close_node(best_open_node,to);
981
982         if(inwater(n.origin))
983             pathlib_expandnode_box(n,from,to);
984         else
985             pathlib_expandnode(n,from,to);
986
987         if(pathlib_foundgoal)
988         {
989             dprint("Target found. Rebuilding and filtering path...\n");
990             ftime = gettime(GETTIME_REALTIME);
991             ptime = ftime - ptime;
992
993             start = path_build(world,path.origin,world,world);
994             end   = path_build(world,goal_node.origin,world,start);
995             ln    = end;
996
997             open = goal_node;
998             for(open = goal_node; open.path_prev != path; open = open.path_prev)
999             {
1000                 n    = path_build(ln,open.origin,open.path_prev,start);
1001                 ln.path_prev = n;
1002                 ln = n;
1003             }
1004             start.path_next = n;
1005             n.path_prev = start;
1006             ftime = gettime(GETTIME_REALTIME) - ftime;
1007
1008             ctime = gettime(GETTIME_REALTIME);
1009             pathlib_cleanup();
1010             ctime = gettime(GETTIME_REALTIME) - ctime;
1011
1012
1013 #ifdef DEBUGPATHING
1014             pathlib_showpath2(start);
1015
1016             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");
1017             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
1018             dprint("Time used -          cleanup: ", ftos(ctime),"\n");
1019             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
1020             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
1021             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
1022             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
1023             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
1024             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
1025             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
1026
1027         if(pathlib_recycle_cnt)
1028             dprint("Nodes -      make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
1029         if(pathlib_recycle_cnt)
1030             dprint("Nodes -          reused: ", ftos(pathlib_recycle_cnt),"\n");
1031
1032             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
1033             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
1034             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
1035             dprint("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
1036 #endif
1037             return start;
1038         }
1039     }
1040
1041     dprint("A* Faild to find a path! Try a smaller gridsize.\n");
1042
1043     pathlib_cleanup();
1044
1045     return world;
1046 }
1047
1048
1049