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
43 public
61 // ////////////////////////////////////////////////////////////////////////// //
62 type
64 public
67 private
76 public
92 // return true if the current AABB contains the AABB given in parameter
96 // return true if the current AABB is overlapping with the AABB in parameter
97 // two AABBs overlap if they overlap in the two axes at the same time
100 // ray direction must be normalized
101 function intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload;
103 function intersects (constref ray: Ray2D; maxtime: Single; tmino: PSingle=nil): Boolean; inline; overload;
116 // ////////////////////////////////////////////////////////////////////////// //
117 (* Dynamic AABB tree (bounding volume hierarchy)
118 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
119 * Copyright (c) 2010-2016 Daniel Chappuis
120 *
121 * This software is provided 'as-is', without any express or implied warranty.
122 * In no event will the authors be held liable for any damages arising from the
123 * use of this software.
124 *
125 * Permission is granted to anyone to use this software for any purpose,
126 * including commercial applications, and to alter it and redistribute it
127 * freely, subject to the following restrictions:
128 *
129 * 1. The origin of this software must not be misrepresented; you must not claim
130 * that you wrote the original software. If you use this software in a
131 * product, an acknowledgment in the product documentation would be
132 * appreciated but is not required.
133 *
134 * 2. Altered source versions must be plainly marked as such, and must not be
135 * misrepresented as being the original software.
136 *
137 * 3. This notice may not be removed or altered from any source distribution.
138 *)
139 // ////////////////////////////////////////////////////////////////////////// //
140 (*
141 * This class implements a dynamic AABB tree that is used for broad-phase
142 * collision detection. This data structure is inspired by Nathanael Presson's
143 * dynamic tree implementation in BulletPhysics. The following implementation is
144 * based on the one from Erin Catto in Box2D as described in the book
145 * "Introduction to Game Physics with Box2D" by Ian Parberry.
146 *)
147 // ////////////////////////////////////////////////////////////////////////// //
148 // Dynamic AABB Tree: can be used to speed up broad phase in various engines
149 type
151 public
154 private
155 type
158 public
162 public
163 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
165 //nextNodeId: Integer;
166 // a node is either a leaf (has data) or is an internal node (has children)
167 children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
168 // height of the node in the tree (-1 for free nodes)
170 // fat axis aligned bounding box (AABB) corresponding to the node
172 //TODO: `flesh` can be united with `children`
176 public
177 // return true if the node is a leaf of the tree
182 //property flesh: Integer read children[0] write children[0];
188 //TVisitVisitorCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested;
194 public
195 // return `true` to stop
196 type TForEachLeafCB = function (abody: TTreeFlesh; constref aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
198 public
199 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
200 // also inflated in direction of the linear motion of the body by mutliplying the
201 // followin constant with the linear velocity and the elapsed time between two frames
202 {$IFDEF aabbtree_use_floats}
204 {$ELSE}
206 {$ENDIF}
208 public
209 // called when a overlapping node has been found during the call to forEachAABBOverlap()
210 // return `true` to stop
212 type TSegQueryCallback = function (abody: TTreeFlesh; var ray: Ray2D): Single is nested; // return hit time
224 private
227 mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
231 // extra AABB Gap used to allow the collision shape to move a little bit
232 // without triggering a large modification of the tree which can be costly
237 // for segment query
253 private
262 function visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer;
266 public
267 {$IFDEF aabbtree_query_count}
269 {$ENDIF}
271 public
275 // clear all the nodes and reset the tree
285 // returns `false` if nodeid is not leaf
288 // return `false` for invalid flesh
289 function getFleshAABB (out aabb: AABB2D; flesh: TTreeFlesh; tag: Integer): Boolean; virtual; abstract;
291 // insert an object into the tree
292 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
293 // AABB for static object will not be "fat" (simple optimization)
294 // WARNING! inserting the same object several times *WILL* break everything!
295 function insertObject (flesh: TTreeFlesh; tag: Integer=-1; staticObject: Boolean=false): Integer;
297 // remove an object from the tree
298 // WARNING: ids of removed objects can be reused on later insertions!
301 (** update the dynamic tree after an object has moved.
302 *
303 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
304 * otherwise, the corresponding node is removed and reinserted into the tree.
305 * the method returns true if the object has been reinserted into the tree.
306 * the `dispX` and `dispY` parameters are the linear velocity of the AABB multiplied by the elapsed time between two frames.
307 * if the `forceReinsert` parameter is `true`, we force a removal and reinsertion of the node
308 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
309 *
310 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
311 *
312 * if `forceReinsert` = `true` and both `dispX` and `dispY` are zeroes, convert object to "static" (don't extrude AABB).
313 *
314 * return `true` if the tree was modified.
315 *)
316 function updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload;
319 function aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
321 function segmentQuery (out qr: TSegmentQueryResult; const ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean;
328 {$IFDEF aabbtree_query_count}
331 {$ELSE}
334 {$ENDIF}
348 implementation
350 uses
351 SysUtils;
354 // ////////////////////////////////////////////////////////////////////////// //
355 function dtMinI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
356 function dtMaxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
358 function dtMinF (a, b: TreeNumber): TreeNumber; inline; begin if (a < b) then result := a else result := b; end;
359 function dtMaxF (a, b: TreeNumber): TreeNumber; inline; begin if (a > b) then result := a else result := b; end;
361 function minSingle (a, b: Single): Single; inline; begin if (a < b) then result := a else result := b; end;
362 function maxSingle (a, b: Single): Single; inline; begin if (a > b) then result := a else result := b; end;
365 // ////////////////////////////////////////////////////////////////////////// //
366 constructor Ray2D.Create (ax, ay: Single; aangle: Single); begin setXYAngle(ax, ay, aangle); end;
367 constructor Ray2D.Create (ax0, ay0, ax1, ay1: Single); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end;
371 function Ray2D.getOrigN (idx: Integer): Single; inline; begin if (idx = 0) then result := origX else if (idx = 1) then result := origY else result := 0; end;
372 function Ray2D.getDirN (idx: Integer): Single; inline; begin if (idx = 0) then result := dirX else if (idx = 1) then result := dirY else result := 0; end;
376 begin
384 var
386 begin
393 begin
401 begin
411 begin
417 // ////////////////////////////////////////////////////////////////////////// //
419 begin
424 begin
429 begin
434 begin
441 function AABB2D.getvalid (): Boolean; inline; begin result := (minX <= maxX) and (minY <= maxY); end;
443 {$IFDEF aabbtree_use_floats}
446 {$ELSE}
449 {$ENDIF}
453 function AABB2D.getMinN (idx: Integer): TreeNumber; inline; begin if (idx = 0) then result := minX else if (idx = 1) then result := minY else result := 0; end;
454 function AABB2D.getMaxN (idx: Integer): TreeNumber; inline; begin if (idx = 0) then result := maxX else if (idx = 1) then result := maxY else result := 0; end;
457 begin
462 {$IF DEFINED(D2F_DEBUG)}
464 {$ENDIF}
469 begin
474 {$IF DEFINED(D2F_DEBUG)}
476 {$ENDIF}
481 begin
482 {$IF DEFINED(D2F_DEBUG)}
485 {$ENDIF}
490 {$IF DEFINED(D2F_DEBUG)}
492 {$ENDIF}
497 begin
503 begin
504 {$IF DEFINED(D2F_DEBUG)}
506 {$ENDIF}
511 {$IF DEFINED(D2F_DEBUG)}
513 {$ENDIF}
518 begin
519 result :=
526 begin
532 begin
534 // exit with no intersection if found separated along any axis
541 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
542 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
543 {
544 function AABB2D.intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload;
545 var
546 dinv, t1, t2, tmp: Single;
547 tmin, tmax: Single;
548 begin
549 // ok with coplanars
550 tmin := -1.0e100;
551 tmax := 1.0e100;
552 // do X
553 if (ray.dirX <> 0.0) then
554 begin
555 dinv := 1.0/ray.dirX;
556 t1 := (minX-ray.origX)*dinv;
557 t2 := (maxX-ray.origX)*dinv;
558 if (t1 < t2) then tmin := t1 else tmin := t2;
559 if (t1 > t2) then tmax := t1 else tmax := t2;
560 end;
561 // do Y
562 if (ray.dirY <> 0.0) then
563 begin
564 dinv := 1.0/ray.dirY;
565 t1 := (minY-ray.origY)*dinv;
566 t2 := (maxY-ray.origY)*dinv;
567 // tmin
568 if (t1 < t2) then tmp := t1 else tmp := t2; // min(t1, t2)
569 if (tmax < tmp) then tmp := tmax; // min(tmax, tmp)
570 if (tmin > tmp) then tmin := tmp; // max(tmin, tmp)
571 // tmax
572 if (t1 > t2) then tmp := t1 else tmp := t2; // max(t1, t2)
573 if (tmin > tmp) then tmp := tmin; // max(tmin, tmp)
574 if (tmax < tmp) then tmax := tmp; // min(tmax, tmp)
575 end;
576 if (tmin > 0) then tmp := tmin else tmp := 0;
577 if (tmax > tmp) then
578 begin
579 if (tmino <> nil) then tmino^ := tmin;
580 if (tmaxo <> nil) then tmaxo^ := tmax;
581 result := true;
582 end
583 else
584 begin
585 result := false;
586 end;
587 end;
588 }
591 function AABB2D.intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload;
592 var
595 begin
599 begin
601 begin
602 //t1 := (self.min[i]-ray.orig[i])/ray.dir[i];
603 //t2 := (self.max[i]-ray.orig[i])/ray.dir[i];
609 end
611 begin
613 exit;
619 begin
626 function AABB2D.intersects (ax, ay, bx, by: Single; tmino: PSingle=nil): Boolean; inline; overload;
627 var
630 begin
633 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
636 // nope, do it hard way
638 if not intersects(ray, @tmin) then begin if (tmino <> nil) then tmino^ := tmin; result := false; exit; end;
647 function AABB2D.intersects (constref ray: Ray2D; maxtime: Single; tmino: PSingle=nil): Boolean; inline; overload;
648 var
650 begin
652 if (ray.origX >= minX) and (ray.origY >= minY) and (ray.origX <= maxX) and (ray.origY <= maxY) then
653 begin
655 exit;
657 if not intersects(ray, @tmin) then begin if (tmino <> nil) then tmino^ := -1.0; result := false; exit; end;
664 // ////////////////////////////////////////////////////////////////////////// //
665 constructor TDynAABBTreeBase.TSegmentQueryResult.Create (fuckyoufpc: Boolean); begin time := -1; flesh := Default(ITP); end;
666 procedure TDynAABBTreeBase.TSegmentQueryResult.reset (); inline; begin time := -1; flesh := Default(ITP); end;
667 function TDynAABBTreeBase.TSegmentQueryResult.valid (): Boolean; inline; begin result := (time >= 0) and (flesh <> Default(ITP)); end;
670 // ////////////////////////////////////////////////////////////////////////// //
671 function TDynAABBTreeBase.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end;
672 function TDynAABBTreeBase.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end;
675 begin
689 begin
690 e_WriteLog(Format('NODE: parentId=%d; children=[%d,%d]; height=%d; tag=%d; fleshX=%d; fleshY=%d; aabb=(%d,%d)-(%d,%d)',
691 [parentId, children[0], children[1], Integer(height), tag, fleshX, fleshY, aabb.minX, aabb.minY, aabb.maxX, aabb.maxY]),
692 MSG_NOTIFY);
696 // ////////////////////////////////////////////////////////////////////////// //
697 // allocate and return a node to use in the tree
699 var
702 begin
703 // if there is no more allocated node to use
705 begin
707 // allocate more nodes in the tree
711 // initialize the allocated nodes
713 begin
720 // get the next free node
731 //e_WriteLog(Format('tree: allocated node #%d', [result]), MSG_NOTIFY);
735 // release a node
737 begin
747 //e_WriteLog(Format('tree: released node #%d', [nodeId]), MSG_NOTIFY);
751 // insert a leaf node in the tree
752 // 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
754 var
761 begin
762 // if the tree is empty
764 begin
767 exit;
772 // find the best sibling node for the new node
776 begin
780 // compute the merged AABB
785 // compute the cost of making the current node the sibling of the new node
788 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
791 // compute the cost of descending into the left child
796 // compute the cost of descending into the right child
801 // 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
804 // it is cheaper to go down into a child of the current node, choose the best child
805 //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
811 // create a new parent for the new node and the sibling node
819 // if the sibling node was not the root node
821 begin
824 begin
826 end
827 else
828 begin
835 end
836 else
837 begin
838 // if the sibling node was the root node
846 // move up in the tree to change the AABBs that have changed
850 begin
851 // balance the sub-tree of the current node if it is not balanced
861 // recompute the height of the node in the tree
865 // recompute the AABB of the node
875 // remove a leaf node from the tree
877 var
880 begin
884 // if we are removing the root node (root node is a leaf in this case)
891 begin
893 end
894 else
895 begin
899 // if the parent of the node to remove is not the root node
901 begin
902 // destroy the parent node
904 begin
906 end
907 else
908 begin
909 {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
915 // 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
918 begin
919 // balance the current sub-tree if necessary
924 // get the two children of the current node
928 // recompute the AABB and the height of the current node
930 mNodes[currentNodeId].height := dtMaxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
935 end
936 else
937 begin
938 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
946 // balance the sub-tree of a given node using left or right rotations
947 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
948 // this method returns the new root node id
950 var
954 begin
959 // if the node is a leaf or the height of A's sub-tree is less than 2
960 if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
962 // get the two children nodes
970 // compute the factor of the left and right sub-trees
973 // if the right node C is 2 higher than left node B
975 begin
990 begin
992 begin
994 end
995 else
996 begin
997 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
1000 end
1001 else
1002 begin
1009 // if the right node C was higher than left node B because of the F node
1011 begin
1016 // recompute the AABB of node A and C
1020 // recompute the height of node A and C
1025 end
1026 else
1027 begin
1028 // if the right node C was higher than left node B because of node G
1033 // recompute the AABB of node A and C
1037 // recompute the height of node A and C
1044 // return the new root of the sub-tree
1046 exit;
1049 // if the left node B is 2 higher than right node C
1051 begin
1066 begin
1068 begin
1070 end
1071 else
1072 begin
1073 {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
1076 end
1077 else
1078 begin
1085 // if the left node B was higher than right node C because of the F node
1087 begin
1092 // recompute the AABB of node A and B
1096 // recompute the height of node A and B
1101 end
1102 else
1103 begin
1104 // if the left node B was higher than right node C because of node G
1109 // recompute the AABB of node A and B
1113 // recompute the height of node A and B
1120 // return the new root of the sub-tree
1122 exit;
1125 // if the sub-tree is balanced, return the current root node
1130 // compute the height of a given node in the tree
1132 var
1135 begin
1139 // if the node is a leaf, its height is zero
1142 // compute the height of the left and right sub-tree
1146 // return the height of the node
1151 // internally add an object into the tree
1152 function TDynAABBTreeBase.insertObjectInternal (constref aabb: AABB2D; staticObject: Boolean): Integer;
1153 var
1156 begin
1157 // get the next available node (or allocate new ones if necessary)
1162 // create the fat aabb to use in the tree
1165 begin
1172 // set the height of the node in the tree
1175 // insert the new leaf node in the tree
1181 // return the id of the node
1186 // initialize the tree
1188 var
1190 begin
1197 //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1200 // initialize the allocated nodes
1202 begin
1211 // also, checks if the tree structure is valid (for debugging purpose)
1213 var
1217 begin
1220 // if it is the root
1222 // get the children nodes
1226 begin
1227 {$IFDEF aabbtree_use_floats}
1228 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);
1229 {$ELSE}
1230 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);
1231 {$ENDIF}
1233 begin
1235 {$IFDEF aabbtree_use_floats}
1236 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);
1237 {$ELSE}
1238 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);
1239 {$ENDIF}
1244 // if the current node is a leaf
1246 begin
1249 end
1250 else
1251 begin
1254 // check that the children node Ids are valid
1257 // check that the children nodes have the correct parent node
1260 // check the height of node
1263 // check the AABB of the node
1269 // recursively check the children nodes
1276 // also, checks if the tree structure is valid (for debugging purpose)
1278 begin
1279 // recursively check each node
1284 // return `true` from visitor to stop immediately
1285 // checker should check if this node should be considered to further checking
1286 // returns tree node if visitor says stop or -1
1287 function TDynAABBTreeBase.visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer;
1288 const
1290 var
1297 begin
1299 //if not assigned(visitor) and not assigned(visdg) then raise Exception.Create('dyntree: empty visitors aren''t supported');
1305 {$IFDEF aabbtree_query_count}
1308 {$ENDIF}
1310 // start from root node
1311 // we can't have nested functions in generics, sorry
1312 {$IF FALSE}
1314 {$ELSE}
1318 {$ENDIF}
1320 // while there are still nodes to visit
1322 begin
1323 // get the next node id to visit
1324 // we can't have nested functions in generics, sorry
1325 {$IF FALSE}
1327 {$ELSE}
1330 {$ENDIF}
1331 // skip it if it is a nil node
1334 // get the corresponding node
1336 // should we investigate this node?
1339 ModeAABB:
1340 begin
1341 //doNode := caabb.overlaps(node.aabb);
1342 // this gives small speedup (or not...)
1343 // exit with no intersection if found separated along any axis
1348 ModePoint:
1349 begin
1350 //doNode := node.aabb.contains(caabb.minX, caabb.minY);
1351 // this gives small speedup
1352 doNode := (caabb.minX >= node.aabb.minX) and (caabb.minY >= node.aabb.minY) and (caabb.minX <= node.aabb.maxX) and (caabb.minY <= node.aabb.maxY);
1356 begin
1357 // if the node is a leaf
1359 begin
1360 // call visitor on it
1363 begin
1365 // update object vars from cache, so recursive calls to `visit()` will work
1368 // call callbacks
1371 // do some sanity checks
1373 // should we exit?
1375 begin
1379 exit;
1382 end
1383 else
1384 begin
1385 // if the node is not a leaf, we need to visit its children
1386 // we can't have nested functions in generics, sorry
1387 {$IF FALSE}
1390 {$ELSE}
1396 {$ENDIF}
1407 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1408 // 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
1410 begin
1420 begin
1427 // clear all the nodes and reset the tree
1429 begin
1435 function TDynAABBTreeBase.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1438 // return the root AABB of the tree
1440 begin
1441 {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mAllocCount));{$ENDIF}
1446 // does the given id represents a valid object?
1447 // WARNING: ids of removed objects can be reused on later insertions!
1449 begin
1454 // get object by nodeid; can return nil for invalid ids
1456 begin
1457 if (nodeid >= 0) and (nodeid < mAllocCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := Default(ITP);
1460 // get fat object AABB by nodeid; returns random shit for invalid ids
1462 begin
1463 if (nodeid >= 0) and (nodeid < mAllocCount) and (not mNodes[nodeid].isfree) then aabb := AABB2D.Create(mNodes[nodeid].aabb) else aabb := AABB2D.Create(0, 0, 0, 0);
1467 begin
1469 begin
1471 {$IFDEF aabbtree_use_floats}
1474 {$ELSE}
1477 {$ENDIF}
1478 end
1479 else
1480 begin
1484 //if (nodeid >= 0) and (nodeid < mAllocCount) then mNodes[nodeid].dumpToLog();
1489 // insert an object into the tree
1490 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1491 // AABB for static object will not be "fat" (simple optimization)
1492 // WARNING! inserting the same object several times *WILL* break everything!
1493 function TDynAABBTreeBase.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer;
1494 var
1497 begin
1499 begin
1500 {$IFDEF aabbtree_use_floats}
1501 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);
1502 {$ELSE}
1503 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);
1504 {$ENDIF}
1505 //raise Exception.Create('trying to insert invalid flesh in dyntree');
1507 exit;
1510 begin
1511 {$IFDEF aabbtree_use_floats}
1512 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);
1513 {$ELSE}
1514 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);
1515 {$ENDIF}
1518 exit;
1520 //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);
1533 // remove an object from the tree
1534 // WARNING: ids of removed objects can be reused on later insertions!
1536 begin
1537 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase');
1538 // remove the node from the tree
1544 function TDynAABBTreeBase.updateObject (nodeId: Integer; forceReinsert: Boolean=false): Boolean; overload;
1545 var
1548 begin
1549 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject');
1551 if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject');
1552 if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject');
1563 function TDynAABBTreeBase.updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload;
1564 var
1568 begin
1569 if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject');
1571 if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject');
1572 if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject');
1577 // if the new AABB is still inside the fat AABB of the node
1579 begin
1584 exit;
1587 // if the new AABB is outside the fat AABB, we remove the corresponding node
1592 // compute the fat AABB by inflating the AABB with a constant gap
1598 begin
1605 // inflate the fat AABB in direction of the linear motion of the AABB
1607 begin
1608 node.aabb.minX += LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1609 end
1610 else
1611 begin
1612 node.aabb.maxX += LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1616 begin
1617 node.aabb.minY += LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1618 end
1619 else
1620 begin
1621 node.aabb.maxY += LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF};
1626 // reinsert the node into the tree
1634 begin
1639 // report all shapes overlapping with the AABB given in parameter
1640 function TDynAABBTreeBase.aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
1641 var
1644 begin
1648 //chkAABB := AABB2D.Create(ax, ay, ax+aw, ay+ah);
1661 begin
1666 // report body that contains the given point, or nil
1667 function TDynAABBTreeBase.pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh;
1668 var
1671 begin
1675 {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mAllocCount) and (mNodes[nid].leaf)));{$ENDIF}
1682 //var tmin: Single = 0;
1683 begin
1684 {$IF FALSE}
1693 tmin,
1696 {$ELSE}
1698 if (node.aabb.maxX < minSingle(curax, curbx)) or (node.aabb.maxY < minSingle(curay, curby)) then exit;
1699 if (node.aabb.minX > maxSingle(curax, curbx)) or (node.aabb.minY > maxSingle(curay, curby)) then exit;
1701 {
1702 e_WriteLog(Format('intersect: (%f,%f)-(%f,%f) (%d,%d)-(%d,%d) tmin=%f res=%d frac=%f', [
1703 curax, curay, curbx, curby,
1704 node.aabb.minX, node.aabb.minY,
1705 node.aabb.maxX, node.aabb.maxY,
1706 tmin,
1707 Integer(result),
1708 qSRes.time
1709 ]), MSG_NOTIFY);
1710 }
1711 {$ENDIF}
1716 var
1719 begin
1725 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1727 begin
1731 exit;
1733 // if the user returned a positive fraction
1735 begin
1736 // we update the maxFraction value and the ray AABB using the new maximum fraction
1738 begin
1741 // fix curb here
1742 //curb := cura+dir*hitFraction;
1751 // segment querying method
1752 function TDynAABBTreeBase.segmentQuery (out qr: TSegmentQueryResult; const ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean;
1753 var
1761 begin
1775 //qr.time := sqrt((bx-ax)*(bx-ax)+(by-ay)*(by-ay))+1.0;
1783 // normalize
1793 //chkAABB := AABB2D.Create(0, 0, 1, 1);
1811 begin
1813 end
1814 else
1815 begin