91a98190c352c73f6d06dd310c9f98007ef3209d
1 {$INCLUDE ../shared/a_modes.inc}
2 {$DEFINE aabbtree_many_asserts}
3 {$DEFINE aabbtree_query_count}
6 interface
8 // ////////////////////////////////////////////////////////////////////////// //
9 type
14 // ////////////////////////////////////////////////////////////////////////// //
15 type
17 public
21 public
28 // ////////////////////////////////////////////////////////////////////////// //
29 type
31 public
34 private
41 public
51 // return true if the current AABB contains the AABB given in parameter
55 // return true if the current AABB is overlapping with the AABB in parameter
56 // two AABBs overlap if they overlap in the two axes at the same time
59 // ray direction must be normalized
71 // ////////////////////////////////////////////////////////////////////////// //
72 (* Dynamic AABB tree (bounding volume hierarchy)
73 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
74 * Copyright (c) 2010-2016 Daniel Chappuis
75 *
76 * This software is provided 'as-is', without any express or implied warranty.
77 * In no event will the authors be held liable for any damages arising from the
78 * use of this software.
79 *
80 * Permission is granted to anyone to use this software for any purpose,
81 * including commercial applications, and to alter it and redistribute it
82 * freely, subject to the following restrictions:
83 *
84 * 1. The origin of this software must not be misrepresented; you must not claim
85 * that you wrote the original software. If you use this software in a
86 * product, an acknowledgment in the product documentation would be
87 * appreciated but is not required.
88 *
89 * 2. Altered source versions must be plainly marked as such, and must not be
90 * misrepresented as being the original software.
91 *
92 * 3. This notice may not be removed or altered from any source distribution.
93 *)
94 // ////////////////////////////////////////////////////////////////////////// //
95 (*
96 * This class implements a dynamic AABB tree that is used for broad-phase
97 * collision detection. This data structure is inspired by Nathanael Presson's
98 * dynamic tree implementation in BulletPhysics. The following implementation is
99 * based on the one from Erin Catto in Box2D as described in the book
100 * "Introduction to Game Physics with Box2D" by Ian Parberry.
101 *)
102 type
105 private
112 private
118 public
131 // ////////////////////////////////////////////////////////////////////////// //
132 // Dynamic AABB Tree: can be used to speed up broad phase in various engines
133 type
135 private
136 type
139 public
143 public
144 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
146 //nextNodeId: Integer;
147 // a node is either a leaf (has data) or is an internal node (has children)
148 children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
149 //TODO: `flesh` can be united with `children`
150 //flesh: Integer;
151 // height of the node in the tree (-1 for free nodes)
153 // fat axis aligned bounding box (AABB) corresponding to the node
155 public
156 // return true if the node is a leaf of the tree
167 public
168 // return `true` to stop
169 type TForEachLeafCB = function (abody: Integer; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
171 public
172 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
173 // also inflated in direction of the linear motion of the body by mutliplying the
174 // followin constant with the linear velocity and the elapsed time between two frames
177 private
180 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
184 // extra AABB Gap used to allow the collision shape to move a little bit
185 // without triggering a large modification of the tree which can be costly
188 private
199 public
200 {$IFDEF aabbtree_query_count}
202 {$ENDIF}
204 public
205 // called when a overlapping node has been found during the call to forEachAABBOverlap()
206 // return `true` to stop
208 type TSegQueryCallback = function (abody: Integer; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
218 public
222 // clear all the nodes and reset the tree
232 // return `false` for invalid flesh
235 // insert an object into the tree
236 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
237 // AABB for static object will not be "fat" (simple optimization)
238 // WARNING! inserting the same object several times *WILL* break everything!
241 // remove an object from the tree
242 // WARNING: ids of removed objects can be reused on later insertions!
245 (** update the dynamic tree after an object has moved.
246 *
247 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
248 * otherwise, the corresponding node is removed and reinserted into the tree.
249 * the method returns true if the object has been reinserted into the tree.
250 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
251 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
252 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
253 *
254 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
255 *
256 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
257 *
258 * return `true` if the tree was modified.
259 *)
260 function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
264 function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
274 implementation
276 uses
277 SysUtils;
280 // ////////////////////////////////////////////////////////////////////////// //
281 function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
282 function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
285 // ////////////////////////////////////////////////////////////////////////// //
287 begin
302 // ////////////////////////////////////////////////////////////////////////// //
304 var
306 begin
313 begin
321 begin
330 // ////////////////////////////////////////////////////////////////////////// //
340 begin
347 begin
358 var
360 begin
375 var
377 begin
385 begin
394 begin
395 result :=
402 begin
408 begin
410 // exit with no intersection if found separated along any axis
417 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
418 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
419 function AABB2D.intersects (var ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
420 var
423 begin
424 // ok with coplanars
427 // do X
429 begin
436 // do Y
438 begin
442 // tmin
446 // tmax
453 begin
457 end
458 else
459 begin
465 var
468 begin
470 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
473 // nope, do it hard way
483 // ////////////////////////////////////////////////////////////////////////// //
485 function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; begin result := (dist >= 0) and (flesh >= 0); end;
488 // ////////////////////////////////////////////////////////////////////////// //
493 begin
497 //flesh: Integer;
503 // ////////////////////////////////////////////////////////////////////////// //
504 // allocate and return a node to use in the tree
506 var
508 begin
509 // if there is no more allocated node to use
511 begin
513 // allocate more nodes in the tree
517 // initialize the allocated nodes
519 begin
527 // get the next free node
529 {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF}
538 // release a node
540 begin
551 // insert a leaf node in the tree
552 // 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
554 var
561 begin
562 // if the tree is empty
564 begin
567 exit;
572 // find the best sibling node for the new node
576 begin
580 // compute the merged AABB
585 // compute the cost of making the current node the sibling of the new node
588 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
591 // compute the cost of descending into the left child
594 begin
596 end
597 else
598 begin
602 // compute the cost of descending into the right child
605 begin
607 end
608 else
609 begin
613 // 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
616 // it is cheaper to go down into a child of the current node, choose the best child
617 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
623 // create a new parent for the new node and the sibling node
631 // if the sibling node was not the root node
633 begin
636 begin
638 end
639 else
640 begin
647 end
648 else
649 begin
650 // if the sibling node was the root node
658 // move up in the tree to change the AABBs that have changed
662 begin
663 // balance the sub-tree of the current node if it is not balanced
673 // recompute the height of the node in the tree
677 // recompute the AABB of the node
687 // remove a leaf node from the tree
689 var
692 begin
696 // if we are removing the root node (root node is a leaf in this case)
703 begin
705 end
706 else
707 begin
711 // if the parent of the node to remove is not the root node
713 begin
714 // destroy the parent node
716 begin
718 end
719 else
720 begin
721 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
727 // 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
730 begin
731 // balance the current sub-tree if necessary
736 // get the two children of the current node
740 // recompute the AABB and the height of the current node
742 mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
747 end
748 else
749 begin
750 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
758 // balance the sub-tree of a given node using left or right rotations
759 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
760 // this method returns the new root node id
762 var
766 begin
771 // if the node is a leaf or the height of A's sub-tree is less than 2
772 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
774 // get the two children nodes
782 // compute the factor of the left and right sub-trees
785 // if the right node C is 2 higher than left node B
787 begin
802 begin
804 begin
806 end
807 else
808 begin
809 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
812 end
813 else
814 begin
821 // if the right node C was higher than left node B because of the F node
823 begin
828 // recompute the AABB of node A and C
832 // recompute the height of node A and C
837 end
838 else
839 begin
840 // if the right node C was higher than left node B because of node G
845 // recompute the AABB of node A and C
849 // recompute the height of node A and C
856 // return the new root of the sub-tree
858 exit;
861 // if the left node B is 2 higher than right node C
863 begin
878 begin
880 begin
882 end
883 else
884 begin
885 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
888 end
889 else
890 begin
897 // if the left node B was higher than right node C because of the F node
899 begin
904 // recompute the AABB of node A and B
908 // recompute the height of node A and B
913 end
914 else
915 begin
916 // if the left node B was higher than right node C because of node G
921 // recompute the AABB of node A and B
925 // recompute the height of node A and B
932 // return the new root of the sub-tree
934 exit;
937 // if the sub-tree is balanced, return the current root node
942 // compute the height of a given node in the tree
944 var
947 begin
951 // if the node is a leaf, its height is zero
954 // compute the height of the left and right sub-tree
958 // return the height of the node
963 // internally add an object into the tree
965 var
967 begin
968 // get the next available node (or allocate new ones if necessary)
971 // create the fat aabb to use in the tree
974 begin
981 // set the height of the node in the tree
984 // insert the new leaf node in the tree
990 // return the id of the node
995 // initialize the tree
997 var
999 begin
1005 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1008 // initialize the allocated nodes
1010 begin
1020 // also, checks if the tree structure is valid (for debugging purpose)
1023 var
1027 begin
1030 // if it is the root
1032 // get the children nodes
1036 // if the current node is a leaf
1038 begin
1041 end
1042 else
1043 begin
1046 // check that the children node Ids are valid
1049 // check that the children nodes have the correct parent node
1052 // check the height of node
1055 // check the AABB of the node
1061 // recursively check the children nodes
1067 begin
1070 // recursively check each node
1075 // return `true` from visitor to stop immediately
1076 // checker should check if this node should be considered to further checking
1077 // returns tree node if visitor says stop or -1
1079 var
1085 var
1087 begin
1089 begin
1090 // use "small stack"
1093 end
1094 else
1095 begin
1096 // use "big stack"
1099 begin
1100 // reuse
1102 end
1103 else
1104 begin
1105 // grow
1114 begin
1117 begin
1118 // use "small stack"
1121 end
1122 else
1123 begin
1124 // use "big stack"
1130 var
1133 begin
1135 //if not assigned(visitor) then begin result := -1; exit; end;
1136 try
1137 {$IFDEF aabbtree_query_count}
1140 {$ENDIF}
1142 // start from root node
1145 // while there are still nodes to visit
1147 begin
1148 // get the next node id to visit
1150 // skip it if it is a nil node
1153 // get the corresponding node
1155 // should we investigate this node?
1157 begin
1158 // if the node is a leaf
1160 begin
1161 // call visitor on it
1164 begin
1167 end
1168 else
1169 begin
1170 // if the node is not a leaf, we need to visit its children
1178 finally
1184 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1185 // 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
1187 begin
1194 begin
1200 // clear all the nodes and reset the tree
1202 begin
1208 function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1211 // return the root AABB of the tree
1213 begin
1214 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mNodeCount));{$ENDIF}
1219 // does the given id represents a valid object?
1220 // WARNING: ids of removed objects can be reused on later insertions!
1222 begin
1227 // get object by nodeid; can return nil for invalid ids
1229 begin
1230 if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := -1;
1233 // get fat object AABB by nodeid; returns random shit for invalid ids
1235 begin
1236 if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].free) then aabb := mNodes[nodeid].aabb else aabb.setX0Y0X1Y1(0, 0, -1, -1);
1240 // insert an object into the tree
1241 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1242 // AABB for static object will not be "fat" (simple optimization)
1243 // WARNING! inserting the same object several times *WILL* break everything!
1245 var
1248 begin
1257 // remove an object from the tree
1258 // WARNING: ids of removed objects can be reused on later insertions!
1260 begin
1261 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1262 // remove the node from the tree
1268 function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
1269 var
1271 begin
1272 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1274 if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree');
1276 // if the new AABB is still inside the fat AABB of the node
1277 if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end;
1279 // if the new AABB is outside the fat AABB, we remove the corresponding node
1282 // compute the fat AABB by inflating the AABB with a constant gap
1285 begin
1292 // inflate the fat AABB in direction of the linear motion of the AABB
1294 begin
1296 end
1297 else
1298 begin
1302 begin
1304 end
1305 else
1306 begin
1312 // reinsert the node into the tree
1319 // report all shapes overlapping with the AABB given in parameter
1321 var
1324 begin
1327 begin
1334 // report body that contains the given point, or -1
1336 var
1339 begin
1342 begin
1344 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF}
1349 // segment querying method
1350 function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
1351 var
1359 begin
1364 var
1366 begin
1368 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1370 begin
1374 exit;
1376 // if the user returned a positive fraction
1378 begin
1379 // we update the maxFraction value and the ray AABB using the new maximum fraction
1381 begin
1385 // fix curb here
1386 //curb := cura+dir*hitFraction;
1394 begin
1406 // normalize