8260d9e82aea1122836db9da2e21624f4c312909
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
156 public
157 // return true if the node is a leaf of the tree
162 //property flesh: Integer read children[0] write children[0];
168 public
169 // return `true` to stop
170 type TForEachLeafCB = function (abody: TTreeFlesh; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
172 public
173 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
174 // also inflated in direction of the linear motion of the body by mutliplying the
175 // followin constant with the linear velocity and the elapsed time between two frames
178 private
181 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
185 // extra AABB Gap used to allow the collision shape to move a little bit
186 // without triggering a large modification of the tree which can be costly
189 private
200 public
201 {$IFDEF aabbtree_query_count}
203 {$ENDIF}
205 public
206 // called when a overlapping node has been found during the call to forEachAABBOverlap()
207 // return `true` to stop
209 type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
219 public
223 // clear all the nodes and reset the tree
233 // return `false` for invalid flesh
236 // insert an object into the tree
237 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
238 // AABB for static object will not be "fat" (simple optimization)
239 // WARNING! inserting the same object several times *WILL* break everything!
242 // remove an object from the tree
243 // WARNING: ids of removed objects can be reused on later insertions!
246 (** update the dynamic tree after an object has moved.
247 *
248 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
249 * otherwise, the corresponding node is removed and reinserted into the tree.
250 * the method returns true if the object has been reinserted into the tree.
251 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
252 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
253 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
254 *
255 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
256 *
257 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
258 *
259 * return `true` if the tree was modified.
260 *)
261 function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
265 function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
275 implementation
277 uses
278 SysUtils;
281 // ////////////////////////////////////////////////////////////////////////// //
282 function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
283 function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
285 function minF (a, b: Float): Float; inline; begin if (a < b) then result := a else result := b; end;
286 function maxF (a, b: Float): Float; inline; begin if (a > b) then result := a else result := b; end;
289 // ////////////////////////////////////////////////////////////////////////// //
291 constructor Ray2D.Create (ax0, ay0, ax1, ay1: Float); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end;
296 begin
304 var
306 begin
313 begin
321 begin
330 // ////////////////////////////////////////////////////////////////////////// //
332 begin
337 begin
342 begin
346 function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end;
355 begin
364 begin
373 begin
382 begin
388 begin
397 begin
398 result :=
405 begin
411 begin
413 // exit with no intersection if found separated along any axis
420 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
421 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
422 function AABB2D.intersects (const ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
423 var
426 begin
427 // ok with coplanars
430 // do X
432 begin
439 // do Y
441 begin
445 // tmin
449 // tmax
456 begin
460 end
461 else
462 begin
468 var
471 begin
473 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
476 // nope, do it hard way
486 // ////////////////////////////////////////////////////////////////////////// //
487 procedure TDynAABBTree.TSegmentQueryResult.reset (); inline; begin dist := -1; flesh := nil; end;
488 function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; inline; begin result := (dist >= 0) and (flesh <> nil); end;
491 // ////////////////////////////////////////////////////////////////////////// //
496 begin
502 //aabb.setX0Y0X1Y1(0, 0, 0, 0);
510 // ////////////////////////////////////////////////////////////////////////// //
511 // allocate and return a node to use in the tree
513 var
516 begin
517 // if there is no more allocated node to use
519 begin
521 // allocate more nodes in the tree
525 // initialize the allocated nodes
527 begin
535 // get the next free node
537 {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF}
548 // release a node
550 begin
561 // insert a leaf node in the tree
562 // 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
564 var
571 begin
572 // if the tree is empty
574 begin
577 exit;
582 // find the best sibling node for the new node
586 begin
590 // compute the merged AABB
595 // compute the cost of making the current node the sibling of the new node
598 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
601 // compute the cost of descending into the left child
606 // compute the cost of descending into the right child
611 // 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
614 // it is cheaper to go down into a child of the current node, choose the best child
615 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
621 // create a new parent for the new node and the sibling node
629 // if the sibling node was not the root node
631 begin
634 begin
636 end
637 else
638 begin
645 end
646 else
647 begin
648 // if the sibling node was the root node
656 // move up in the tree to change the AABBs that have changed
660 begin
661 // balance the sub-tree of the current node if it is not balanced
671 // recompute the height of the node in the tree
675 // recompute the AABB of the node
685 // remove a leaf node from the tree
687 var
690 begin
694 // if we are removing the root node (root node is a leaf in this case)
701 begin
703 end
704 else
705 begin
709 // if the parent of the node to remove is not the root node
711 begin
712 // destroy the parent node
714 begin
716 end
717 else
718 begin
719 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
725 // 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
728 begin
729 // balance the current sub-tree if necessary
734 // get the two children of the current node
738 // recompute the AABB and the height of the current node
740 mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
745 end
746 else
747 begin
748 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
756 // balance the sub-tree of a given node using left or right rotations
757 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
758 // this method returns the new root node id
760 var
764 begin
769 // if the node is a leaf or the height of A's sub-tree is less than 2
770 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
772 // get the two children nodes
780 // compute the factor of the left and right sub-trees
783 // if the right node C is 2 higher than left node B
785 begin
800 begin
802 begin
804 end
805 else
806 begin
807 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
810 end
811 else
812 begin
819 // if the right node C was higher than left node B because of the F node
821 begin
826 // recompute the AABB of node A and C
830 // recompute the height of node A and C
835 end
836 else
837 begin
838 // if the right node C was higher than left node B because of node G
843 // recompute the AABB of node A and C
847 // recompute the height of node A and C
854 // return the new root of the sub-tree
856 exit;
859 // if the left node B is 2 higher than right node C
861 begin
876 begin
878 begin
880 end
881 else
882 begin
883 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
886 end
887 else
888 begin
895 // if the left node B was higher than right node C because of the F node
897 begin
902 // recompute the AABB of node A and B
906 // recompute the height of node A and B
911 end
912 else
913 begin
914 // if the left node B was higher than right node C because of node G
919 // recompute the AABB of node A and B
923 // recompute the height of node A and B
930 // return the new root of the sub-tree
932 exit;
935 // if the sub-tree is balanced, return the current root node
940 // compute the height of a given node in the tree
942 var
945 begin
949 // if the node is a leaf, its height is zero
952 // compute the height of the left and right sub-tree
956 // return the height of the node
961 // internally add an object into the tree
963 var
965 begin
966 // get the next available node (or allocate new ones if necessary)
969 // create the fat aabb to use in the tree
972 begin
979 // set the height of the node in the tree
982 // insert the new leaf node in the tree
988 // return the id of the node
993 // initialize the tree
995 var
997 begin
1003 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1006 // initialize the allocated nodes
1008 begin
1018 // also, checks if the tree structure is valid (for debugging purpose)
1021 var
1025 begin
1028 // if it is the root
1030 // get the children nodes
1033 e_WriteLog(Format('AABB:(%f,%f)-(%f,%f); volume=%f; valid=%d', [pNode.aabb.minX, pNode.aabb.minY, pNode.aabb.maxX, pNode.aabb.maxY, pNode.aabb.volume, Integer(pNode.aabb.valid)]), MSG_NOTIFY);
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
1068 // recursively check each node
1073 // return `true` from visitor to stop immediately
1074 // checker should check if this node should be considered to further checking
1075 // returns tree node if visitor says stop or -1
1077 var
1083 var
1085 begin
1087 begin
1088 // use "small stack"
1091 end
1092 else
1093 begin
1094 // use "big stack"
1097 begin
1098 // reuse
1100 end
1101 else
1102 begin
1103 // grow
1112 begin
1115 begin
1116 // use "small stack"
1119 end
1120 else
1121 begin
1122 // use "big stack"
1128 var
1131 begin
1133 //if not assigned(visitor) then begin result := -1; exit; end;
1134 try
1135 {$IFDEF aabbtree_query_count}
1138 {$ENDIF}
1140 // start from root node
1143 // while there are still nodes to visit
1145 begin
1146 // get the next node id to visit
1148 // skip it if it is a nil node
1151 // get the corresponding node
1153 // should we investigate this node?
1155 begin
1156 // if the node is a leaf
1158 begin
1159 // call visitor on it
1162 begin
1165 end
1166 else
1167 begin
1168 // if the node is not a leaf, we need to visit its children
1176 finally
1182 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1183 // 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
1185 begin
1192 begin
1198 // clear all the nodes and reset the tree
1200 begin
1206 function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1209 // return the root AABB of the tree
1211 begin
1212 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mNodeCount));{$ENDIF}
1217 // does the given id represents a valid object?
1218 // WARNING: ids of removed objects can be reused on later insertions!
1220 begin
1225 // get object by nodeid; can return nil for invalid ids
1227 begin
1228 if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil;
1231 // get fat object AABB by nodeid; returns random shit for invalid ids
1233 begin
1234 if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].free) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0);
1238 // insert an object into the tree
1239 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1240 // AABB for static object will not be "fat" (simple optimization)
1241 // WARNING! inserting the same object several times *WILL* break everything!
1243 var
1246 begin
1248 begin
1249 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);
1251 exit;
1253 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);
1261 // remove an object from the tree
1262 // WARNING: ids of removed objects can be reused on later insertions!
1264 begin
1265 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1266 // remove the node from the tree
1272 function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
1273 var
1275 begin
1276 if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1278 if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree');
1280 // if the new AABB is still inside the fat AABB of the node
1281 if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end;
1283 // if the new AABB is outside the fat AABB, we remove the corresponding node
1286 // compute the fat AABB by inflating the AABB with a constant gap
1289 begin
1296 // inflate the fat AABB in direction of the linear motion of the AABB
1298 begin
1300 end
1301 else
1302 begin
1306 begin
1308 end
1309 else
1310 begin
1316 // reinsert the node into the tree
1323 // report all shapes overlapping with the AABB given in parameter
1325 var
1328 begin
1331 begin
1339 // report body that contains the given point, or nil
1341 var
1344 begin
1347 begin
1349 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF}
1354 // segment querying method
1355 function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
1356 var
1364 begin
1369 var
1371 begin
1373 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1375 begin
1379 exit;
1381 // if the user returned a positive fraction
1383 begin
1384 // we update the maxFraction value and the ray AABB using the new maximum fraction
1386 begin
1390 // fix curb here
1391 //curb := cura+dir*hitFraction;
1399 begin
1411 // normalize