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