Scaled the weapon models down about 30% to match 2.5.2 sizes. Except the tuba, I...
[divverent/nexuiz.git] / data / 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 not (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 not(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)
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 not(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)
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 not(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 not(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     }
602     else
603     {
604         if(inwater(to))
605         {
606             pathlib_expandnode = pathlib_expandnode_box;
607             pathlib_movenode   = pathlib_swimnode;
608         }
609         else
610         {
611
612             pathlib_expandnode = pathlib_expandnode_star;
613             pathlib_movenode   = pathlib_walknode;
614             doedge = 1;
615         }
616     }
617
618     where = pathlib_movenode(parent.origin,to,0);
619     if not(pathlib_movenode_goodnode)
620         return 0;
621
622     if(doedge)
623     if(edge_check(where,pathlib_edge_check_size))
624         return 0;
625
626     if(parent.path_prev)
627         pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
628
629     h = pathlib_heuristic(where,goal);
630     g = pathlib_cost(parent,where,cost);
631     f = g + h;
632
633     node = findradius(where,pathlib_gridsize * 0.75);
634     while(node)
635     {
636         if(node.is_path_node == TRUE)
637         {
638             ++pathlib_merge_cnt;
639             if(node.owner == openlist)
640             {
641                 if(node.pathlib_node_g > g)
642                 {
643                     node.pathlib_node_h = h;
644                     node.pathlib_node_g = g;
645                     node.pathlib_node_f = f;
646                     node.path_prev = parent;
647                 }
648
649                 if not (best_open_node)
650                     best_open_node = node;
651                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
652                     best_open_node = node;
653             }
654
655             return 1;
656         }
657         node = node.chain;
658     }
659
660     node = pathlib_mknode(where,parent);
661     node.pathlib_node_h = h;
662     node.pathlib_node_g = g;
663     node.pathlib_node_f = f;
664
665     if not (best_open_node)
666         best_open_node = node;
667     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
668         best_open_node = node;
669
670     return 1;
671 }
672
673 entity pathlib_getbestopen()
674 {
675     entity node;
676     entity bestnode;
677
678     if(best_open_node)
679     {
680         ++pathlib_bestcash_hits;
681         pathlib_bestcash_saved += pathlib_open_cnt;
682
683         return best_open_node;
684     }
685
686     node = findchainentity(owner,openlist);
687     if(!node)
688         return world;
689
690     bestnode = node;
691     while(node)
692     {
693         ++pathlib_bestopen_seached;
694         if(node.pathlib_node_f < bestnode.pathlib_node_f)
695             bestnode = node;
696
697         node = node.chain;
698     }
699
700     return bestnode;
701 }
702
703 void pathlib_close_node(entity node,vector goal)
704 {
705
706     if(node.owner == closedlist)
707     {
708         dprint("Pathlib: Tried to close a closed node!\n");
709         return;
710     }
711
712     if(node == best_open_node)
713         best_open_node = world;
714
715     ++pathlib_closed_cnt;
716     --pathlib_open_cnt;
717
718     node.owner = closedlist;
719
720     if(vlen(node.origin - goal) <= pathlib_gridsize)
721     {
722         vector goalmove;
723
724         goalmove = pathlib_walknode(node.origin,goal,1);
725         if(pathlib_movenode_goodnode)
726         {
727             goal_node         = node;
728             pathlib_foundgoal = TRUE;
729         }
730     }
731 }
732
733 float pathlib_expandnode_star(entity node, vector start, vector goal)
734 {
735     vector point;
736     vector where;
737     float nodecnt;
738
739     where = node.origin;
740
741     v_forward = '1 0 0';
742     v_right   = '0 1 0';
743
744     // Forward
745     point = where + v_forward * pathlib_gridsize;
746     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
747
748     // Back
749     point = where - v_forward * pathlib_gridsize;
750     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
751
752     // Right
753     point = where + v_right * pathlib_gridsize;
754     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
755
756     // Left
757     point = where - v_right * pathlib_gridsize;
758     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
759
760     // Forward-right
761     point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
762     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
763
764     // Forward-left
765     point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
766     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
767
768     // Back-right
769     point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
770     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
771
772     // Back-left
773     point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
774     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
775
776     return pathlib_open_cnt;
777 }
778
779 float pathlib_expandnode_box(entity node, vector start, vector goal)
780 {
781     vector v;
782
783     for(v_z = node.origin_z - pathlib_gridsize; v_z <= node.origin_z + pathlib_gridsize; v_z += pathlib_gridsize)
784     for(v_y = node.origin_y - pathlib_gridsize; v_y <= node.origin_y + pathlib_gridsize; v_y += pathlib_gridsize)
785     for(v_x = node.origin_x - pathlib_gridsize; v_x <= node.origin_x + pathlib_gridsize; v_x += pathlib_gridsize)
786     {
787         if(vlen(v - node.origin))
788             pathlib_makenode(node,start,v,goal,pathlib_movecost);
789     }
790
791     return pathlib_open_cnt;
792 }
793
794 void pathlib_cleanup()
795 {
796     entity node;
797
798     node = findfloat(world,is_path_node, TRUE);
799     while(node)
800     {
801         dumpnode(node);
802         node = findfloat(node,is_path_node, TRUE);
803     }
804
805     if(openlist)
806         remove(openlist);
807
808     if(closedlist)
809         remove(closedlist);
810
811     best_open_node = world;
812     openlist       = world;
813     closedlist     = world;
814 }
815
816 var float buildpath_nodefilter(vector n,vector c,vector p);
817 float buildpath_nodefilter_directional(vector n,vector c,vector p)
818 {
819     vector d1,d2;
820
821     d2 = normalize(p - c);
822     d1 = normalize(c - n);
823
824     if(vlen(d1-d2) < 0.25)
825         return 1;
826
827     return 0;
828 }
829
830 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
831 {
832     pathlib_walknode(p,n,1);
833     if(pathlib_movenode_goodnode)
834         return 1;
835
836     return 0;
837 }
838
839 entity path_build(entity next, vector where, entity prev, entity start)
840 {
841     entity path;
842
843     if(prev && next)
844         if(buildpath_nodefilter)
845             if(buildpath_nodefilter(next.origin,where,prev.origin))
846                 return next;
847
848
849     path           = spawn();
850     path.owner     = start;
851     path.path_next = next;
852
853     setorigin(path,where);
854
855     if(!next)
856         path.classname = "path_end";
857     else
858     {
859         if(!prev)
860             path.classname = "path_start";
861         else
862             path.classname = "path_node";
863     }
864
865     return path;
866 }
867
868 entity pathlib_astar(vector from,vector to)
869 {
870     entity path, start, end, open, n, ln;
871     float ptime, ftime, ctime;
872
873     ptime = gettime(GETTIME_REALTIME);
874
875     pathlib_cleanup();
876
877     // Select water<->land capable node make/link
878     pathlib_makenode     = pathlib_makenode_adaptive;
879     // Select XYZ cost estimate
880     pathlib_heuristic    = pathlib_h_diagonal3;
881     // Select distance + waterfactor cost
882     pathlib_cost         = pathlib_g_euclidean_water;
883     // Select star expander
884     pathlib_expandnode   = pathlib_expandnode_star;
885     // Select walk simulation movement test
886     pathlib_movenode     = pathlib_walknode;
887     // Filter final nodes by direction
888     buildpath_nodefilter = buildpath_nodefilter_directional;
889
890     // If the start is in water we need diffrent settings
891     if(inwater(from))
892     {
893         // Select volumetric node expaner
894         pathlib_expandnode = pathlib_expandnode_box;
895
896         // Water movement test
897         pathlib_movenode   = pathlib_swimnode;
898     }
899
900     if not(openlist)
901         openlist       = spawn();
902
903     if not(closedlist)
904         closedlist     = spawn();
905
906     if not(scraplist)
907         scraplist      = spawn();
908
909     pathlib_closed_cnt       = 0;
910     pathlib_open_cnt         = 0;
911     pathlib_made_cnt         = 0;
912     pathlib_merge_cnt        = 0;
913     pathlib_searched_cnt     = 0;
914     pathlib_bestopen_seached = 0;
915     pathlib_bestcash_hits    = 0;
916     pathlib_bestcash_saved   = 0;
917     pathlib_recycle_cnt      = 0;
918
919     pathlib_gridsize       = 128;
920     pathlib_movecost       = pathlib_gridsize;
921     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
922     pathlib_movecost_waterfactor = 1.1;
923     pathlib_foundgoal      = 0;
924
925     walknode_boxmax   = self.maxs * 1.5;
926     walknode_boxmin   = self.mins * 1.5;
927
928     pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
929
930     walknode_boxup    = '0 0 2' * self.maxs_z;
931     walknode_stepsize = 32;
932     walknode_stepup   = '0 0 1' * walknode_stepsize;
933     walknode_maxdrop  = '0 0 3' * walknode_stepsize;
934
935     from_x = fsnap(from_x,pathlib_gridsize);
936     from_y = fsnap(from_y,pathlib_gridsize);
937
938     to_x = fsnap(to_x,pathlib_gridsize);
939     to_y = fsnap(to_y,pathlib_gridsize);
940
941     dprint("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
942     path = pathlib_mknode(from,world);
943     pathlib_close_node(path,to);
944     if(pathlib_foundgoal)
945     {
946         dprint("AStar: Goal found on first node!\n");
947
948         open           = spawn();
949         open.owner     = open;
950         open.classname = "path_end";
951         setorigin(open,path.origin);
952
953         pathlib_cleanup();
954
955         return open;
956     }
957
958     if(pathlib_expandnode(path,from,to) <= 0)
959     {
960         dprint("AStar path fail.\n");
961         pathlib_cleanup();
962
963         return world;
964     }
965
966     best_open_node = pathlib_getbestopen();
967     n = best_open_node;
968     pathlib_close_node(best_open_node,to);
969     if(inwater(n.origin))
970         pathlib_expandnode_box(n,from,to);
971     else
972         pathlib_expandnode_star(n,from,to);
973
974     while(pathlib_open_cnt)
975     {
976         best_open_node = pathlib_getbestopen();
977         n = best_open_node;
978         pathlib_close_node(best_open_node,to);
979
980         if(inwater(n.origin))
981             pathlib_expandnode_box(n,from,to);
982         else
983             pathlib_expandnode(n,from,to);
984
985         if(pathlib_foundgoal)
986         {
987             dprint("Target found. Rebuilding and filtering path...\n");
988             ftime = gettime(GETTIME_REALTIME);
989             ptime = ftime - ptime;
990
991             start = path_build(world,path.origin,world,world);
992             end   = path_build(world,goal_node.origin,world,start);
993             ln    = end;
994
995             open = goal_node;
996             for(open = goal_node; open.path_prev != path; open = open.path_prev)
997             {
998                 n    = path_build(ln,open.origin,open.path_prev,start);
999                 ln.path_prev = n;
1000                 ln = n;
1001             }
1002             start.path_next = n;
1003             n.path_prev = start;
1004             ftime = gettime(GETTIME_REALTIME) - ftime;
1005
1006             ctime = gettime(GETTIME_REALTIME);
1007             pathlib_cleanup();
1008             ctime = gettime(GETTIME_REALTIME) - ctime;
1009
1010
1011 #ifdef DEBUGPATHING
1012             pathlib_showpath2(start);
1013
1014             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");
1015             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
1016             dprint("Time used -          cleanup: ", ftos(ctime),"\n");
1017             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
1018             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
1019             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
1020             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
1021             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
1022             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
1023             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
1024
1025         if(pathlib_recycle_cnt)
1026             dprint("Nodes -      make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
1027         if(pathlib_recycle_cnt)
1028             dprint("Nodes -          reused: ", ftos(pathlib_recycle_cnt),"\n");
1029
1030             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
1031             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
1032             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
1033             dprint("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
1034 #endif
1035             return start;
1036         }
1037     }
1038
1039     dprint("A* Faild to find a path! Try a smaller gridsize.\n");
1040
1041     pathlib_cleanup();
1042
1043     return world;
1044 }
1045
1046
1047