1 (* Copyright (C) DooM 2D:Forever Developers
2 *
3 * This program is free software: you can redistribute it and/or modify
4 * it under the terms of the GNU General Public License as published by
5 * the Free Software Foundation, either version 3 of the License, or
6 * (at your option) any later version.
7 *
8 * This program is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *)
16 {$INCLUDE ../shared/a_modes.inc}
17 {$DEFINE aabbtree_many_asserts}
18 {$DEFINE aabbtree_query_count}
21 interface
23 // ////////////////////////////////////////////////////////////////////////// //
24 type
31 // ////////////////////////////////////////////////////////////////////////// //
32 type
34 public
38 public
45 // ////////////////////////////////////////////////////////////////////////// //
46 type
48 public
51 private
58 public
68 // return true if the current AABB contains the AABB given in parameter
72 // return true if the current AABB is overlapping with the AABB in parameter
73 // two AABBs overlap if they overlap in the two axes at the same time
76 // ray direction must be normalized
88 // ////////////////////////////////////////////////////////////////////////// //
89 (* Dynamic AABB tree (bounding volume hierarchy)
90 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
91 * Copyright (c) 2010-2016 Daniel Chappuis
92 *
93 * This software is provided 'as-is', without any express or implied warranty.
94 * In no event will the authors be held liable for any damages arising from the
95 * use of this software.
96 *
97 * Permission is granted to anyone to use this software for any purpose,
98 * including commercial applications, and to alter it and redistribute it
99 * freely, subject to the following restrictions:
100 *
101 * 1. The origin of this software must not be misrepresented; you must not claim
102 * that you wrote the original software. If you use this software in a
103 * product, an acknowledgment in the product documentation would be
104 * appreciated but is not required.
105 *
106 * 2. Altered source versions must be plainly marked as such, and must not be
107 * misrepresented as being the original software.
108 *
109 * 3. This notice may not be removed or altered from any source distribution.
110 *)
111 // ////////////////////////////////////////////////////////////////////////// //
112 (*
113 * This class implements a dynamic AABB tree that is used for broad-phase
114 * collision detection. This data structure is inspired by Nathanael Presson's
115 * dynamic tree implementation in BulletPhysics. The following implementation is
116 * based on the one from Erin Catto in Box2D as described in the book
117 * "Introduction to Game Physics with Box2D" by Ian Parberry.
118 *)
119 type
122 private
129 private
135 public
148 // ////////////////////////////////////////////////////////////////////////// //
149 // Dynamic AABB Tree: can be used to speed up broad phase in various engines
150 type
152 private
153 type
156 public
160 public
161 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
163 //nextNodeId: Integer;
164 // a node is either a leaf (has data) or is an internal node (has children)
165 children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
166 //TODO: `flesh` can be united with `children`
168 // height of the node in the tree (-1 for free nodes)
170 // fat axis aligned bounding box (AABB) corresponding to the node
172 public
173 // return true if the node is a leaf of the tree
178 //property flesh: Integer read children[0] write children[0];
184 public
185 // return `true` to stop
186 type TForEachLeafCB = function (abody: TTreeFlesh; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
188 public
189 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
190 // also inflated in direction of the linear motion of the body by mutliplying the
191 // followin constant with the linear velocity and the elapsed time between two frames
194 private
197 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
201 // extra AABB Gap used to allow the collision shape to move a little bit
202 // without triggering a large modification of the tree which can be costly
205 private
216 public
217 {$IFDEF aabbtree_query_count}
219 {$ENDIF}
221 public
222 // called when a overlapping node has been found during the call to forEachAABBOverlap()
223 // return `true` to stop
225 type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
235 public
239 // clear all the nodes and reset the tree
249 // return `false` for invalid flesh
252 // insert an object into the tree
253 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
254 // AABB for static object will not be "fat" (simple optimization)
255 // WARNING! inserting the same object several times *WILL* break everything!
258 // remove an object from the tree
259 // WARNING: ids of removed objects can be reused on later insertions!
262 (** update the dynamic tree after an object has moved.
263 *
264 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
265 * otherwise, the corresponding node is removed and reinserted into the tree.
266 * the method returns true if the object has been reinserted into the tree.
267 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
268 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
269 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
270 *
271 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
272 *
273 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
274 *
275 * return `true` if the tree was modified.
276 *)
277 function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
281 function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
291 implementation
293 uses
294 SysUtils;
297 // ////////////////////////////////////////////////////////////////////////// //
298 function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
299 function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
302 // ////////////////////////////////////////////////////////////////////////// //
304 begin
319 // ////////////////////////////////////////////////////////////////////////// //
321 var
323 begin
330 begin
338 begin
347 // ////////////////////////////////////////////////////////////////////////// //
357 begin
364 begin
375 var
377 begin
392 var
394 begin
402 begin
411 begin
412 result :=
419 begin
425 begin
427 // exit with no intersection if found separated along any axis
434 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
435 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
436 function AABB2D.intersects (var ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
437 var
440 begin
441 // ok with coplanars
444 // do X
446 begin
453 // do Y
455 begin
459 // tmin
463 // tmax
470 begin
474 end
475 else
476 begin
482 var
485 begin
487 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
490 // nope, do it hard way
500 // ////////////////////////////////////////////////////////////////////////// //
502 function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; begin result := (dist >= 0) and (flesh <> nil); end;
505 // ////////////////////////////////////////////////////////////////////////// //
510 begin
520 // ////////////////////////////////////////////////////////////////////////// //
521 // allocate and return a node to use in the tree
523 var
525 begin
526 // if there is no more allocated node to use
528 begin
530 // allocate more nodes in the tree
534 // initialize the allocated nodes
536 begin
544 // get the next free node
546 {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF}
555 // release a node
557 begin
568 // insert a leaf node in the tree
569 // the process of inserting a new leaf node in the dynamic tree is described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
571 var
578 begin
579 // if the tree is empty
581 begin
584 exit;
589 // find the best sibling node for the new node
593 begin
597 // compute the merged AABB
602 // compute the cost of making the current node the sibling of the new node
605 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
608 // compute the cost of descending into the left child
611 begin
613 end
614 else
615 begin
619 // compute the cost of descending into the right child
622 begin
624 end
625 else
626 begin
630 // if the cost of making the current node a sibling of the new node is smaller than the cost of going down into the left or right child
633 // it is cheaper to go down into a child of the current node, choose the best child
634 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
640 // create a new parent for the new node and the sibling node
648 // if the sibling node was not the root node
650 begin
653 begin
655 end
656 else
657 begin
664 end
665 else
666 begin
667 // if the sibling node was the root node
675 // move up in the tree to change the AABBs that have changed
679 begin
680 // balance the sub-tree of the current node if it is not balanced
690 // recompute the height of the node in the tree
694 // recompute the AABB of the node
704 // remove a leaf node from the tree
706 var
709 begin
713 // if we are removing the root node (root node is a leaf in this case)
720 begin
722 end
723 else
724 begin
728 // if the parent of the node to remove is not the root node
730 begin
731 // destroy the parent node
733 begin
735 end
736 else
737 begin
738 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
744 // now, we need to recompute the AABBs of the node on the path back to the root and make sure that the tree is still balanced
747 begin
748 // balance the current sub-tree if necessary
753 // get the two children of the current node
757 // recompute the AABB and the height of the current node
759 mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
764 end
765 else
766 begin
767 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
775 // balance the sub-tree of a given node using left or right rotations
776 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
777 // this method returns the new root node id
779 var
783 begin
788 // if the node is a leaf or the height of A's sub-tree is less than 2
789 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
791 // get the two children nodes
799 // compute the factor of the left and right sub-trees
802 // if the right node C is 2 higher than left node B
804 begin
819 begin
821 begin
823 end
824 else
825 begin
826 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
829 end
830 else
831 begin
838 // if the right node C was higher than left node B because of the F node
840 begin
845 // recompute the AABB of node A and C
849 // recompute the height of node A and C
854 end
855 else
856 begin
857 // if the right node C was higher than left node B because of node G
862 // recompute the AABB of node A and C
866 // recompute the height of node A and C
873 // return the new root of the sub-tree
875 exit;
878 // if the left node B is 2 higher than right node C
880 begin
895 begin
897 begin
899 end
900 else
901 begin
902 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
905 end
906 else
907 begin
914 // if the left node B was higher than right node C because of the F node
916 begin
921 // recompute the AABB of node A and B
925 // recompute the height of node A and B
930 end
931 else
932 begin
933 // if the left node B was higher than right node C because of node G
938 // recompute the AABB of node A and B
942 // recompute the height of node A and B
949 // return the new root of the sub-tree
951 exit;
954 // if the sub-tree is balanced, return the current root node
959 // compute the height of a given node in the tree
961 var
964 begin
968 // if the node is a leaf, its height is zero
971 // compute the height of the left and right sub-tree
975 // return the height of the node
980 // internally add an object into the tree
982 var
984 begin
985 // get the next available node (or allocate new ones if necessary)
988 // create the fat aabb to use in the tree
991 begin
998 // set the height of the node in the tree
1001 // insert the new leaf node in the tree
1007 // return the id of the node
1012 // initialize the tree
1014 var
1016 begin
1022 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1025 // initialize the allocated nodes
1027 begin
1037 // also, checks if the tree structure is valid (for debugging purpose)
1040 var
1044 begin
1047 // if it is the root
1049 // get the children nodes
1053 // if the current node is a leaf
1055 begin
1058 end
1059 else
1060 begin
1063 // check that the children node Ids are valid
1066 // check that the children nodes have the correct parent node
1069 // check the height of node
1072 // check the AABB of the node
1078 // recursively check the children nodes
1084 begin
1087 // recursively check each node
1092 // return `true` from visitor to stop immediately
1093 // checker should check if this node should be considered to further checking
1094 // returns tree node if visitor says stop or -1
1096 var
1102 var
1104 begin
1106 begin
1107 // use "small stack"
1110 end
1111 else
1112 begin
1113 // use "big stack"
1116 begin
1117 // reuse
1119 end
1120 else
1121 begin
1122 // grow
1131 begin
1134 begin
1135 // use "small stack"
1138 end
1139 else
1140 begin
1141 // use "big stack"
1147 var
1150 begin
1152 //if not assigned(visitor) then begin result := -1; exit; end;
1153 try
1154 {$IFDEF aabbtree_query_count}
1157 {$ENDIF}
1159 // start from root node
1162 // while there are still nodes to visit
1164 begin
1165 // get the next node id to visit
1167 // skip it if it is a nil node
1170 // get the corresponding node
1172 // should we investigate this node?
1174 begin
1175 // if the node is a leaf
1177 begin
1178 // call visitor on it
1181 begin
1184 end
1185 else
1186 begin
1187 // if the node is not a leaf, we need to visit its children
1195 finally
1201 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1202 // extra AABB Gap used to allow the collision shape to move a little bit without triggering a large modification of the tree which can be costly
1204 begin
1211 begin
1217 // clear all the nodes and reset the tree
1219 begin
1225 function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1228 // return the root AABB of the tree
1230 begin
1231 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mNodeCount));{$ENDIF}
1236 // does the given id represents a valid object?
1237 // WARNING: ids of removed objects can be reused on later insertions!
1239 begin
1244 // get object by nodeid; can return nil for invalid ids
1246 begin
1247 if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil;
1250 // get fat object AABB by nodeid; returns random shit for invalid ids
1252 begin
1253 if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].free) then aabb := mNodes[nodeid].aabb else aabb.setX0Y0X1Y1(0, 0, -1, -1);
1257 // insert an object into the tree
1258 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1259 // AABB for static object will not be "fat" (simple optimization)
1260 // WARNING! inserting the same object several times *WILL* break everything!
1262 var
1265 begin
1274 // remove an object from the tree
1275 // WARNING: ids of removed objects can be reused on later insertions!
1277 begin
1278 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1279 // remove the node from the tree
1285 function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
1286 var
1288 begin
1289 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1291 if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree');
1293 // if the new AABB is still inside the fat AABB of the node
1294 if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end;
1296 // if the new AABB is outside the fat AABB, we remove the corresponding node
1299 // compute the fat AABB by inflating the AABB with a constant gap
1302 begin
1309 // inflate the fat AABB in direction of the linear motion of the AABB
1311 begin
1313 end
1314 else
1315 begin
1319 begin
1321 end
1322 else
1323 begin
1329 // reinsert the node into the tree
1336 // report all shapes overlapping with the AABB given in parameter
1338 var
1341 begin
1344 begin
1351 // report body that contains the given point, or nil
1353 var
1356 begin
1359 begin
1361 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF}
1366 // segment querying method
1367 function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
1368 var
1376 begin
1381 var
1383 begin
1385 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1387 begin
1391 exit;
1393 // if the user returned a positive fraction
1395 begin
1396 // we update the maxFraction value and the ray AABB using the new maximum fraction
1398 begin
1402 // fix curb here
1403 //curb := cura+dir*hitFraction;
1411 begin
1423 // normalize