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
26 // ////////////////////////////////////////////////////////////////////////// //
27 type
34 // ////////////////////////////////////////////////////////////////////////// //
35 type
37 public
41 public
54 // ////////////////////////////////////////////////////////////////////////// //
55 type
57 public
60 private
67 public
81 // return true if the current AABB contains the AABB given in parameter
85 // return true if the current AABB is overlapping with the AABB in parameter
86 // two AABBs overlap if they overlap in the two axes at the same time
89 // ray direction must be normalized
101 // ////////////////////////////////////////////////////////////////////////// //
102 (* Dynamic AABB tree (bounding volume hierarchy)
103 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
104 * Copyright (c) 2010-2016 Daniel Chappuis
105 *
106 * This software is provided 'as-is', without any express or implied warranty.
107 * In no event will the authors be held liable for any damages arising from the
108 * use of this software.
109 *
110 * Permission is granted to anyone to use this software for any purpose,
111 * including commercial applications, and to alter it and redistribute it
112 * freely, subject to the following restrictions:
113 *
114 * 1. The origin of this software must not be misrepresented; you must not claim
115 * that you wrote the original software. If you use this software in a
116 * product, an acknowledgment in the product documentation would be
117 * appreciated but is not required.
118 *
119 * 2. Altered source versions must be plainly marked as such, and must not be
120 * misrepresented as being the original software.
121 *
122 * 3. This notice may not be removed or altered from any source distribution.
123 *)
124 // ////////////////////////////////////////////////////////////////////////// //
125 (*
126 * This class implements a dynamic AABB tree that is used for broad-phase
127 * collision detection. This data structure is inspired by Nathanael Presson's
128 * dynamic tree implementation in BulletPhysics. The following implementation is
129 * based on the one from Erin Catto in Box2D as described in the book
130 * "Introduction to Game Physics with Box2D" by Ian Parberry.
131 *)
132 // ////////////////////////////////////////////////////////////////////////// //
133 // Dynamic AABB Tree: can be used to speed up broad phase in various engines
134 type
136 private
137 type
140 public
144 public
145 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
147 //nextNodeId: Integer;
148 // a node is either a leaf (has data) or is an internal node (has children)
149 children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
150 //TODO: `flesh` can be united with `children`
152 // height of the node in the tree (-1 for free nodes)
154 // fat axis aligned bounding box (AABB) corresponding to the node
157 public
158 // return true if the node is a leaf of the tree
163 //property flesh: Integer read children[0] write children[0];
167 //TVisitVisitorCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested;
169 public
170 // return `true` to stop
171 type TForEachLeafCB = function (abody: TTreeFlesh; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
173 public
174 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
175 // also inflated in direction of the linear motion of the body by mutliplying the
176 // followin constant with the linear velocity and the elapsed time between two frames
179 private
182 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
186 // extra AABB Gap used to allow the collision shape to move a little bit
187 // without triggering a large modification of the tree which can be costly
190 public
191 // called when a overlapping node has been found during the call to forEachAABBOverlap()
192 // return `true` to stop
194 type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
204 private
213 function visit (checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer;
215 public
216 {$IFDEF aabbtree_query_count}
218 {$ENDIF}
220 public
224 // clear all the nodes and reset the tree
234 // return `false` for invalid flesh
237 // insert an object into the tree
238 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
239 // AABB for static object will not be "fat" (simple optimization)
240 // WARNING! inserting the same object several times *WILL* break everything!
243 // remove an object from the tree
244 // WARNING: ids of removed objects can be reused on later insertions!
247 (** update the dynamic tree after an object has moved.
248 *
249 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
250 * otherwise, the corresponding node is removed and reinserted into the tree.
251 * the method returns true if the object has been reinserted into the tree.
252 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
253 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
254 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
255 *
256 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
257 *
258 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
259 *
260 * return `true` if the tree was modified.
261 *)
262 function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
264 function aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
266 function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
273 {$IFDEF aabbtree_query_count}
276 {$ELSE}
279 {$ENDIF}
283 implementation
285 uses
286 SysUtils;
289 // ////////////////////////////////////////////////////////////////////////// //
290 function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
291 function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
293 function minF (a, b: Float): Float; inline; begin if (a < b) then result := a else result := b; end;
294 function maxF (a, b: Float): Float; inline; begin if (a > b) then result := a else result := b; end;
297 // ////////////////////////////////////////////////////////////////////////// //
299 constructor Ray2D.Create (ax0, ay0, ax1, ay1: Float); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end;
304 begin
312 var
314 begin
321 begin
329 begin
338 // ////////////////////////////////////////////////////////////////////////// //
340 begin
345 begin
350 begin
354 function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end;
363 begin
368 {$IF DEFINED(D2F_DEBUG)}
370 {$ENDIF}
375 begin
380 {$IF DEFINED(D2F_DEBUG)}
382 {$ENDIF}
387 begin
388 {$IF DEFINED(D2F_DEBUG)}
391 {$ENDIF}
396 {$IF DEFINED(D2F_DEBUG)}
398 {$ENDIF}
403 begin
409 begin
410 {$IF DEFINED(D2F_DEBUG)}
412 {$ENDIF}
417 {$IF DEFINED(D2F_DEBUG)}
419 {$ENDIF}
424 begin
425 result :=
432 begin
438 begin
440 // exit with no intersection if found separated along any axis
447 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
448 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
449 function AABB2D.intersects (const ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
450 var
453 begin
454 // ok with coplanars
457 // do X
459 begin
466 // do Y
468 begin
472 // tmin
476 // tmax
483 begin
487 end
488 else
489 begin
495 var
498 begin
500 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
503 // nope, do it hard way
513 // ////////////////////////////////////////////////////////////////////////// //
514 procedure TDynAABBTree.TSegmentQueryResult.reset (); inline; begin dist := -1; flesh := nil; end;
515 function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; inline; begin result := (dist >= 0) and (flesh <> nil); end;
518 // ////////////////////////////////////////////////////////////////////////// //
523 begin
537 // ////////////////////////////////////////////////////////////////////////// //
538 // allocate and return a node to use in the tree
540 var
543 begin
544 // if there is no more allocated node to use
546 begin
548 // allocate more nodes in the tree
552 // initialize the allocated nodes
554 begin
561 // get the next free node
563 {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF}
574 // release a node
576 begin
588 // insert a leaf node in the tree
589 // 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
591 var
598 begin
599 // if the tree is empty
601 begin
604 exit;
609 // find the best sibling node for the new node
613 begin
617 // compute the merged AABB
622 // compute the cost of making the current node the sibling of the new node
625 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
628 // compute the cost of descending into the left child
633 // compute the cost of descending into the right child
638 // 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
641 // it is cheaper to go down into a child of the current node, choose the best child
642 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
648 // create a new parent for the new node and the sibling node
656 // if the sibling node was not the root node
658 begin
661 begin
663 end
664 else
665 begin
672 end
673 else
674 begin
675 // if the sibling node was the root node
683 // move up in the tree to change the AABBs that have changed
687 begin
688 // balance the sub-tree of the current node if it is not balanced
698 // recompute the height of the node in the tree
702 // recompute the AABB of the node
712 // remove a leaf node from the tree
714 var
717 begin
721 // if we are removing the root node (root node is a leaf in this case)
728 begin
730 end
731 else
732 begin
736 // if the parent of the node to remove is not the root node
738 begin
739 // destroy the parent node
741 begin
743 end
744 else
745 begin
746 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
752 // 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
755 begin
756 // balance the current sub-tree if necessary
761 // get the two children of the current node
765 // recompute the AABB and the height of the current node
767 mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
772 end
773 else
774 begin
775 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
783 // balance the sub-tree of a given node using left or right rotations
784 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
785 // this method returns the new root node id
787 var
791 begin
796 // if the node is a leaf or the height of A's sub-tree is less than 2
797 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
799 // get the two children nodes
807 // compute the factor of the left and right sub-trees
810 // if the right node C is 2 higher than left node B
812 begin
827 begin
829 begin
831 end
832 else
833 begin
834 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
837 end
838 else
839 begin
846 // if the right node C was higher than left node B because of the F node
848 begin
853 // recompute the AABB of node A and C
857 // recompute the height of node A and C
862 end
863 else
864 begin
865 // if the right node C was higher than left node B because of node G
870 // recompute the AABB of node A and C
874 // recompute the height of node A and C
881 // return the new root of the sub-tree
883 exit;
886 // if the left node B is 2 higher than right node C
888 begin
903 begin
905 begin
907 end
908 else
909 begin
910 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
913 end
914 else
915 begin
922 // if the left node B was higher than right node C because of the F node
924 begin
929 // recompute the AABB of node A and B
933 // recompute the height of node A and B
938 end
939 else
940 begin
941 // if the left node B was higher than right node C because of node G
946 // recompute the AABB of node A and B
950 // recompute the height of node A and B
957 // return the new root of the sub-tree
959 exit;
962 // if the sub-tree is balanced, return the current root node
967 // compute the height of a given node in the tree
969 var
972 begin
976 // if the node is a leaf, its height is zero
979 // compute the height of the left and right sub-tree
983 // return the height of the node
988 // internally add an object into the tree
990 var
992 begin
993 // get the next available node (or allocate new ones if necessary)
996 // create the fat aabb to use in the tree
999 begin
1006 // set the height of the node in the tree
1009 // insert the new leaf node in the tree
1015 // return the id of the node
1020 // initialize the tree
1022 var
1024 begin
1030 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1033 // initialize the allocated nodes
1035 begin
1044 // also, checks if the tree structure is valid (for debugging purpose)
1047 var
1051 begin
1054 // if it is the root
1056 // get the children nodes
1060 begin
1061 e_WriteLog(Format('AABB:(%f,%f)-(%f,%f); volume=%f; valid=%d; height=%d; leaf=%d', [pNode.aabb.minX, pNode.aabb.minY, pNode.aabb.maxX, pNode.aabb.maxY, pNode.aabb.volume, Integer(pNode.aabb.valid), pNode.height, Integer(pNode.leaf)]), MSG_NOTIFY);
1063 begin
1065 e_WriteLog(Format(' LEAF AABB:(%f,%f)-(%f,%f); valid=%d; volume=%f', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, Integer(aabb.valid), aabb.volume]), MSG_NOTIFY);
1070 // if the current node is a leaf
1072 begin
1075 end
1076 else
1077 begin
1080 // check that the children node Ids are valid
1083 // check that the children nodes have the correct parent node
1086 // check the height of node
1089 // check the AABB of the node
1095 // recursively check the children nodes
1101 begin
1102 // recursively check each node
1107 // return `true` from visitor to stop immediately
1108 // checker should check if this node should be considered to further checking
1109 // returns tree node if visitor says stop or -1
1110 function TDynAABBTree.visit (checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer;
1111 var
1117 var
1119 begin
1121 begin
1122 // use "small stack"
1125 end
1126 else
1127 begin
1128 // use "big stack"
1131 begin
1132 // reuse
1134 end
1135 else
1136 begin
1137 // grow
1145 (*
1146 function spop (): Integer; inline;
1147 begin
1148 {$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF}
1149 if (sp <= length(stack)) then
1150 begin
1151 // use "small stack"
1152 Dec(sp);
1153 result := stack[sp];
1154 end
1155 else
1156 begin
1157 // use "big stack"
1158 Dec(sp);
1159 result := bigstack[sp-length(stack)];
1160 end;
1161 end;
1162 *)
1164 var
1167 begin
1169 //if not assigned(visitor) then begin result := -1; exit; end;
1170 try
1171 {$IFDEF aabbtree_query_count}
1174 {$ENDIF}
1176 // start from root node
1179 // while there are still nodes to visit
1181 begin
1182 // get the next node id to visit
1183 //nodeId := spop();
1186 begin
1187 // use "small stack"
1190 end
1191 else
1192 begin
1193 // use "big stack"
1198 // skip it if it is a nil node
1201 // get the corresponding node
1203 // should we investigate this node?
1205 begin
1206 // if the node is a leaf
1208 begin
1209 // call visitor on it
1212 begin
1215 end
1216 else
1217 begin
1218 // if the node is not a leaf, we need to visit its children
1226 finally
1232 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1233 // 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
1235 begin
1242 begin
1248 // clear all the nodes and reset the tree
1250 begin
1256 function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1259 // return the root AABB of the tree
1261 begin
1262 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mNodeCount));{$ENDIF}
1267 // does the given id represents a valid object?
1268 // WARNING: ids of removed objects can be reused on later insertions!
1270 begin
1275 // get object by nodeid; can return nil for invalid ids
1277 begin
1278 if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil;
1281 // get fat object AABB by nodeid; returns random shit for invalid ids
1283 begin
1284 if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].isfree) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0);
1288 // insert an object into the tree
1289 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1290 // AABB for static object will not be "fat" (simple optimization)
1291 // WARNING! inserting the same object several times *WILL* break everything!
1292 function TDynAABBTree.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer;
1293 var
1296 begin
1298 begin
1299 e_WriteLog(Format('trying to insert FUCKED FLESH:(%f,%f)-(%f,%f); volume=%f; valid=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, aabb.volume, Integer(aabb.valid)]), MSG_WARNING);
1300 //raise Exception.Create('trying to insert invalid flesh in dyntree');
1302 exit;
1305 begin
1306 e_WriteLog(Format('trying to insert FUCKED AABB:(%f,%f)-(%f,%f); volume=%f; valid=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, aabb.volume, Integer(aabb.valid)]), MSG_WARNING);
1309 exit;
1311 //e_WriteLog(Format('inserting AABB:(%f,%f)-(%f,%f); volume=%f; valid=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, aabb.volume, Integer(aabb.valid)]), MSG_NOTIFY);
1320 // remove an object from the tree
1321 // WARNING: ids of removed objects can be reused on later insertions!
1323 begin
1324 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1325 // remove the node from the tree
1331 function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
1332 var
1334 begin
1335 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject');
1337 if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject');
1338 if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTree.updateObject');
1340 // if the new AABB is still inside the fat AABB of the node
1341 if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end;
1343 // if the new AABB is outside the fat AABB, we remove the corresponding node
1346 // compute the fat AABB by inflating the AABB with a constant gap
1349 begin
1356 // inflate the fat AABB in direction of the linear motion of the AABB
1358 begin
1360 end
1361 else
1362 begin
1366 begin
1368 end
1369 else
1370 begin
1376 // reinsert the node into the tree
1383 // report all shapes overlapping with the AABB given in parameter
1384 function TDynAABBTree.aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
1385 var
1388 begin
1391 var
1393 begin
1403 // report body that contains the given point, or nil
1406 begin
1409 var
1411 begin
1413 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF}
1418 // segment querying method
1419 function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
1420 var
1428 begin
1433 var
1435 begin
1437 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1439 begin
1443 exit;
1445 // if the user returned a positive fraction
1447 begin
1448 // we update the maxFraction value and the ray AABB using the new maximum fraction
1450 begin
1454 // fix curb here
1455 //curb := cura+dir*hitFraction;
1463 begin
1475 // normalize