-
Notifications
You must be signed in to change notification settings - Fork 3
/
RN_DetourNavMeshQuery.pas
4213 lines (3676 loc) · 142 KB
/
RN_DetourNavMeshQuery.pas
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
//
// Copyright (c) 2009-2010 Mikko Mononen [email protected]
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
{$POINTERMATH ON}
unit RN_DetourNavMeshQuery;
interface
uses Classes, RN_DetourCommon, RN_DetourNavMesh, RN_DetourNavMeshHelper, RN_DetourNode, RN_DetourStatus;
type
// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter.
// On certain platforms indirect or virtual function call is expensive. The default
// setting is to use non-virtual functions, the actual implementations of the functions
// are declared as inline for maximum speed.
//#define DT_VIRTUAL_QUERYFILTER 1
/// Defines polygon filtering and traversal costs for navigation mesh query operations.
/// @ingroup detour
TdtQueryFilter = class
private
m_areaCost: array [0..DT_MAX_AREAS-1] of Single; ///< Cost per area type. (Used by default implementation.)
m_includeFlags: Word; ///< Flags for polygons that can be visited. (Used by default implementation.)
m_excludeFlags: Word; ///< Flags for polygons that should not be visted. (Used by default implementation.)
public
constructor Create;
/// Returns true if the polygon can be visited. (I.e. Is traversable.)
/// @param[in] ref The reference id of the polygon test.
/// @param[in] tile The tile containing the polygon.
/// @param[in] poly The polygon to test.
function passFilter(ref: TdtPolyRef; tile: PdtMeshTile; poly: PdtPoly): Boolean;
/// Returns cost to move from the beginning to the end of a line segment
/// that is fully contained within a polygon.
/// @param[in] pa The start position on the edge of the previous and current polygon. [(x, y, z)]
/// @param[in] pb The end position on the edge of the current and next polygon. [(x, y, z)]
/// @param[in] prevRef The reference id of the previous polygon. [opt]
/// @param[in] prevTile The tile containing the previous polygon. [opt]
/// @param[in] prevPoly The previous polygon. [opt]
/// @param[in] curRef The reference id of the current polygon.
/// @param[in] curTile The tile containing the current polygon.
/// @param[in] curPoly The current polygon.
/// @param[in] nextRef The refernece id of the next polygon. [opt]
/// @param[in] nextTile The tile containing the next polygon. [opt]
/// @param[in] nextPoly The next polygon. [opt]
function getCost(pa, pb: PSingle;
prevRef: TdtPolyRef; prevTile: PdtMeshTile; prevPoly: PdtPoly;
curRef: TdtPolyRef; curTile: PdtMeshTile; curPoly: PdtPoly;
nextRef: TdtPolyRef; nextTile: PdtMeshTile; nextPoly: PdtPoly): Single;
/// @name Getters and setters for the default implementation data.
///@{
/// Returns the traversal cost of the area.
/// @param[in] i The id of the area.
/// @returns The traversal cost of the area.
function getAreaCost(i: Integer): Single; { return m_areaCost[i]; }
/// Sets the traversal cost of the area.
/// @param[in] i The id of the area.
/// @param[in] cost The new cost of traversing the area.
procedure setAreaCost(i: Integer; cost: Single); { m_areaCost[i] = cost; }
/// Returns the include flags for the filter.
/// Any polygons that include one or more of these flags will be
/// included in the operation.
function getIncludeFlags(): Word; { return m_includeFlags; }
/// Sets the include flags for the filter.
/// @param[in] flags The new flags.
procedure setIncludeFlags(flags: Word); { m_includeFlags = flags; }
/// Returns the exclude flags for the filter.
/// Any polygons that include one ore more of these flags will be
/// excluded from the operation.
function getExcludeFlags(): Word; { return m_excludeFlags; }
/// Sets the exclude flags for the filter.
/// @param[in] flags The new flags.
procedure setExcludeFlags(flags: Word); { m_excludeFlags = flags; }
///@}
end;
/// Provides information about raycast hit
/// filled by dtNavMeshQuery::raycast
/// @ingroup detour
PdtRaycastHit = ^TdtRaycastHit;
TdtRaycastHit = record
/// The hit parameter. (FLT_MAX if no wall hit.)
t: Single;
/// hitNormal The normal of the nearest wall hit. [(x, y, z)]
hitNormal: array [0..2] of Single;
/// Pointer to an array of reference ids of the visited polygons. [opt]
path: PdtPolyRef;
/// The number of visited polygons. [opt]
pathCount: Integer;
/// The maximum number of polygons the @p path array can hold.
maxPath: Integer;
/// The cost of the path until hit.
pathCost: Single;
end;
TdtQueryData = record
status: TdtStatus;
lastBestNode: PdtNode;
lastBestNodeCost: Single;
startRef, endRef: TdtPolyRef;
startPos, endPos: array [0..2] of Single;
filter: TdtQueryFilter;
options: Cardinal;
raycastLimitSqr: Single;
end;
/// Provides the ability to perform pathfinding related queries against
/// a navigation mesh.
/// @ingroup detour
TdtNavMeshQuery = class
private
m_nav: TdtNavMesh; ///< Pointer to navmesh data.
m_tinyNodePool: TdtNodePool;///< Pointer to small node pool.
m_nodePool: TdtNodePool; ///< Pointer to node pool.
m_openList: TdtNodeQueue; ///< Pointer to open list queue.
m_query: TdtQueryData; ///< Sliced query state.
public
constructor Create;
destructor Destroy; override;
/// Initializes the query object.
/// @param[in] nav Pointer to the dtNavMesh object to use for all queries.
/// @param[in] maxNodes Maximum number of search nodes. [Limits: 0 < value <= 65536]
/// @returns The status flags for the query.
function init(nav: TdtNavMesh; maxNodes: Integer): TdtStatus;
/// @name Standard Pathfinding Functions
// /@{
/// Finds a path from the start polygon to the end polygon.
/// @param[in] startRef The refrence id of the start polygon.
/// @param[in] endRef The reference id of the end polygon.
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// [(polyRef) * @p pathCount]
/// @param[out] pathCount The number of polygons returned in the @p path array.
/// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1]
function findPath(startRef, endRef: TdtPolyRef;
startPos, endPos: PSingle;
filter: TdtQueryFilter;
path: PdtPolyRef; pathCount: PInteger; maxPath: Integer): TdtStatus;
/// Finds the straight path from the start to the end position within the polygon corridor.
/// @param[in] startPos Path start position. [(x, y, z)]
/// @param[in] endPos Path end position. [(x, y, z)]
/// @param[in] path An array of polygon references that represent the path corridor.
/// @param[in] pathSize The number of polygons in the @p path array.
/// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount].
/// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt]
/// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt]
/// @param[out] straightPathCount The number of points in the straight path.
/// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0]
/// @param[in] options Query options. (see: #dtStraightPathOptions)
/// @returns The status flags for the query.
function findStraightPath(startPos, endPos: PSingle;
path: PdtPolyRef; pathSize: Integer;
straightPath: PSingle; straightPathFlags: PByte; straightPathRefs: PdtPolyRef;
straightPathCount: PInteger; maxStraightPath: Integer; options: Integer = 0): TdtStatus;
///@}
/// @name Sliced Pathfinding Functions
/// Common use case:
/// -# Call initSlicedFindPath() to initialize the sliced path query.
/// -# Call updateSlicedFindPath() until it returns complete.
/// -# Call finalizeSlicedFindPath() to get the path.
///@{
/// Intializes a sliced path query.
/// @param[in] startRef The refrence id of the start polygon.
/// @param[in] endRef The reference id of the end polygon.
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] options query options (see: #dtFindPathOptions)
/// @returns The status flags for the query.
function initSlicedFindPath(startRef, endRef: TdtPolyRef;
startPos, endPos: PSingle;
filter: TdtQueryFilter; options: Integer = 0): TdtStatus;
/// Updates an in-progress sliced path query.
/// @param[in] maxIter The maximum number of iterations to perform.
/// @param[out] doneIters The actual number of iterations completed. [opt]
/// @returns The status flags for the query.
function updateSlicedFindPath(maxIter: Integer; doneIters: PInteger): TdtStatus;
/// Finalizes and returns the results of a sliced path query.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// [(polyRef) * @p pathCount]
/// @param[out] pathCount The number of polygons returned in the @p path array.
/// @param[in] maxPath The max number of polygons the path array can hold. [Limit: >= 1]
/// @returns The status flags for the query.
function finalizeSlicedFindPath(path: PdtPolyRef; pathCount: PInteger; maxPath: Integer): TdtStatus;
/// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest
/// polygon on the existing path that was visited during the search.
/// @param[in] existing An array of polygon references for the existing path.
/// @param[in] existingSize The number of polygon in the @p existing array.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// [(polyRef) * @p pathCount]
/// @param[out] pathCount The number of polygons returned in the @p path array.
/// @param[in] maxPath The max number of polygons the @p path array can hold. [Limit: >= 1]
/// @returns The status flags for the query.
function finalizeSlicedFindPathPartial(existing: PdtPolyRef; existingSize: Integer;
path: PdtPolyRef; pathCount: PInteger; maxPath: Integer): TdtStatus;
///@}
/// @name Dijkstra Search Functions
/// @{
/// Finds the polygons along the navigation graph that touch the specified circle.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] radius The radius of the search circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt]
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// Zero if a result polygon has no parent. [opt]
/// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt]
/// @param[out] resultCount The number of polygons found. [opt]
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
function findPolysAroundCircle(startRef: TdtPolyRef; centerPos: PSingle; radius: Single;
filter: TdtQueryFilter;
resultRef, resultParent: PdtPolyRef; resultCost: PSingle;
resultCount: PInteger; maxResult: Integer): TdtStatus;
/// Finds the polygons along the naviation graph that touch the specified convex polygon.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] verts The vertices describing the convex polygon. (CCW)
/// [(x, y, z) * @p nverts]
/// @param[in] nverts The number of vertices in the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt]
/// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a
/// result polygon has no parent. [opt]
/// @param[out] resultCost The search cost from the centroid point to the polygon. [opt]
/// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
function findPolysAroundShape(startRef: TdtPolyRef; verts: PSingle; nverts: Integer;
filter: TdtQueryFilter;
resultRef, resultParent: PdtPolyRef; resultCost: PSingle;
resultCount: PInteger; maxResult: Integer): TdtStatus;
/// @}
/// @name Local Query Functions
///@{
/// Finds the polygon nearest to the specified center point.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] nearestRef The reference id of the nearest polygon.
/// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)]
/// @returns The status flags for the query.
function findNearestPoly(center, extents: PSingle; filter: TdtQueryFilter; nearestRef: PdtPolyRef; nearestPt: PSingle): TdtStatus;
/// Finds polygons that overlap the search box.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] polys The reference ids of the polygons that overlap the query box.
/// @param[out] polyCount The number of polygons in the search result.
/// @param[in] maxPolys The maximum number of polygons the search result can hold.
/// @returns The status flags for the query.
function queryPolygons(center, extents: PSingle; filter: TdtQueryFilter; polys: PdtPolyRef; polyCount: PInteger; maxPolys: Integer): TdtStatus;
/// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the query circle. [(x, y, z)]
/// @param[in] radius The radius of the query circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the circle.
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// Zero if a result polygon has no parent. [opt]
/// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
function findLocalNeighbourhood(startRef: TdtPolyRef; centerPos: PSingle; radius: Single;
filter: TdtQueryFilter;
resultRef, resultParent: PdtPolyRef;
resultCount: PInteger; maxResult: Integer): TdtStatus;
/// Moves from the start to the end position constrained to the navigation mesh.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)]
/// @param[in] endPos The desired end position of the mover. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultPos The result position of the mover. [(x, y, z)]
/// @param[out] visited The reference ids of the polygons visited during the move.
/// @param[out] visitedCount The number of polygons visited during the move.
/// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold.
/// @returns The status flags for the query.
function moveAlongSurface(startRef: TdtPolyRef; startPos, endPos: PSingle;
filter: TdtQueryFilter;
resultPos: PSingle; visited: PdtPolyRef; visitedCount: PInteger; maxVisitedSize: Integer): TdtStatus;
/// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position.
/// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[out] t The hit parameter. (FLT_MAX if no wall hit.)
/// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] path The reference ids of the visited polygons. [opt]
/// @param[out] pathCount The number of visited polygons. [opt]
/// @param[in] maxPath The maximum number of polygons the @p path array can hold.
/// @returns The status flags for the query.
function raycast(startRef: TdtPolyRef; startPos, endPos: PSingle;
filter: TdtQueryFilter;
t, hitNormal: PSingle; path: PdtPolyRef; pathCount: PInteger; maxPath: Integer): TdtStatus; overload;
/// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] flags govern how the raycast behaves. See dtRaycastOptions
/// @param[out] hit Pointer to a raycast hit structure which will be filled by the results.
/// @param[in] prevRef parent of start ref. Used during for cost calculation [opt]
/// @returns The status flags for the query.
function raycast(startRef: TdtPolyRef; startPos, endPos: PSingle;
filter: TdtQueryFilter; options: Cardinal;
hit: PdtRaycastHit; prevRef: TdtPolyRef = 0): TdtStatus; overload;
/// Finds the distance from the specified position to the nearest polygon wall.
/// @param[in] startRef The reference id of the polygon containing @p centerPos.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] maxRadius The radius of the search circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] hitDist The distance to the nearest wall from @p centerPos.
/// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)]
/// @param[out] hitNormal The normalized ray formed from the wall point to the
/// source point. [(x, y, z)]
/// @returns The status flags for the query.
function findDistanceToWall(startRef: TdtPolyRef; centerPos: PSingle; maxRadius: Single;
filter: TdtQueryFilter;
hitDist, hitPos, hitNormal: PSingle): TdtStatus;
/// Returns the segments for the specified polygon, optionally including portals.
/// @param[in] ref The reference id of the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
/// @param[out] segmentRefs The reference ids of each segment's neighbor polygon.
/// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount]
/// @param[out] segmentCount The number of segments returned.
/// @param[in] maxSegments The maximum number of segments the result arrays can hold.
/// @returns The status flags for the query.
function getPolyWallSegments(ref: TdtPolyRef; filter: TdtQueryFilter;
segmentVerts: PSingle; segmentRefs: PdtPolyRef; segmentCount: PInteger;
maxSegments: Integer): TdtStatus;
/// Returns random location on navmesh.
/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] frand Function returning a random number [0..1).
/// @param[out] randomRef The reference id of the random location.
/// @param[out] randomPt The random location.
/// @returns The status flags for the query.
function findRandomPoint(filter: TdtQueryFilter; //float ( *frand)(),
randomRef: PdtPolyRef; randomPt: PSingle): TdtStatus;
/// Returns random location on navmesh within the reach of specified location.
/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
/// The location is not exactly constrained by the circle, but it limits the visited polygons.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] frand Function returning a random number [0..1).
/// @param[out] randomRef The reference id of the random location.
/// @param[out] randomPt The random location. [(x, y, z)]
/// @returns The status flags for the query.
function findRandomPointAroundCircle(startRef: TdtPolyRef; centerPos: PSingle; maxRadius: Single;
filter: TdtQueryFilter; //{float ( *frand)(),
randomRef: PdtPolyRef; randomPt: PSingle): TdtStatus;
/// Finds the closest point on the specified polygon.
/// @param[in] ref The reference id of the polygon.
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point on the polygon. [(x, y, z)]
/// @param[out] posOverPoly True of the position is over the polygon.
/// @returns The status flags for the query.
function closestPointOnPoly(ref: TdtPolyRef; pos, closest: PSingle; posOverPoly: PBoolean): TdtStatus;
/// Returns a point on the boundary closest to the source point if the source point is outside the
/// polygon's xz-bounds.
/// @param[in] ref The reference id to the polygon.
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point. [(x, y, z)]
/// @returns The status flags for the query.
function closestPointOnPolyBoundary(ref: TdtPolyRef; pos, closest: PSingle): TdtStatus;
/// Gets the height of the polygon at the provided position using the height detail. (Most accurate.)
/// @param[in] ref The reference id of the polygon.
/// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)]
/// @param[out] height The height at the surface of the polygon.
/// @returns The status flags for the query.
function getPolyHeight(ref: TdtPolyRef; pos, height: PSingle): TdtStatus;
/// @}
/// @name Miscellaneous Functions
/// @{
/// Returns true if the polygon reference is valid and passes the filter restrictions.
/// @param[in] ref The polygon reference to check.
/// @param[in] filter The filter to apply.
function isValidPolyRef(ref: TdtPolyRef; filter: TdtQueryFilter): Boolean;
/// Returns true if the polygon reference is in the closed list.
/// @param[in] ref The reference id of the polygon to check.
/// @returns True if the polygon is in closed list.
function isInClosedList(ref: TdtPolyRef): Boolean;
/// Gets the node pool.
/// @returns The node pool.
property getNodePool: TdtNodePool read m_nodePool;
/// Gets the navigation mesh the query object is using.
/// @return The navigation mesh the query object is using.
property getAttachedNavMesh: TdtNavMesh read m_nav;
/// @}
private
/// Returns neighbour tile based on side.
{ function getNeighbourTileAt(x, y, side: Integer): PdtMeshTile;}
/// Queries polygons within a tile.
function queryPolygonsInTile(tile: PdtMeshTile; qmin, qmax: PSingle; filter: TdtQueryFilter;
polys: PdtPolyRef; maxPolys: Integer): Integer;
/// Returns portal points between two polygons.
function getPortalPoints(from, &to: TdtPolyRef; left, right: PSingle;
fromType, toType: PByte): TdtStatus; overload;
function getPortalPoints(from: TdtPolyRef; fromPoly: PdtPoly; fromTile: PdtMeshTile;
&to: TdtPolyRef; toPoly: PdtPoly; toTile: PdtMeshTile;
left, right: PSingle): TdtStatus; overload;
/// Returns edge mid point between two polygons.
function getEdgeMidPoint(from, &to: TdtPolyRef; mid: PSingle): TdtStatus; overload;
function getEdgeMidPoint(from: TdtPolyRef; fromPoly: PdtPoly; fromTile: PdtMeshTile;
&to: TdtPolyRef; toPoly: PdtPoly; toTile: PdtMeshTile;
mid: PSingle): TdtStatus; overload;
// Appends vertex to a straight path
function appendVertex(pos: PSingle; flags: Integer; ref: TdtPolyRef;
straightPath: PSingle; straightPathFlags: PByte; straightPathRefs: PdtPolyRef;
straightPathCount: PInteger; maxStraightPath: Integer): TdtStatus;
// Appends intermediate portal points to a straight path.
function appendPortals(startIdx, endIdx: Integer; endPos: PSingle; path: PdtPolyRef;
straightPath: PSingle; straightPathFlags: PByte; straightPathRefs: PdtPolyRef;
straightPathCount: PInteger; maxStraightPath, options: Integer): TdtStatus;
end;
/// Allocates a query object using the Detour allocator.
/// @return An allocated query object, or null on failure.
/// @ingroup detour
function dtAllocNavMeshQuery(): TdtNavMeshQuery;
/// Frees the specified query object using the Detour allocator.
/// @param[in] query A query object allocated using #dtAllocNavMeshQuery
/// @ingroup detour
procedure dtFreeNavMeshQuery(var navmesh: TdtNavMeshQuery);
implementation
uses Math;
/// @class dtQueryFilter
///
/// <b>The Default Implementation</b>
///
/// At construction: All area costs default to 1.0. All flags are included
/// and none are excluded.
///
/// If a polygon has both an include and an exclude flag, it will be excluded.
///
/// The way filtering works, a navigation mesh polygon must have at least one flag
/// set to ever be considered by a query. So a polygon with no flags will never
/// be considered.
///
/// Setting the include flags to 0 will result in all polygons being excluded.
///
/// <b>Custom Implementations</b>
///
/// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class.
///
/// Implement a custom query filter by overriding the virtual passFilter()
/// and getCost() functions. If this is done, both functions should be as
/// fast as possible. Use cached local copies of data rather than accessing
/// your own objects where possible.
///
/// Custom implementations do not need to adhere to the flags or cost logic
/// used by the default implementation.
///
/// In order for A* searches to work properly, the cost should be proportional to
/// the travel distance. Implementing a cost modifier less than 1.0 is likely
/// to lead to problems during pathfinding.
///
/// @see dtNavMeshQuery
constructor TdtQueryFilter.Create;
var i: Integer;
begin
m_includeFlags := $ffff;
m_excludeFlags := 0;
for i := 0 to DT_MAX_AREAS - 1 do
m_areaCost[i] := 1.0;
end;
function TdtQueryFilter.passFilter(ref: TdtPolyRef; tile: PdtMeshTile; poly: PdtPoly): Boolean;
begin
Result := ((poly.flags and m_includeFlags) <> 0) and ((poly.flags and m_excludeFlags) = 0);
end;
function TdtQueryFilter.getCost(pa, pb: PSingle;
prevRef: TdtPolyRef; prevTile: PdtMeshTile; prevPoly: PdtPoly;
curRef: TdtPolyRef; curTile: PdtMeshTile; curPoly: PdtPoly;
nextRef: TdtPolyRef; nextTile: PdtMeshTile; nextPoly: PdtPoly): Single;
begin
Result := dtVdist(pa, pb) * m_areaCost[curPoly.getArea()];
end;
function TdtQueryFilter.getAreaCost(i: Integer): Single; begin Result := m_areaCost[i]; end;
procedure TdtQueryFilter.setAreaCost(i: Integer; cost: Single); begin m_areaCost[i] := cost; end;
function TdtQueryFilter.getIncludeFlags(): Word; begin Result := m_includeFlags; end;
procedure TdtQueryFilter.setIncludeFlags(flags: Word); begin m_includeFlags := flags; end;
function TdtQueryFilter.getExcludeFlags(): Word; begin Result := m_excludeFlags; end;
procedure TdtQueryFilter.setExcludeFlags(flags: Word); begin m_excludeFlags := flags; end;
const H_SCALE = 0.999; // Search heuristic scale.
function dtAllocNavMeshQuery(): TdtNavMeshQuery;
begin
Result := TdtNavMeshQuery.Create;
end;
procedure dtFreeNavMeshQuery(var navmesh: TdtNavMeshQuery);
begin
if (navmesh = nil) then Exit;
navMesh.Free;
navMesh := nil;
end;
//////////////////////////////////////////////////////////////////////////////////////////
/// @class dtNavMeshQuery
///
/// For methods that support undersized buffers, if the buffer is too small
/// to hold the entire result set the return status of the method will include
/// the #DT_BUFFER_TOO_SMALL flag.
///
/// Constant member functions can be used by multiple clients without side
/// effects. (E.g. No change to the closed list. No impact on an in-progress
/// sliced path query. Etc.)
///
/// Walls and portals: A @e wall is a polygon segment that is
/// considered impassable. A @e portal is a passable segment between polygons.
/// A portal may be treated as a wall based on the dtQueryFilter used for a query.
///
/// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery()
constructor TdtNavMeshQuery.Create;
begin
inherited;
end;
destructor TdtNavMeshQuery.Destroy;
begin
m_tinyNodePool.Free;
m_nodePool.Free;
m_openList.Free;
inherited;
end;
/// @par
///
/// Must be the first function called after construction, before other
/// functions are used.
///
/// This function can be used multiple times.
function TdtNavMeshQuery.init(nav: TdtNavMesh; maxNodes: Integer): TdtStatus;
begin
m_nav := nav;
if (m_nodePool = nil) or (m_nodePool.getMaxNodes < maxNodes) then
begin
if (m_nodePool <> nil) then
begin
m_nodePool.Free;
m_nodePool := nil;
end;
m_nodePool := TdtNodePool.Create(maxNodes, dtNextPow2(maxNodes div 4));
if (m_nodePool = nil) then
Exit(DT_FAILURE or DT_OUT_OF_MEMORY);
end
else
begin
m_nodePool.clear();
end;
if (m_tinyNodePool = nil) then
begin
m_tinyNodePool := TdtNodePool.Create(64, 32);
if (m_tinyNodePool = nil) then
Exit(DT_FAILURE or DT_OUT_OF_MEMORY);
end
else
begin
m_tinyNodePool.clear();
end;
// TODO: check the open list size too.
if (m_openList = nil) or (m_openList.getCapacity < maxNodes) then
begin
if (m_openList <> nil) then
begin
m_openList.Free;
m_openList := nil;
end;
m_openList := TdtNodeQueue.Create(maxNodes);
if (m_openList = nil) then
Exit(DT_FAILURE or DT_OUT_OF_MEMORY);
end
else
begin
m_openList.clear();
end;
Result := DT_SUCCESS;
end;
function TdtNavMeshQuery.findRandomPoint(filter: TdtQueryFilter; //float ( *frand)(),
randomRef: PdtPolyRef; randomPt: PSingle): TdtStatus;
var tile: PdtMeshTile; tsum,area,u,areaSum,polyArea,rs,rt,h: Single; i,j: Integer; t: PdtMeshTile; poly,p: PdtPoly;
polyRef,base,ref: TdtPolyRef; va,vb,vc,v: PSingle; verts: array [0..3*DT_VERTS_PER_POLYGON-1] of Single;
areas: array [0..DT_VERTS_PER_POLYGON-1] of Single; pt: array [0..2] of Single; status: TdtStatus;
begin
Assert(m_nav <> nil);
// Randomly pick one tile. Assume that all tiles cover roughly the same area.
tile := nil;
tsum := 0.0;
for i := 0 to m_nav.getMaxTiles - 1 do
begin
t := m_nav.getTile(i);
if (t = nil) or(t.header = nil) then continue;
// Choose random tile using reservoi sampling.
area := 1.0; // Could be tile area too.
tsum := tsum + area;
u := Random();
if (u*tsum <= area) then
tile := t;
end;
if (tile = nil) then
Exit(DT_FAILURE);
// Randomly pick one polygon weighted by polygon area.
poly := nil;
polyRef := 0;
base := m_nav.getPolyRefBase(tile);
areaSum := 0.0;
for i := 0 to tile.header.polyCount - 1 do
begin
p := @tile.polys[i];
// Do not return off-mesh connection polygons.
if (p.getType() <> DT_POLYTYPE_GROUND) then
continue;
// Must pass filter
ref := base or TdtPolyRef(i);
if (not filter.passFilter(ref, tile, p)) then
continue;
// Calc area of the polygon.
polyArea := 0.0;
for j := 2 to p.vertCount - 1 do
begin
va := @tile.verts[p.verts[0]*3];
vb := @tile.verts[p.verts[j-1]*3];
vc := @tile.verts[p.verts[j]*3];
polyArea := polyArea + dtTriArea2D(va,vb,vc);
end;
// Choose random polygon weighted by area, using reservoi sampling.
areaSum := areaSum + polyArea;
u := Random();
if (u*areaSum <= polyArea) then
begin
poly := p;
polyRef := ref;
end;
end;
if (poly = nil) then
Exit(DT_FAILURE);
// Randomly pick point on polygon.
v := @tile.verts[poly.verts[0]*3];
dtVcopy(@verts[0*3],v);
for j := 1 to poly.vertCount - 1 do
begin
v := @tile.verts[poly.verts[j]*3];
dtVcopy(@verts[j*3],v);
end;
rs := Random();
rt := Random();
dtRandomPointInConvexPoly(@verts[0], poly.vertCount, @areas[0], rs, rt, @pt[0]);
h := 0.0;
status := getPolyHeight(polyRef, @pt[0], @h);
if (dtStatusFailed(status)) then
Exit(status);
pt[1] := h;
dtVcopy(randomPt, @pt[0]);
randomRef^ := polyRef;
Result := DT_SUCCESS;
end;
function TdtNavMeshQuery.findRandomPointAroundCircle(startRef: TdtPolyRef; centerPos: PSingle; maxRadius: Single;
filter: TdtQueryFilter; //float ( *frand)(),
randomRef: PdtPolyRef; randomPt: PSingle): TdtStatus;
var startTile,randomTile,bestTile,parentTile,neighbourTile: PdtMeshTile; startPoly,randomPoly,bestPoly,parentPoly,neighbourPoly: PdtPoly;
startNode,bestNode,neighbourNode: PdtNode; status: TdtStatus; radiusSqr,areaSum: Single; randomPolyRef,bestRef,parentRef,neighbourRef: TdtPolyRef;
polyArea,u,s,t,h,tseg,distSqr,total: Single; j: Integer; va,vb,vc,v: PSingle; i: Cardinal; link: PdtLink; va3,vb3,pt: array [0..2] of Single;
verts: array [0..3*DT_VERTS_PER_POLYGON-1] of Single; areas: array [0..DT_VERTS_PER_POLYGON-1] of Single; stat: TdtStatus;
begin
Assert(m_nav <> nil);
Assert(m_nodePool <> nil);
Assert(m_openList <> nil);
// Validate input
if (startRef = 0) or (not m_nav.isValidPolyRef(startRef)) then
Exit(DT_FAILURE or DT_INVALID_PARAM);
startTile := nil;
startPoly := nil;
m_nav.getTileAndPolyByRefUnsafe(startRef, @startTile, @startPoly);
if (not filter.passFilter(startRef, startTile, startPoly)) then
Exit(DT_FAILURE or DT_INVALID_PARAM);
m_nodePool.clear();
m_openList.clear();
startNode := m_nodePool.getNode(startRef);
dtVcopy(@startNode.pos, centerPos);
startNode.pidx := 0;
startNode.cost := 0;
startNode.total := 0;
startNode.id := startRef;
startNode.flags := DT_NODE_OPEN;
m_openList.push(startNode);
status := DT_SUCCESS;
radiusSqr := Sqr(maxRadius);
areaSum := 0.0;
randomTile := nil;
randomPoly := nil;
randomPolyRef := 0;
while (not m_openList.empty()) do
begin
bestNode := m_openList.pop();
bestNode.flags := bestNode.flags and not DT_NODE_OPEN;
bestNode.flags := bestNode.flags or DT_NODE_CLOSED;
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
bestRef := bestNode.id;
bestTile := nil;
bestPoly := nil;
m_nav.getTileAndPolyByRefUnsafe(bestRef, @bestTile, @bestPoly);
// Place random locations on on ground.
if (bestPoly.getType() = DT_POLYTYPE_GROUND) then
begin
// Calc area of the polygon.
polyArea := 0.0;
for j := 2 to bestPoly.vertCount - 1 do
begin
va := @bestTile.verts[bestPoly.verts[0]*3];
vb := @bestTile.verts[bestPoly.verts[j-1]*3];
vc := @bestTile.verts[bestPoly.verts[j]*3];
polyArea := polyArea + dtTriArea2D(va,vb,vc);
end;
// Choose random polygon weighted by area, using reservoi sampling.
areaSum := areaSum + polyArea;
u := Random;
if (u*areaSum <= polyArea) then
begin
randomTile := bestTile;
randomPoly := bestPoly;
randomPolyRef := bestRef;
end;
end;
// Get parent poly and tile.
parentRef := 0;
parentTile := nil;
parentPoly := nil;
if (bestNode.pidx <> 0) then
parentRef := m_nodePool.getNodeAtIdx(bestNode.pidx).id;
if (parentRef <> 0) then
m_nav.getTileAndPolyByRefUnsafe(parentRef, @parentTile, @parentPoly);
i := bestPoly.firstLink;
while (i <> DT_NULL_LINK) do
begin
link := @bestTile.links[i];
neighbourRef := link.ref;
// Skip invalid neighbours and do not follow back to parent.
if (neighbourRef = 0) or (neighbourRef = parentRef) then
begin
i := bestTile.links[i].next;
continue;
end;
// Expand to neighbour
neighbourTile := nil;
neighbourPoly := nil;
m_nav.getTileAndPolyByRefUnsafe(neighbourRef, @neighbourTile, @neighbourPoly);
// Do not advance if the polygon is excluded by the filter.
if (not filter.passFilter(neighbourRef, neighbourTile, neighbourPoly)) then
begin
i := bestTile.links[i].next;
continue;
end;
// Find edge and calc distance to the edge.
if (getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, @va3[0], @vb3[0]) = 0) then
begin
i := bestTile.links[i].next;
continue;
end;
// If the circle is not touching the next polygon, skip it.
distSqr := dtDistancePtSegSqr2D(centerPos, @va3[0], @vb3[0], @tseg);
if (distSqr > radiusSqr) then
begin
i := bestTile.links[i].next;
continue;
end;
neighbourNode := m_nodePool.getNode(neighbourRef);
if (neighbourNode = nil) then
begin
status := status or DT_OUT_OF_NODES;
i := bestTile.links[i].next;
continue;
end;
if (neighbourNode.flags and DT_NODE_CLOSED) <> 0 then
begin
i := bestTile.links[i].next;
continue;
end;
// Cost
if (neighbourNode.flags = 0) then
dtVlerp(@neighbourNode.pos, @va3[0], @vb3[0], 0.5);
total := bestNode.total + dtVdist(@bestNode.pos, @neighbourNode.pos);
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode.flags and DT_NODE_OPEN) <> 0) and (total >= neighbourNode.total) then
begin
i := bestTile.links[i].next;
continue;
end;
neighbourNode.id := neighbourRef;
neighbourNode.flags := (neighbourNode.flags and not DT_NODE_CLOSED);
neighbourNode.pidx := m_nodePool.getNodeIdx(bestNode);
neighbourNode.total := total;
if (neighbourNode.flags and DT_NODE_OPEN) <> 0 then
begin
m_openList.modify(neighbourNode);
end
else
begin
neighbourNode.flags := DT_NODE_OPEN;
m_openList.push(neighbourNode);
end;
i := bestTile.links[i].next;
end;
end;
if (randomPoly = nil) then
Exit(DT_FAILURE);
// Randomly pick point on polygon.
v := @randomTile.verts[randomPoly.verts[0]*3];
dtVcopy(@verts[0*3],v);
for j := 1 to randomPoly.vertCount - 1 do
begin
v := @randomTile.verts[randomPoly.verts[j]*3];
dtVcopy(@verts[j*3],v);
end;
s := Random;
t := Random;
dtRandomPointInConvexPoly(@verts[0], randomPoly.vertCount, @areas[0], s, t, @pt[0]);
h := 0.0;
stat := getPolyHeight(randomPolyRef, @pt[0], @h);
if (dtStatusFailed(status)) then
Exit(stat);
pt[1] := h;
dtVcopy(randomPt, @pt[0]);
randomRef^ := randomPolyRef;
Result := DT_SUCCESS;
end;
//////////////////////////////////////////////////////////////////////////////////////////
/// @par
///
/// Uses the detail polygons to find the surface height. (Most accurate.)
///
/// @p pos does not have to be within the bounds of the polygon or navigation mesh.
///
/// See closestPointOnPolyBoundary() for a limited but faster option.
///
function TdtNavMeshQuery.closestPointOnPoly(ref: TdtPolyRef; pos, closest: PSingle; posOverPoly: PBoolean): TdtStatus;
var tile: PdtMeshTile; poly: PdtPoly; v0,v1: PSingle; d0,d1,u: Single; ip: Cardinal; pd: PdtPolyDetail;
verts: array [0..DT_VERTS_PER_POLYGON*3-1] of Single; edged: array [0..DT_VERTS_PER_POLYGON-1] of Single;
edget: array [0..DT_VERTS_PER_POLYGON-1] of Single; nv,i,j,k: Integer; dmin: Single; imin: Integer; va,vb: PSingle; t: PByte;
v: array [0..2] of PSingle; h: Single;
begin
//dtAssert(m_nav);
tile := nil;
poly := nil;
if (dtStatusFailed(m_nav.getTileAndPolyByRef(ref, @tile, @poly))) then
Exit(DT_FAILURE or DT_INVALID_PARAM);
if (tile = nil) then
Exit(DT_FAILURE or DT_INVALID_PARAM);
// Off-mesh connections don't have detail polygons.
if (poly.getType() = DT_POLYTYPE_OFFMESH_CONNECTION) then
begin
v0 := @tile.verts[poly.verts[0]*3];
v1 := @tile.verts[poly.verts[1]*3];
d0 := dtVdist(pos, v0);
d1 := dtVdist(pos, v1);
u := d0 / (d0+d1);
dtVlerp(closest, v0, v1, u);