298fa34f4775b23f2875ceb0789d261721bc37c4
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}
19 {.$DEFINE aabbtree_use_floats}
22 interface
24 uses
28 // ////////////////////////////////////////////////////////////////////////// //
29 type
33 // ////////////////////////////////////////////////////////////////////////// //
34 type
36 public
40 public
53 // ////////////////////////////////////////////////////////////////////////// //
54 type
56 public
59 private
66 public
82 // return true if the current AABB contains the AABB given in parameter
86 // return true if the current AABB is overlapping with the AABB in parameter
87 // two AABBs overlap if they overlap in the two axes at the same time
90 // ray direction must be normalized
91 function intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload;
102 // ////////////////////////////////////////////////////////////////////////// //
103 (* Dynamic AABB tree (bounding volume hierarchy)
104 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
105 * Copyright (c) 2010-2016 Daniel Chappuis
106 *
107 * This software is provided 'as-is', without any express or implied warranty.
108 * In no event will the authors be held liable for any damages arising from the
109 * use of this software.
110 *
111 * Permission is granted to anyone to use this software for any purpose,
112 * including commercial applications, and to alter it and redistribute it
113 * freely, subject to the following restrictions:
114 *
115 * 1. The origin of this software must not be misrepresented; you must not claim
116 * that you wrote the original software. If you use this software in a
117 * product, an acknowledgment in the product documentation would be
118 * appreciated but is not required.
119 *
120 * 2. Altered source versions must be plainly marked as such, and must not be
121 * misrepresented as being the original software.
122 *
123 * 3. This notice may not be removed or altered from any source distribution.
124 *)
125 // ////////////////////////////////////////////////////////////////////////// //
126 (*
127 * This class implements a dynamic AABB tree that is used for broad-phase
128 * collision detection. This data structure is inspired by Nathanael Presson's
129 * dynamic tree implementation in BulletPhysics. The following implementation is
130 * based on the one from Erin Catto in Box2D as described in the book
131 * "Introduction to Game Physics with Box2D" by Ian Parberry.
132 *)
133 // ////////////////////////////////////////////////////////////////////////// //
134 // Dynamic AABB Tree: can be used to speed up broad phase in various engines
135 type
137 public
140 private
141 type
144 public
148 public
149 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
151 //nextNodeId: Integer;
152 // a node is either a leaf (has data) or is an internal node (has children)
153 children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
154 // height of the node in the tree (-1 for free nodes)
156 // fat axis aligned bounding box (AABB) corresponding to the node
158 //TODO: `flesh` can be united with `children`
162 public
163 // return true if the node is a leaf of the tree
168 //property flesh: Integer read children[0] write children[0];
174 //TVisitVisitorCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested;
180 public
181 // return `true` to stop
182 type TForEachLeafCB = function (abody: TTreeFlesh; constref aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
184 public
185 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
186 // also inflated in direction of the linear motion of the body by mutliplying the
187 // followin constant with the linear velocity and the elapsed time between two frames
188 {$IFDEF aabbtree_use_floats}
190 {$ELSE}
192 {$ENDIF}
194 public
195 // called when a overlapping node has been found during the call to forEachAABBOverlap()
196 // return `true` to stop
198 type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Single): Single is nested; // return dist from (ax,ay) to abody
209 private
212 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
216 // extra AABB Gap used to allow the collision shape to move a little bit
217 // without triggering a large modification of the tree which can be costly
222 // for segment query
236 private
245 function visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer;
249 public
250 {$IFDEF aabbtree_query_count}
252 {$ENDIF}
254 public
258 // clear all the nodes and reset the tree
268 // returns `false` if nodeid is not leaf
271 // return `false` for invalid flesh
272 function getFleshAABB (out aabb: AABB2D; flesh: TTreeFlesh; tag: Integer): Boolean; virtual; abstract;
274 // insert an object into the tree
275 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
276 // AABB for static object will not be "fat" (simple optimization)
277 // WARNING! inserting the same object several times *WILL* break everything!
278 function insertObject (flesh: TTreeFlesh; tag: Integer=-1; staticObject: Boolean=false): Integer;
280 // remove an object from the tree
281 // WARNING: ids of removed objects can be reused on later insertions!
284 (** update the dynamic tree after an object has moved.
285 *
286 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
287 * otherwise, the corresponding node is removed and reinserted into the tree.
288 * the method returns true if the object has been reinserted into the tree.
289 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
290 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
291 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
292 *
293 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
294 *
295 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
296 *
297 * return `true` if the tree was modified.
298 *)
299 function updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload;
302 function aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
304 function segmentQuery (out qr: TSegmentQueryResult; ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean;
311 {$IFDEF aabbtree_query_count}
314 {$ELSE}
317 {$ENDIF}
328 implementation
330 uses
331 SysUtils;
334 // ////////////////////////////////////////////////////////////////////////// //
335 function dtMinI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
336 function dtMaxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
338 function dtMinF (a, b: TreeNumber): TreeNumber; inline; begin if (a < b) then result := a else result := b; end;
339 function dtMaxF (a, b: TreeNumber): TreeNumber; inline; begin if (a > b) then result := a else result := b; end;
342 // ////////////////////////////////////////////////////////////////////////// //
343 constructor Ray2D.Create (ax, ay: Single; aangle: Single); begin setXYAngle(ax, ay, aangle); end;
344 constructor Ray2D.Create (ax0, ay0, ax1, ay1: Single); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end;
349 begin
357 var
359 begin
366 begin
374 begin
383 // ////////////////////////////////////////////////////////////////////////// //
385 begin
390 begin
395 begin
400 begin
407 function AABB2D.getvalid (): Boolean; inline; begin result := (minX <= maxX) and (minY <= maxY); end;
409 {$IFDEF aabbtree_use_floats}
412 {$ELSE}
415 {$ENDIF}
420 begin
425 {$IF DEFINED(D2F_DEBUG)}
427 {$ENDIF}
432 begin
437 {$IF DEFINED(D2F_DEBUG)}
439 {$ENDIF}
444 begin
445 {$IF DEFINED(D2F_DEBUG)}
448 {$ENDIF}
453 {$IF DEFINED(D2F_DEBUG)}
455 {$ENDIF}
460 begin
466 begin
467 {$IF DEFINED(D2F_DEBUG)}
469 {$ENDIF}
474 {$IF DEFINED(D2F_DEBUG)}
476 {$ENDIF}
481 begin
482 result :=
489 begin
495 begin
497 // exit with no intersection if found separated along any axis
504 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
505 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
506 function AABB2D.intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload;
507 var
510 begin
511 // ok with coplanars
514 // do X
516 begin
523 // do Y
525 begin
529 // tmin
533 // tmax
540 begin
544 end
545 else
546 begin
552 var
555 begin
557 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
560 // nope, do it hard way
570 // ////////////////////////////////////////////////////////////////////////// //
571 procedure TDynAABBTreeBase.TSegmentQueryResult.reset (); inline; begin dist := -1; flesh := Default(ITP); end;
572 function TDynAABBTreeBase.TSegmentQueryResult.valid (): Boolean; inline; begin result := (dist >= 0) and (flesh <> Default(ITP)); end;
575 // ////////////////////////////////////////////////////////////////////////// //
576 function TDynAABBTreeBase.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end;
577 function TDynAABBTreeBase.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end;
580 begin
594 begin
595 e_WriteLog(Format('NODE: parentId=%d; children=[%d,%d]; height=%d; tag=%d; fleshX=%d; fleshY=%d; aabb=(%d,%d)-(%d,%d)',
596 [parentId, children[0], children[1], Integer(height), tag, fleshX, fleshY, aabb.minX, aabb.minY, aabb.maxX, aabb.maxY]),
597 MSG_NOTIFY);
601 // ////////////////////////////////////////////////////////////////////////// //
602 // allocate and return a node to use in the tree
604 var
607 begin
608 // if there is no more allocated node to use
610 begin
612 // allocate more nodes in the tree
616 // initialize the allocated nodes
618 begin
625 // get the next free node
636 //e_WriteLog(Format('tree: allocated node #%d', [result]), MSG_NOTIFY);
640 // release a node
642 begin
652 //e_WriteLog(Format('tree: released node #%d', [nodeId]), MSG_NOTIFY);
656 // insert a leaf node in the tree
657 // 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
659 var
666 begin
667 // if the tree is empty
669 begin
672 exit;
677 // find the best sibling node for the new node
681 begin
685 // compute the merged AABB
690 // compute the cost of making the current node the sibling of the new node
693 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
696 // compute the cost of descending into the left child
701 // compute the cost of descending into the right child
706 // 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
709 // it is cheaper to go down into a child of the current node, choose the best child
710 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
716 // create a new parent for the new node and the sibling node
724 // if the sibling node was not the root node
726 begin
729 begin
731 end
732 else
733 begin
740 end
741 else
742 begin
743 // if the sibling node was the root node
751 // move up in the tree to change the AABBs that have changed
755 begin
756 // balance the sub-tree of the current node if it is not balanced
766 // recompute the height of the node in the tree
770 // recompute the AABB of the node
780 // remove a leaf node from the tree
782 var
785 begin
789 // if we are removing the root node (root node is a leaf in this case)
796 begin
798 end
799 else
800 begin
804 // if the parent of the node to remove is not the root node
806 begin
807 // destroy the parent node
809 begin
811 end
812 else
813 begin
814 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
820 // 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
823 begin
824 // balance the current sub-tree if necessary
829 // get the two children of the current node
833 // recompute the AABB and the height of the current node
835 mNodes[currentNodeId].height := dtMaxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
840 end
841 else
842 begin
843 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
851 // balance the sub-tree of a given node using left or right rotations
852 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
853 // this method returns the new root node id
855 var
859 begin
864 // if the node is a leaf or the height of A's sub-tree is less than 2
865 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
867 // get the two children nodes
875 // compute the factor of the left and right sub-trees
878 // if the right node C is 2 higher than left node B
880 begin
895 begin
897 begin
899 end
900 else
901 begin
902 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
905 end
906 else
907 begin
914 // if the right node C was higher than left node B because of the F node
916 begin
921 // recompute the AABB of node A and C
925 // recompute the height of node A and C
930 end
931 else
932 begin
933 // if the right node C was higher than left node B because of node G
938 // recompute the AABB of node A and C
942 // recompute the height of node A and C
949 // return the new root of the sub-tree
951 exit;
954 // if the left node B is 2 higher than right node C
956 begin
971 begin
973 begin
975 end
976 else
977 begin
978 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
981 end
982 else
983 begin
990 // if the left node B was higher than right node C because of the F node
992 begin
997 // recompute the AABB of node A and B
1001 // recompute the height of node A and B
1006 end
1007 else
1008 begin
1009 // if the left node B was higher than right node C because of node G
1014 // recompute the AABB of node A and B
1018 // recompute the height of node A and B
1025 // return the new root of the sub-tree
1027 exit;
1030 // if the sub-tree is balanced, return the current root node
1035 // compute the height of a given node in the tree
1037 var
1040 begin
1044 // if the node is a leaf, its height is zero
1047 // compute the height of the left and right sub-tree
1051 // return the height of the node
1056 // internally add an object into the tree
1057 function TDynAABBTreeBase.insertObjectInternal (constref aabb: AABB2D; staticObject: Boolean): Integer;
1058 var
1061 begin
1062 // get the next available node (or allocate new ones if necessary)
1067 // create the fat aabb to use in the tree
1070 begin
1077 // set the height of the node in the tree
1080 // insert the new leaf node in the tree
1086 // return the id of the node
1091 // initialize the tree
1093 var
1095 begin
1101 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1104 // initialize the allocated nodes
1106 begin
1115 // also, checks if the tree structure is valid (for debugging purpose)
1117 var
1121 begin
1124 // if it is the root
1126 // get the children nodes
1130 begin
1131 {$IFDEF aabbtree_use_floats}
1132 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);
1133 {$ELSE}
1134 e_WriteLog(Format('AABB:(%d,%d)-(%d,%d); volume=%d; 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);
1135 {$ENDIF}
1137 begin
1139 {$IFDEF aabbtree_use_floats}
1140 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);
1141 {$ELSE}
1142 e_WriteLog(Format(' LEAF AABB:(%d,%d)-(%d,%d); valid=%d; volume=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, Integer(aabb.valid), aabb.volume]), MSG_NOTIFY);
1143 {$ENDIF}
1148 // if the current node is a leaf
1150 begin
1153 end
1154 else
1155 begin
1158 // check that the children node Ids are valid
1161 // check that the children nodes have the correct parent node
1164 // check the height of node
1167 // check the AABB of the node
1173 // recursively check the children nodes
1180 // also, checks if the tree structure is valid (for debugging purpose)
1182 begin
1183 // recursively check each node
1188 // return `true` from visitor to stop immediately
1189 // checker should check if this node should be considered to further checking
1190 // returns tree node if visitor says stop or -1
1191 function TDynAABBTreeBase.visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer;
1192 var
1197 (*
1198 procedure spush (id: Integer); inline;
1199 var
1200 xsp: Integer;
1201 begin
1202 if (sp < length(stack)) then
1203 begin
1204 // use "small stack"
1205 stack[sp] := id;
1206 Inc(sp);
1207 end
1208 else
1209 begin
1210 // use "big stack"
1211 xsp := sp-length(stack);
1212 if (xsp < length(bigstack)) then
1213 begin
1214 // reuse
1215 bigstack[xsp] := id;
1216 end
1217 else
1218 begin
1219 // grow
1220 SetLength(bigstack, length(bigstack)+1);
1221 bigstack[high(bigstack)] := id;
1222 end;
1223 Inc(sp);
1224 end;
1225 end;
1227 function spop (): Integer; inline;
1228 begin
1229 //{$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF}
1230 if (sp <= length(stack)) then
1231 begin
1232 // use "small stack"
1233 Dec(sp);
1234 result := stack[sp];
1235 end
1236 else
1237 begin
1238 // use "big stack"
1239 Dec(sp);
1240 result := bigstack[sp-length(stack)];
1241 end;
1242 end;
1243 *)
1245 var
1250 begin
1252 //if not assigned(visitor) and not assigned(visdg) then raise Exception.Create('dyntree: empty visitors aren''t supported');
1253 //try
1254 {$IFDEF aabbtree_query_count}
1257 {$ENDIF}
1259 // start from root node
1260 {$IF FALSE}
1262 {$ELSE}
1264 else begin xsp := sp-length(stack); if (xsp < length(bigstack)) then bigstack[xsp] := mRootNodeId
1265 else begin SetLength(bigstack, length(bigstack)+1); bigstack[high(bigstack)] := mRootNodeId; end;
1268 {$ENDIF}
1270 // while there are still nodes to visit
1272 begin
1273 // get the next node id to visit
1274 {$IF FALSE}
1276 {$ELSE}
1279 {$ENDIF}
1280 // skip it if it is a nil node
1283 // get the corresponding node
1285 // should we investigate this node?
1288 ModeAABB:
1289 begin
1290 //doNode := caabb.overlaps(node.aabb);
1291 // this gives small speedup (or not...)
1292 // exit with no intersection if found separated along any axis
1297 ModePoint:
1298 begin
1299 //doNode := node.aabb.contains(caabb.minX, caabb.minY);
1300 // this gives small speedup
1301 doNode := (caabb.minX >= node.aabb.minX) and (caabb.minY >= node.aabb.minY) and (caabb.minX <= node.aabb.maxX) and (caabb.minY <= node.aabb.maxY);
1305 begin
1306 // if the node is a leaf
1308 begin
1309 // call visitor on it
1312 begin
1314 begin
1318 begin
1322 end
1323 else
1324 begin
1325 // if the node is not a leaf, we need to visit its children
1326 {$IF FALSE}
1329 {$ELSE}
1331 else begin xsp := sp-length(stack); if (xsp < length(bigstack)) then bigstack[xsp] := node.children[TTreeNode.Left]
1332 else begin SetLength(bigstack, length(bigstack)+1); bigstack[high(bigstack)] := node.children[TTreeNode.Left]; end;
1337 else begin xsp := sp-length(stack); if (xsp < length(bigstack)) then bigstack[xsp] := node.children[TTreeNode.Right]
1338 else begin SetLength(bigstack, length(bigstack)+1); bigstack[high(bigstack)] := node.children[TTreeNode.Right]; end;
1342 {$ENDIF}
1349 //finally
1350 // bigstack := nil;
1351 //end;
1355 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1356 // 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
1358 begin
1365 begin
1371 // clear all the nodes and reset the tree
1373 begin
1379 function TDynAABBTreeBase.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1382 // return the root AABB of the tree
1384 begin
1385 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mAllocCount));{$ENDIF}
1390 // does the given id represents a valid object?
1391 // WARNING: ids of removed objects can be reused on later insertions!
1393 begin
1398 // get object by nodeid; can return nil for invalid ids
1400 begin
1401 if (nodeid >= 0) and (nodeid < mAllocCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := Default(ITP);
1404 // get fat object AABB by nodeid; returns random shit for invalid ids
1406 begin
1407 if (nodeid >= 0) and (nodeid < mAllocCount) and (not mNodes[nodeid].isfree) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0);
1411 begin
1413 begin
1415 {$IFDEF aabbtree_use_floats}
1418 {$ELSE}
1421 {$ENDIF}
1422 end
1423 else
1424 begin
1428 //if (nodeid >= 0) and (nodeid < mAllocCount) then mNodes[nodeid].dumpToLog();
1433 // insert an object into the tree
1434 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1435 // AABB for static object will not be "fat" (simple optimization)
1436 // WARNING! inserting the same object several times *WILL* break everything!
1437 function TDynAABBTreeBase.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer;
1438 var
1441 begin
1443 begin
1444 {$IFDEF aabbtree_use_floats}
1445 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);
1446 {$ELSE}
1447 e_WriteLog(Format('trying to insert FUCKED FLESH:(%d,%d)-(%d,%d); volume=%d; valid=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, aabb.volume, Integer(aabb.valid)]), MSG_WARNING);
1448 {$ENDIF}
1449 //raise Exception.Create('trying to insert invalid flesh in dyntree');
1451 exit;
1454 begin
1455 {$IFDEF aabbtree_use_floats}
1456 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);
1457 {$ELSE}
1458 e_WriteLog(Format('trying to insert FUCKED AABB:(%d,%d)-(%d,%d); volume=%d; valid=%d', [aabb.minX, aabb.minY, aabb.maxX, aabb.maxY, aabb.volume, Integer(aabb.valid)]), MSG_WARNING);
1459 {$ENDIF}
1462 exit;
1464 //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);
1477 // remove an object from the tree
1478 // WARNING: ids of removed objects can be reused on later insertions!
1480 begin
1481 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase');
1482 // remove the node from the tree
1488 function TDynAABBTreeBase.updateObject (nodeId: Integer; forceReinsert: Boolean=false): Boolean; overload;
1489 var
1492 begin
1493 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject');
1495 if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject');
1496 if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject');
1507 function TDynAABBTreeBase.updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload;
1508 var
1512 begin
1513 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject');
1515 if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject');
1516 if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject');
1521 // if the new AABB is still inside the fat AABB of the node
1523 begin
1528 exit;
1531 // if the new AABB is outside the fat AABB, we remove the corresponding node
1536 // compute the fat AABB by inflating the AABB with a constant gap
1542 begin
1549 // inflate the fat AABB in direction of the linear motion of the AABB
1551 begin
1552 node.aabb.minX += LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1553 end
1554 else
1555 begin
1556 node.aabb.maxX += LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1560 begin
1561 node.aabb.minY += LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1562 end
1563 else
1564 begin
1565 node.aabb.maxY += LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1570 // reinsert the node into the tree
1578 begin
1583 // report all shapes overlapping with the AABB given in parameter
1584 function TDynAABBTreeBase.aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
1585 var
1588 begin
1592 //chkAABB := AABB2D.Create(ax, ay, ax+aw, ay+ah);
1605 begin
1610 // report body that contains the given point, or nil
1611 function TDynAABBTreeBase.pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
1612 var
1615 begin
1619 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mAllocCount) and (mNodes[nid].leaf)));{$ENDIF}
1626 begin
1631 var
1633 begin
1635 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1637 begin
1641 exit;
1643 // if the user returned a positive fraction
1645 begin
1646 // we update the maxFraction value and the ray AABB using the new maximum fraction
1648 begin
1652 // fix curb here
1653 //curb := cura+dir*hitFraction;
1662 // segment querying method
1663 function TDynAABBTreeBase.segmentQuery (out qr: TSegmentQueryResult; ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean;
1664 var
1672 begin
1693 // normalize
1698 //chkAABB := AABB2D.Create(0, 0, 1, 1);