src/game/z_aabbtree.pas

   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 unit z_aabbtree;
  20
  21 interface
  22
  23 uses e_log;
  24
  25
  26 // ////////////////////////////////////////////////////////////////////////// //
  27 type
  28   Float = Single;
  29   PFloat = ^Float;
  30
  31   TTreeFlesh = TObject;
  32
  33
  34 // ////////////////////////////////////////////////////////////////////////// //
  35 type
  36   Ray2D = record
  37   public
  38     origX, origY: Float;
  39     dirX, dirY: Float;
  40
  41   public
  42     constructor Create (ax, ay: Float; aangle: Float); overload;
  43     constructor Create (ax0, ay0, ax1, ay1: Float); overload;
  44     constructor Create (const aray: Ray2D); overload;
  45
  46     procedure copyFrom (const aray: Ray2D); inline;
  47
  48     procedure normalizeDir (); inline;
  49
  50     procedure setXYAngle (ax, ay: Float; aangle: Float); inline;
  51     procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline;
  52   end;
  53
  54 // ////////////////////////////////////////////////////////////////////////// //
  55 type
  56   AABB2D = record
  57   public
  58     minX, minY, maxX, maxY: Float;
  59
  60   private
  61     function getvalid (): Boolean; inline;
  62     function getcenterX (): Float; inline;
  63     function getcenterY (): Float; inline;
  64     function getextentX (): Float; inline;
  65     function getextentY (): Float; inline;
  66
  67   public
  68     constructor Create (x0, y0, x1, y1: Float); overload;
  69     constructor Create (const aabb: AABB2D); overload;
  70     constructor Create (const aabb0, aabb1: AABB2D); overload;
  71
  72     procedure copyFrom (const aabb: AABB2D); inline;
  73     procedure setDims (x0, y0, x1, y1: Float); inline;
  74
  75     procedure setMergeTwo (const aabb0, aabb1: AABB2D); inline;
  76
  77     function volume (): Float; inline;
  78
  79     procedure merge (const aabb: AABB2D); inline;
  80
  81     // return true if the current AABB contains the AABB given in parameter
  82     function contains (const aabb: AABB2D): Boolean; inline; overload;
  83     function contains (ax, ay: Float): Boolean; inline; overload;
  84
  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
  87     function overlaps (const aabb: AABB2D): Boolean; inline; overload;
  88
  89     // ray direction must be normalized
  90     function intersects (const ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
  91     function intersects (ax, ay, bx, by: Float): Boolean; inline; overload;
  92
  93     property valid: Boolean read getvalid;
  94     property centerX: Float read getcenterX;
  95     property centerY: Float read getcenterY;
  96     property extentX: Float read getextentX;
  97     property extentY: Float read getextentY;
  98   end;
  99
 100
 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
 135   TDynAABBTree = class(TObject)
 136   private
 137     type
 138       PTreeNode = ^TTreeNode;
 139       TTreeNode = record
 140       public
 141         const NullTreeNode = -1;
 142         const Left = 0;
 143         const Right = 1;
 144       public
 145         // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
 146         parentId: Integer;
 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`
 151         flesh: TTreeFlesh;
 152         // height of the node in the tree (-1 for free nodes)
 153         height: SmallInt;
 154         // fat axis aligned bounding box (AABB) corresponding to the node
 155         aabb: AABB2D;
 156       public
 157         // return true if the node is a leaf of the tree
 158         procedure clear (); inline;
 159         function leaf (): Boolean; inline;
 160         function isfree (): Boolean; inline;
 161         property nextNodeId: Integer read parentId write parentId;
 162         //property flesh: Integer read children[0] write children[0];
 163       end;
 164
 165       TVisitCheckerCB = function (node: PTreeNode): Boolean is nested;
 166       TVisitVisitorCB = function (abody: TTreeFlesh): Boolean is nested;
 167
 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!
 171
 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
 176     const LinearMotionGapMultiplier = 1.7;
 177
 178   private
 179     mNodes: array of TTreeNode; // nodes of the tree
 180     mRootNodeId: Integer; // id of the root node of the tree
 181     mFreeNodeId: Integer; // id of the first node of the list of free (allocated) nodes in the tree that we can use
 182     mAllocCount: Integer; // number of allocated nodes in the tree
 183     mNodeCount: Integer; // number of nodes in the tree
 184
 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
 187     mExtraGap: Float;
 188
 189   private
 190     function allocateNode (): Integer;
 191     procedure releaseNode (nodeId: Integer);
 192     procedure insertLeafNode (nodeId: Integer);
 193     procedure removeLeafNode (nodeId: Integer);
 194     function balanceSubTreeAtNode (nodeId: Integer): Integer;
 195     function computeHeight (nodeId: Integer): Integer;
 196     function insertObjectInternal (var aabb: AABB2D; staticObject: Boolean): Integer;
 197     procedure setup ();
 198     function visit (checker: TVisitCheckerCB; visitor: TVisitVisitorCB): Integer;
 199
 200   public
 201     {$IFDEF aabbtree_query_count}
 202     mNodesVisited, mNodesDeepVisited: Integer;
 203     {$ENDIF}
 204
 205   public
 206     // called when a overlapping node has been found during the call to forEachAABBOverlap()
 207     // return `true` to stop
 208     type TQueryOverlapCB = function (abody: TTreeFlesh): Boolean is nested;
 209     type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
 210
 211     TSegmentQueryResult = record
 212       dist: Float; // <0: nothing was hit
 213       flesh: TTreeFlesh;
 214
 215       procedure reset (); inline;
 216       function valid (): Boolean; inline;
 217     end;
 218
 219   public
 220     constructor Create (extraAABBGap: Float=0.0);
 221     destructor Destroy (); override;
 222
 223     // clear all the nodes and reset the tree
 224     procedure reset ();
 225
 226     function forEachLeaf (dg: TForEachLeafCB): Boolean; // WARNING! don't modify AABB/tree here!
 227     procedure getRootAABB (var aabb: AABB2D);
 228
 229     function isValidId (id: Integer): Boolean; inline;
 230     function getNodeObjectId (nodeid: Integer): TTreeFlesh; inline;
 231     procedure getNodeFatAABB (var aabb: AABB2D; nodeid: Integer); inline;
 232
 233     // return `false` for invalid flesh
 234     function getFleshAABB (var aabb: AABB2D; flesh: TTreeFlesh): Boolean; virtual; abstract;
 235
 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!
 240     function insertObject (flesh: TTreeFlesh; staticObject: Boolean=false): Integer;
 241
 242     // remove an object from the tree
 243     // WARNING: ids of removed objects can be reused on later insertions!
 244     procedure removeObject (nodeId: Integer);
 245
 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;
 262
 263     function aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB): Boolean;
 264     function pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh;
 265     function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
 266
 267     function computeTreeHeight (): Integer; // compute the height of the tree
 268
 269     property extraGap: Float read mExtraGap write mExtraGap;
 270     property nodeCount: Integer read mNodeCount;
 271     property nodeAlloced: Integer read mAllocCount;
 272     {$IFDEF aabbtree_query_count}
 273     property nodesVisited: Integer read mNodesVisited;
 274     property nodesDeepVisited: Integer read mNodesDeepVisited;
 275     {$ELSE}
 276     const nodesVisited = 0;
 277     const nodesDeepVisited = 0;
 278     {$ENDIF}
 279   end;
 280
 281
 282 implementation
 283
 284 uses
 285   SysUtils;
 286
 287
 288 // ////////////////////////////////////////////////////////////////////////// //
 289 function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end;
 290 function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end;
 291
 292 function minF (a, b: Float): Float; inline; begin if (a < b) then result := a else result := b; end;
 293 function maxF (a, b: Float): Float; inline; begin if (a > b) then result := a else result := b; end;
 294
 295
 296 // ////////////////////////////////////////////////////////////////////////// //
 297 constructor Ray2D.Create (ax, ay: Float; aangle: Float); begin setXYAngle(ax, ay, aangle); end;
 298 constructor Ray2D.Create (ax0, ay0, ax1, ay1: Float); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end;
 299 constructor Ray2D.Create (const aray: Ray2D); overload; begin copyFrom(aray); end;
 300
 301
 302 procedure Ray2D.copyFrom (const aray: Ray2D); inline;
 303 begin
 304   origX := aray.origX;
 305   origY := aray.origY;
 306   dirX := aray.dirX;
 307   dirY := aray.dirY;
 308 end;
 309
 310 procedure Ray2D.normalizeDir (); inline;
 311 var
 312   invlen: Float;
 313 begin
 314   invlen := 1.0/sqrt(dirX*dirX+dirY*dirY);
 315   dirX *= invlen;
 316   dirY *= invlen;
 317 end;
 318
 319 procedure Ray2D.setXYAngle (ax, ay: Float; aangle: Float); inline;
 320 begin
 321   origX := ax;
 322   origY := ay;
 323   dirX := cos(aangle);
 324   dirY := sin(aangle);
 325 end;
 326
 327 procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline;
 328 begin
 329   origX := ax0;
 330   origY := ay0;
 331   dirX := ax1-ax0;
 332   dirY := ay1-ay0;
 333   normalizeDir();
 334 end;
 335
 336
 337 // ////////////////////////////////////////////////////////////////////////// //
 338 constructor AABB2D.Create (x0, y0, x1, y1: Float); overload;
 339 begin
 340   setDims(x0, y0, x1, y1);
 341 end;
 342
 343 constructor AABB2D.Create (const aabb: AABB2D); overload;
 344 begin
 345   copyFrom(aabb);
 346 end;
 347
 348 constructor AABB2D.Create (const aabb0, aabb1: AABB2D); overload;
 349 begin
 350   setMergeTwo(aabb0, aabb1);
 351 end;
 352
 353 function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end;
 354
 355 function AABB2D.getcenterX (): Float; inline; begin result := (minX+maxX)/2.0; end;
 356 function AABB2D.getcenterY (): Float; inline; begin result := (minY+maxY)/2.0; end;
 357 function AABB2D.getextentX (): Float; inline; begin result := (maxX-minX)+1.0; end;
 358 function AABB2D.getextentY (): Float; inline; begin result := (maxY-minY)+1.0; end;
 359
 360
 361 procedure AABB2D.copyFrom (const aabb: AABB2D); inline;
 362 begin
 363   minX := aabb.minX;
 364   minY := aabb.minY;
 365   maxX := aabb.maxX;
 366   maxY := aabb.maxY;
 367   {$IF DEFINED(D2F_DEBUG)}
 368   if not valid then raise Exception.Create('copyFrom: result is fucked');
 369   {$ENDIF}
 370 end;
 371
 372
 373 procedure AABB2D.setDims (x0, y0, x1, y1: Float); inline;
 374 begin
 375   minX := minF(x0, x1);
 376   minY := minF(y0, y1);
 377   maxX := maxF(x0, x1);
 378   maxY := maxF(y0, y1);
 379   {$IF DEFINED(D2F_DEBUG)}
 380   if not valid then raise Exception.Create('setDims: result is fucked');
 381   {$ENDIF}
 382 end;
 383
 384
 385 procedure AABB2D.setMergeTwo (const aabb0, aabb1: AABB2D); inline;
 386 begin
 387   {$IF DEFINED(D2F_DEBUG)}
 388   if not aabb0.valid then raise Exception.Create('setMergeTwo: aabb0 is fucked');
 389   if not aabb1.valid then raise Exception.Create('setMergeTwo: aabb0 is fucked');
 390   {$ENDIF}
 391   minX := minF(aabb0.minX, aabb1.minX);
 392   minY := minF(aabb0.minY, aabb1.minY);
 393   maxX := maxF(aabb0.maxX, aabb1.maxX);
 394   maxY := maxF(aabb0.maxY, aabb1.maxY);
 395   {$IF DEFINED(D2F_DEBUG)}
 396   if not valid then raise Exception.Create('setMergeTwo: result is fucked');
 397   {$ENDIF}
 398 end;
 399
 400
 401 function AABB2D.volume (): Float; inline;
 402 begin
 403   result := (maxX-minX)*(maxY-minY);
 404 end;
 405
 406
 407 procedure AABB2D.merge (const aabb: AABB2D); inline;
 408 begin
 409   {$IF DEFINED(D2F_DEBUG)}
 410   if not aabb.valid then raise Exception.Create('merge: aabb is fucked');
 411   {$ENDIF}
 412   minX := minF(minX, aabb.minX);
 413   minY := minF(minY, aabb.minY);
 414   maxX := maxF(maxX, aabb.maxX);
 415   maxY := maxF(maxY, aabb.maxY);
 416   {$IF DEFINED(D2F_DEBUG)}
 417   if not valid then raise Exception.Create('setMergeTwo: result is fucked');
 418   {$ENDIF}
 419 end;
 420
 421
 422 function AABB2D.contains (const aabb: AABB2D): Boolean; inline; overload;
 423 begin
 424   result :=
 425     (aabb.minX >= minX) and (aabb.minY >= minY) and
 426     (aabb.maxX <= maxX) and (aabb.maxY <= maxY);
 427 end;
 428
 429
 430 function AABB2D.contains (ax, ay: Float): Boolean; inline; overload;
 431 begin
 432   result := (ax >= minX) and (ay >= minY) and (ax <= maxX) and (ay <= maxY);
 433 end;
 434
 435
 436 function AABB2D.overlaps (const aabb: AABB2D): Boolean; inline; overload;
 437 begin
 438   result := false;
 439   // exit with no intersection if found separated along any axis
 440   if (maxX < aabb.minX) or (minX > aabb.maxX) then exit;
 441   if (maxY < aabb.minY) or (minY > aabb.maxY) then exit;
 442   result := true;
 443 end;
 444
 445
 446 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
 447 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
 448 function AABB2D.intersects (const ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
 449 var
 450   dinv, t1, t2, tmp: Float;
 451   tmin, tmax: Float;
 452 begin
 453   // ok with coplanars
 454   tmin := -1.0e100;
 455   tmax := 1.0e100;
 456   // do X
 457   if (ray.dirX <> 0.0) then
 458   begin
 459     dinv := 1.0/ray.dirX;
 460     t1 := (minX-ray.origX)*dinv;
 461     t2 := (maxX-ray.origX)*dinv;
 462     if (t1 < t2) then tmin := t1 else tmin := t2;
 463     if (t1 > t2) then tmax := t1 else tmax := t2;
 464   end;
 465   // do Y
 466   if (ray.dirY <> 0.0) then
 467   begin
 468     dinv := 1.0/ray.dirY;
 469     t1 := (minY-ray.origY)*dinv;
 470     t2 := (maxY-ray.origY)*dinv;
 471     // tmin
 472     if (t1 < t2) then tmp := t1 else tmp := t2; // min(t1, t2)
 473     if (tmax < tmp) then tmp := tmax; // min(tmax, tmp)
 474     if (tmin > tmp) then tmin := tmp; // max(tmin, tmp)
 475     // tmax
 476     if (t1 > t2) then tmp := t1 else tmp := t2; // max(t1, t2)
 477     if (tmin > tmp) then tmp := tmin; // max(tmin, tmp)
 478     if (tmax < tmp) then tmax := tmp; // min(tmax, tmp)
 479   end;
 480   if (tmin > 0) then tmp := tmin else tmp := 0;
 481   if (tmax > tmp) then
 482   begin
 483     if (tmino <> nil) then tmino^ := tmin;
 484     if (tmaxo <> nil) then tmaxo^ := tmax;
 485     result := true;
 486   end
 487   else
 488   begin
 489     result := false;
 490   end;
 491 end;
 492
 493 function AABB2D.intersects (ax, ay, bx, by: Float): Boolean; inline; overload;
 494 var
 495   tmin: Float;
 496   ray: Ray2D;
 497 begin
 498   result := true;
 499   // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
 500   if (ax >= minX) and (ay >= minY) and (ax <= maxX) and (ay <= maxY) then exit; // a
 501   if (bx >= minX) and (by >= minY) and (bx <= maxX) and (by <= maxY) then exit; // b
 502   // nope, do it hard way
 503   ray := Ray2D.Create(ax, ay, bx, by);
 504   if not intersects(ray, @tmin) then begin result := false; exit; end;
 505   if (tmin < 0) then exit; // inside, just in case
 506   bx := bx-ax;
 507   by := by-ay;
 508   result := (tmin*tmin <= bx*bx+by*by);
 509 end;
 510
 511
 512 // ////////////////////////////////////////////////////////////////////////// //
 513 procedure TDynAABBTree.TSegmentQueryResult.reset (); inline; begin dist := -1; flesh := nil; end;
 514 function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; inline; begin result := (dist >= 0) and (flesh <> nil); end;
 515
 516
 517 // ////////////////////////////////////////////////////////////////////////// //
 518 function TDynAABBTree.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end;
 519 function TDynAABBTree.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end;
 520
 521 procedure TDynAABBTree.TTreeNode.clear (); inline;
 522 begin
 523   parentId := 0;
 524   children[0] := 0;
 525   children[1] := 0;
 526   flesh := nil;
 527   height := 0;
 528   aabb.minX := 0;
 529   aabb.minY := 0;
 530   aabb.maxX := 0;
 531   aabb.maxY := 0;
 532 end;
 533
 534
 535 // ////////////////////////////////////////////////////////////////////////// //
 536 // allocate and return a node to use in the tree
 537 function TDynAABBTree.allocateNode (): Integer;
 538 var
 539   i, newsz, freeNodeId: Integer;
 540   node: PTreeNode;
 541 begin
 542   // if there is no more allocated node to use
 543   if (mFreeNodeId = TTreeNode.NullTreeNode) then
 544   begin
 545     {$IFDEF aabbtree_many_asserts}assert(mNodeCount = mAllocCount);{$ENDIF}
 546     // allocate more nodes in the tree
 547     if (mAllocCount < 32768) then newsz := mAllocCount*2 else newsz := mAllocCount+16384;
 548     SetLength(mNodes, newsz);
 549     mAllocCount := newsz;
 550     // initialize the allocated nodes
 551     for i := mNodeCount to mAllocCount-1 do
 552     begin
 553       mNodes[i].nextNodeId := i+1;
 554       mNodes[i].height := -1;
 555     end;
 556     mNodes[mAllocCount-1].nextNodeId := TTreeNode.NullTreeNode;
 557     mFreeNodeId := mNodeCount;
 558   end;
 559   // get the next free node
 560   freeNodeId := mFreeNodeId;
 561   {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF}
 562   node := @mNodes[freeNodeId];
 563   mFreeNodeId := node.nextNodeId;
 564   node.clear();
 565   node.parentId := TTreeNode.NullTreeNode;
 566   node.height := 0;
 567   Inc(mNodeCount);
 568   result := freeNodeId;
 569 end;
 570
 571
 572 // release a node
 573 procedure TDynAABBTree.releaseNode (nodeId: Integer);
 574 begin
 575   {$IFDEF aabbtree_many_asserts}assert(mNodeCount > 0);{$ENDIF}
 576   {$IFDEF aabbtree_many_asserts}assert((nodeId >= 0) and (nodeId < mAllocCount));{$ENDIF}
 577   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].height >= 0);{$ENDIF}
 578   mNodes[nodeId].nextNodeId := mFreeNodeId;
 579   mNodes[nodeId].height := -1;
 580   mNodes[nodeId].flesh := nil;
 581   mFreeNodeId := nodeId;
 582   Dec(mNodeCount);
 583 end;
 584
 585
 586 // insert a leaf node in the tree
 587 // 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
 588 procedure TDynAABBTree.insertLeafNode (nodeId: Integer);
 589 var
 590   newNodeAABB, mergedAABBs, currentAndLeftAABB, currentAndRightAABB: AABB2D;
 591   currentNodeId: Integer;
 592   leftChild, rightChild, siblingNode: Integer;
 593   oldParentNode, newParentNode: Integer;
 594   volumeAABB, mergedVolume: Float;
 595   costS, costI, costLeft, costRight: Float;
 596 begin
 597   // if the tree is empty
 598   if (mRootNodeId = TTreeNode.NullTreeNode) then
 599   begin
 600     mRootNodeId := nodeId;
 601     mNodes[mRootNodeId].parentId := TTreeNode.NullTreeNode;
 602     exit;
 603   end;
 604
 605   {$IFDEF aabbtree_many_asserts}assert(mRootNodeId <> TTreeNode.NullTreeNode);{$ENDIF}
 606
 607   // find the best sibling node for the new node
 608   newNodeAABB := AABB2D.Create(mNodes[nodeId].aabb);
 609   currentNodeId := mRootNodeId;
 610   while not mNodes[currentNodeId].leaf do
 611   begin
 612     leftChild := mNodes[currentNodeId].children[TTreeNode.Left];
 613     rightChild := mNodes[currentNodeId].children[TTreeNode.Right];
 614
 615     // compute the merged AABB
 616     volumeAABB := mNodes[currentNodeId].aabb.volume;
 617     mergedAABBs := AABB2D.Create(mNodes[currentNodeId].aabb, newNodeAABB);
 618     mergedVolume := mergedAABBs.volume;
 619
 620     // compute the cost of making the current node the sibling of the new node
 621     costS := 2.0*mergedVolume;
 622
 623     // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
 624     costI := 2.0*(mergedVolume-volumeAABB);
 625
 626     // compute the cost of descending into the left child
 627     currentAndLeftAABB := AABB2D.Create(newNodeAABB, mNodes[leftChild].aabb);
 628     costLeft := currentAndLeftAABB.volume+costI;
 629     if not mNodes[leftChild].leaf then costLeft -= mNodes[leftChild].aabb.volume;
 630
 631     // compute the cost of descending into the right child
 632     currentAndRightAABB := AABB2D.Create(newNodeAABB, mNodes[rightChild].aabb);
 633     costRight := currentAndRightAABB.volume+costI;
 634     if not mNodes[rightChild].leaf then costRight -= mNodes[rightChild].aabb.volume;
 635
 636     // 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
 637     if (costS < costLeft) and (costS < costRight) then break;
 638
 639     // it is cheaper to go down into a child of the current node, choose the best child
 640     //currentNodeId = (costLeft < costRight ? leftChild : rightChild);
 641     if (costLeft < costRight) then currentNodeId := leftChild else currentNodeId := rightChild;
 642   end;
 643
 644   siblingNode := currentNodeId;
 645
 646   // create a new parent for the new node and the sibling node
 647   oldParentNode := mNodes[siblingNode].parentId;
 648   newParentNode := allocateNode();
 649   mNodes[newParentNode].parentId := oldParentNode;
 650   mNodes[newParentNode].aabb.setMergeTwo(mNodes[siblingNode].aabb, newNodeAABB);
 651   mNodes[newParentNode].height := mNodes[siblingNode].height+1;
 652   {$IFDEF aabbtree_many_asserts}assert(mNodes[newParentNode].height > 0);{$ENDIF}
 653
 654   // if the sibling node was not the root node
 655   if (oldParentNode <> TTreeNode.NullTreeNode) then
 656   begin
 657     {$IFDEF aabbtree_many_asserts}assert(not mNodes[oldParentNode].leaf);{$ENDIF}
 658     if (mNodes[oldParentNode].children[TTreeNode.Left] = siblingNode) then
 659     begin
 660       mNodes[oldParentNode].children[TTreeNode.Left] := newParentNode;
 661     end
 662     else
 663     begin
 664       mNodes[oldParentNode].children[TTreeNode.Right] := newParentNode;
 665     end;
 666     mNodes[newParentNode].children[TTreeNode.Left] := siblingNode;
 667     mNodes[newParentNode].children[TTreeNode.Right] := nodeId;
 668     mNodes[siblingNode].parentId := newParentNode;
 669     mNodes[nodeId].parentId := newParentNode;
 670   end
 671   else
 672   begin
 673     // if the sibling node was the root node
 674     mNodes[newParentNode].children[TTreeNode.Left] := siblingNode;
 675     mNodes[newParentNode].children[TTreeNode.Right] := nodeId;
 676     mNodes[siblingNode].parentId := newParentNode;
 677     mNodes[nodeId].parentId := newParentNode;
 678     mRootNodeId := newParentNode;
 679   end;
 680
 681   // move up in the tree to change the AABBs that have changed
 682   currentNodeId := mNodes[nodeId].parentId;
 683   {$IFDEF aabbtree_many_asserts}assert(not mNodes[currentNodeId].leaf);{$ENDIF}
 684   while (currentNodeId <> TTreeNode.NullTreeNode) do
 685   begin
 686     // balance the sub-tree of the current node if it is not balanced
 687     currentNodeId := balanceSubTreeAtNode(currentNodeId);
 688     {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF}
 689
 690     {$IFDEF aabbtree_many_asserts}assert(not mNodes[currentNodeId].leaf);{$ENDIF}
 691     leftChild := mNodes[currentNodeId].children[TTreeNode.Left];
 692     rightChild := mNodes[currentNodeId].children[TTreeNode.Right];
 693     {$IFDEF aabbtree_many_asserts}assert(leftChild <> TTreeNode.NullTreeNode);{$ENDIF}
 694     {$IFDEF aabbtree_many_asserts}assert(rightChild <> TTreeNode.NullTreeNode);{$ENDIF}
 695
 696     // recompute the height of the node in the tree
 697     mNodes[currentNodeId].height := maxI(mNodes[leftChild].height, mNodes[rightChild].height)+1;
 698     {$IFDEF aabbtree_many_asserts}assert(mNodes[currentNodeId].height > 0);{$ENDIF}
 699
 700     // recompute the AABB of the node
 701     mNodes[currentNodeId].aabb.setMergeTwo(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
 702
 703     currentNodeId := mNodes[currentNodeId].parentId;
 704   end;
 705
 706   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF}
 707 end;
 708
 709
 710 // remove a leaf node from the tree
 711 procedure TDynAABBTree.removeLeafNode (nodeId: Integer);
 712 var
 713   currentNodeId, parentNodeId, grandParentNodeId, siblingNodeId: Integer;
 714   leftChildId, rightChildId: Integer;
 715 begin
 716   {$IFDEF aabbtree_many_asserts}assert((nodeId >= 0) and (nodeId < mAllocCount));{$ENDIF}
 717   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF}
 718
 719   // if we are removing the root node (root node is a leaf in this case)
 720   if (mRootNodeId = nodeId) then begin mRootNodeId := TTreeNode.NullTreeNode; exit; end;
 721
 722   parentNodeId := mNodes[nodeId].parentId;
 723   grandParentNodeId := mNodes[parentNodeId].parentId;
 724
 725   if (mNodes[parentNodeId].children[TTreeNode.Left] = nodeId) then
 726   begin
 727     siblingNodeId := mNodes[parentNodeId].children[TTreeNode.Right];
 728   end
 729   else
 730   begin
 731     siblingNodeId := mNodes[parentNodeId].children[TTreeNode.Left];
 732   end;
 733
 734   // if the parent of the node to remove is not the root node
 735   if (grandParentNodeId <> TTreeNode.NullTreeNode) then
 736   begin
 737     // destroy the parent node
 738     if (mNodes[grandParentNodeId].children[TTreeNode.Left] = parentNodeId) then
 739     begin
 740       mNodes[grandParentNodeId].children[TTreeNode.Left] := siblingNodeId;
 741     end
 742     else
 743     begin
 744       {$IFDEF aabbtree_many_asserts}assert(mNodes[grandParentNodeId].children[TTreeNode.Right] = parentNodeId);{$ENDIF}
 745       mNodes[grandParentNodeId].children[TTreeNode.Right] := siblingNodeId;
 746     end;
 747     mNodes[siblingNodeId].parentId := grandParentNodeId;
 748     releaseNode(parentNodeId);
 749
 750     // 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
 751     currentNodeId := grandParentNodeId;
 752     while (currentNodeId <> TTreeNode.NullTreeNode) do
 753     begin
 754       // balance the current sub-tree if necessary
 755       currentNodeId := balanceSubTreeAtNode(currentNodeId);
 756
 757       {$IFDEF aabbtree_many_asserts}assert(not mNodes[currentNodeId].leaf);{$ENDIF}
 758
 759       // get the two children of the current node
 760       leftChildId := mNodes[currentNodeId].children[TTreeNode.Left];
 761       rightChildId := mNodes[currentNodeId].children[TTreeNode.Right];
 762
 763       // recompute the AABB and the height of the current node
 764       mNodes[currentNodeId].aabb.setMergeTwo(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb);
 765       mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1;
 766       {$IFDEF aabbtree_many_asserts}assert(mNodes[currentNodeId].height > 0);{$ENDIF}
 767
 768       currentNodeId := mNodes[currentNodeId].parentId;
 769     end;
 770   end
 771   else
 772   begin
 773     // if the parent of the node to remove is the root node, the sibling node becomes the new root node
 774     mRootNodeId := siblingNodeId;
 775     mNodes[siblingNodeId].parentId := TTreeNode.NullTreeNode;
 776     releaseNode(parentNodeId);
 777   end;
 778 end;
 779
 780
 781 // balance the sub-tree of a given node using left or right rotations
 782 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
 783 // this method returns the new root node id
 784 function TDynAABBTree.balanceSubTreeAtNode (nodeId: Integer): Integer;
 785 var
 786   nodeA, nodeB, nodeC, nodeF, nodeG: PTreeNode;
 787   nodeBId, nodeCId, nodeFId, nodeGId: Integer;
 788   balanceFactor: Integer;
 789 begin
 790   {$IFDEF aabbtree_many_asserts}assert(nodeId <> TTreeNode.NullTreeNode);{$ENDIF}
 791
 792   nodeA := @mNodes[nodeId];
 793
 794   // if the node is a leaf or the height of A's sub-tree is less than 2
 795   if (nodeA.leaf) or (nodeA.height < 2) then begin result := nodeId; exit; end; // do not perform any rotation
 796
 797   // get the two children nodes
 798   nodeBId := nodeA.children[TTreeNode.Left];
 799   nodeCId := nodeA.children[TTreeNode.Right];
 800   {$IFDEF aabbtree_many_asserts}assert((nodeBId >= 0) and (nodeBId < mAllocCount));{$ENDIF}
 801   {$IFDEF aabbtree_many_asserts}assert((nodeCId >= 0) and (nodeCId < mAllocCount));{$ENDIF}
 802   nodeB := @mNodes[nodeBId];
 803   nodeC := @mNodes[nodeCId];
 804
 805   // compute the factor of the left and right sub-trees
 806   balanceFactor := nodeC.height-nodeB.height;
 807
 808   // if the right node C is 2 higher than left node B
 809   if (balanceFactor > 1.0) then
 810   begin
 811     {$IFDEF aabbtree_many_asserts}assert(not nodeC.leaf);{$ENDIF}
 812
 813     nodeFId := nodeC.children[TTreeNode.Left];
 814     nodeGId := nodeC.children[TTreeNode.Right];
 815     {$IFDEF aabbtree_many_asserts}assert((nodeFId >= 0) and (nodeFId < mAllocCount));{$ENDIF}
 816     {$IFDEF aabbtree_many_asserts}assert((nodeGId >= 0) and (nodeGId < mAllocCount));{$ENDIF}
 817     nodeF := @mNodes[nodeFId];
 818     nodeG := @mNodes[nodeGId];
 819
 820     nodeC.children[TTreeNode.Left] := nodeId;
 821     nodeC.parentId := nodeA.parentId;
 822     nodeA.parentId := nodeCId;
 823
 824     if (nodeC.parentId <> TTreeNode.NullTreeNode) then
 825     begin
 826       if (mNodes[nodeC.parentId].children[TTreeNode.Left] = nodeId) then
 827       begin
 828         mNodes[nodeC.parentId].children[TTreeNode.Left] := nodeCId;
 829       end
 830       else
 831       begin
 832         {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeC.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
 833         mNodes[nodeC.parentId].children[TTreeNode.Right] := nodeCId;
 834       end;
 835     end
 836     else
 837     begin
 838       mRootNodeId := nodeCId;
 839     end;
 840
 841     {$IFDEF aabbtree_many_asserts}assert(not nodeC.leaf);{$ENDIF}
 842     {$IFDEF aabbtree_many_asserts}assert(not nodeA.leaf);{$ENDIF}
 843
 844     // if the right node C was higher than left node B because of the F node
 845     if (nodeF.height > nodeG.height) then
 846     begin
 847       nodeC.children[TTreeNode.Right] := nodeFId;
 848       nodeA.children[TTreeNode.Right] := nodeGId;
 849       nodeG.parentId := nodeId;
 850
 851       // recompute the AABB of node A and C
 852       nodeA.aabb.setMergeTwo(nodeB.aabb, nodeG.aabb);
 853       nodeC.aabb.setMergeTwo(nodeA.aabb, nodeF.aabb);
 854
 855       // recompute the height of node A and C
 856       nodeA.height := maxI(nodeB.height, nodeG.height)+1;
 857       nodeC.height := maxI(nodeA.height, nodeF.height)+1;
 858       {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF}
 859       {$IFDEF aabbtree_many_asserts}assert(nodeC.height > 0);{$ENDIF}
 860     end
 861     else
 862     begin
 863       // if the right node C was higher than left node B because of node G
 864       nodeC.children[TTreeNode.Right] := nodeGId;
 865       nodeA.children[TTreeNode.Right] := nodeFId;
 866       nodeF.parentId := nodeId;
 867
 868       // recompute the AABB of node A and C
 869       nodeA.aabb.setMergeTwo(nodeB.aabb, nodeF.aabb);
 870       nodeC.aabb.setMergeTwo(nodeA.aabb, nodeG.aabb);
 871
 872       // recompute the height of node A and C
 873       nodeA.height := maxI(nodeB.height, nodeF.height)+1;
 874       nodeC.height := maxI(nodeA.height, nodeG.height)+1;
 875       {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF}
 876       {$IFDEF aabbtree_many_asserts}assert(nodeC.height > 0);{$ENDIF}
 877     end;
 878
 879     // return the new root of the sub-tree
 880     result := nodeCId;
 881     exit;
 882   end;
 883
 884   // if the left node B is 2 higher than right node C
 885   if (balanceFactor < -1) then
 886   begin
 887     {$IFDEF aabbtree_many_asserts}assert(not nodeB.leaf);{$ENDIF}
 888
 889     nodeFId := nodeB.children[TTreeNode.Left];
 890     nodeGId := nodeB.children[TTreeNode.Right];
 891     {$IFDEF aabbtree_many_asserts}assert((nodeFId >= 0) and (nodeFId < mAllocCount));{$ENDIF}
 892     {$IFDEF aabbtree_many_asserts}assert((nodeGId >= 0) and (nodeGId < mAllocCount));{$ENDIF}
 893     nodeF := @mNodes[nodeFId];
 894     nodeG := @mNodes[nodeGId];
 895
 896     nodeB.children[TTreeNode.Left] := nodeId;
 897     nodeB.parentId := nodeA.parentId;
 898     nodeA.parentId := nodeBId;
 899
 900     if (nodeB.parentId <> TTreeNode.NullTreeNode) then
 901     begin
 902       if (mNodes[nodeB.parentId].children[TTreeNode.Left] = nodeId) then
 903       begin
 904         mNodes[nodeB.parentId].children[TTreeNode.Left] := nodeBId;
 905       end
 906       else
 907       begin
 908         {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeB.parentId].children[TTreeNode.Right] = nodeId);{$ENDIF}
 909         mNodes[nodeB.parentId].children[TTreeNode.Right] := nodeBId;
 910       end;
 911     end
 912     else
 913     begin
 914       mRootNodeId := nodeBId;
 915     end;
 916
 917     {$IFDEF aabbtree_many_asserts}assert(not nodeB.leaf);{$ENDIF}
 918     {$IFDEF aabbtree_many_asserts}assert(not nodeA.leaf);{$ENDIF}
 919
 920     // if the left node B was higher than right node C because of the F node
 921     if (nodeF.height > nodeG.height) then
 922     begin
 923       nodeB.children[TTreeNode.Right] := nodeFId;
 924       nodeA.children[TTreeNode.Left] := nodeGId;
 925       nodeG.parentId := nodeId;
 926
 927       // recompute the AABB of node A and B
 928       nodeA.aabb.setMergeTwo(nodeC.aabb, nodeG.aabb);
 929       nodeB.aabb.setMergeTwo(nodeA.aabb, nodeF.aabb);
 930
 931       // recompute the height of node A and B
 932       nodeA.height := maxI(nodeC.height, nodeG.height)+1;
 933       nodeB.height := maxI(nodeA.height, nodeF.height)+1;
 934       {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF}
 935       {$IFDEF aabbtree_many_asserts}assert(nodeB.height > 0);{$ENDIF}
 936     end
 937     else
 938     begin
 939       // if the left node B was higher than right node C because of node G
 940       nodeB.children[TTreeNode.Right] := nodeGId;
 941       nodeA.children[TTreeNode.Left] := nodeFId;
 942       nodeF.parentId := nodeId;
 943
 944       // recompute the AABB of node A and B
 945       nodeA.aabb.setMergeTwo(nodeC.aabb, nodeF.aabb);
 946       nodeB.aabb.setMergeTwo(nodeA.aabb, nodeG.aabb);
 947
 948       // recompute the height of node A and B
 949       nodeA.height := maxI(nodeC.height, nodeF.height)+1;
 950       nodeB.height := maxI(nodeA.height, nodeG.height)+1;
 951       {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF}
 952       {$IFDEF aabbtree_many_asserts}assert(nodeB.height > 0);{$ENDIF}
 953     end;
 954
 955     // return the new root of the sub-tree
 956     result := nodeBId;
 957     exit;
 958   end;
 959
 960   // if the sub-tree is balanced, return the current root node
 961   result := nodeId;
 962 end;
 963
 964
 965 // compute the height of a given node in the tree
 966 function TDynAABBTree.computeHeight (nodeId: Integer): Integer;
 967 var
 968   node: PTreeNode;
 969   leftHeight, rightHeight: Integer;
 970 begin
 971   {$IFDEF aabbtree_many_asserts}assert((nodeId >= 0) and (nodeId < mAllocCount));{$ENDIF}
 972   node := @mNodes[nodeId];
 973
 974   // if the node is a leaf, its height is zero
 975   if (node.leaf) then begin result := 0; exit; end;
 976
 977   // compute the height of the left and right sub-tree
 978   leftHeight := computeHeight(node.children[TTreeNode.Left]);
 979   rightHeight := computeHeight(node.children[TTreeNode.Right]);
 980
 981   // return the height of the node
 982   result := 1+maxI(leftHeight, rightHeight);
 983 end;
 984
 985
 986 // internally add an object into the tree
 987 function TDynAABBTree.insertObjectInternal (var aabb: AABB2D; staticObject: Boolean): Integer;
 988 var
 989   nodeId: Integer;
 990 begin
 991   // get the next available node (or allocate new ones if necessary)
 992   nodeId := allocateNode();
 993
 994   // create the fat aabb to use in the tree
 995   mNodes[nodeId].aabb := AABB2D.Create(aabb);
 996   if (not staticObject) then
 997   begin
 998     mNodes[nodeId].aabb.minX -= mExtraGap;
 999     mNodes[nodeId].aabb.minY -= mExtraGap;
1000     mNodes[nodeId].aabb.maxX += mExtraGap;
1001     mNodes[nodeId].aabb.maxY += mExtraGap;
1002   end;
1003
1004   // set the height of the node in the tree
1005   mNodes[nodeId].height := 0;
1006
1007   // insert the new leaf node in the tree
1008   insertLeafNode(nodeId);
1009   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF}
1010
1011   {$IFDEF aabbtree_many_asserts}assert(nodeId >= 0);{$ENDIF}
1012
1013   // return the id of the node
1014   result := nodeId;
1015 end;
1016
1017
1018 // initialize the tree
1019 procedure TDynAABBTree.setup ();
1020 var
1021   i: Integer;
1022 begin
1023   mRootNodeId := TTreeNode.NullTreeNode;
1024   mNodeCount := 0;
1025   mAllocCount := 8192;
1026
1027   SetLength(mNodes, mAllocCount);
1028   //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof);
1029   for i := 0 to mAllocCount-1 do mNodes[i].clear();
1030
1031   // initialize the allocated nodes
1032   for i := 0 to mAllocCount-1 do
1033   begin
1034     mNodes[i].nextNodeId := i+1;
1035     mNodes[i].height := -1;
1036   end;
1037   mNodes[mAllocCount-1].nextNodeId := TTreeNode.NullTreeNode;
1038   mFreeNodeId := 0;
1039 end;
1040
1041
1042 // also, checks if the tree structure is valid (for debugging purpose)
1043 function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean;
1044   function forEachNode (nodeId: Integer): Boolean;
1045   var
1046     pNode: PTreeNode;
1047     leftChild, rightChild, height: Integer;
1048     aabb: AABB2D;
1049   begin
1050     result := false;
1051     if (nodeId = TTreeNode.NullTreeNode) then exit;
1052     // if it is the root
1053     if (nodeId = mRootNodeId) then assert(mNodes[nodeId].parentId = TTreeNode.NullTreeNode);
1054     // get the children nodes
1055     pNode := @mNodes[nodeId];
1056     assert(pNode.height >= 0);
1057     if (not pNode.aabb.valid) then
1058     begin
1059       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);
1060       if pNode.leaf then
1061       begin
1062         getFleshAABB(aabb, pNode.flesh);
1063         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);
1064       end;
1065     end;
1066     assert(pNode.aabb.valid);
1067     assert(pNode.aabb.volume > 0);
1068     // if the current node is a leaf
1069     if (pNode.leaf) then
1070     begin
1071       assert(pNode.height = 0);
1072       if assigned(dg) then result := dg(pNode.flesh, pNode.aabb);
1073     end
1074     else
1075     begin
1076       leftChild := pNode.children[TTreeNode.Left];
1077       rightChild := pNode.children[TTreeNode.Right];
1078       // check that the children node Ids are valid
1079       assert((0 <= leftChild) and (leftChild < mAllocCount));
1080       assert((0 <= rightChild) and (rightChild < mAllocCount));
1081       // check that the children nodes have the correct parent node
1082       assert(mNodes[leftChild].parentId = nodeId);
1083       assert(mNodes[rightChild].parentId = nodeId);
1084       // check the height of node
1085       height := 1+maxI(mNodes[leftChild].height, mNodes[rightChild].height);
1086       assert(mNodes[nodeId].height = height);
1087       // check the AABB of the node
1088       aabb := AABB2D.Create(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
1089       assert(aabb.minX = mNodes[nodeId].aabb.minX);
1090       assert(aabb.minY = mNodes[nodeId].aabb.minY);
1091       assert(aabb.maxX = mNodes[nodeId].aabb.maxX);
1092       assert(aabb.maxY = mNodes[nodeId].aabb.maxY);
1093       // recursively check the children nodes
1094       result := forEachNode(leftChild);
1095       if not result then result := forEachNode(rightChild);
1096     end;
1097   end;
1098
1099 begin
1100   // recursively check each node
1101   result := forEachNode(mRootNodeId);
1102 end;
1103
1104
1105 // return `true` from visitor to stop immediately
1106 // checker should check if this node should be considered to further checking
1107 // returns tree node if visitor says stop or -1
1108 function TDynAABBTree.visit (checker: TVisitCheckerCB; visitor: TVisitVisitorCB): Integer;
1109 var
1110   stack: array [0..255] of Integer; // stack with the nodes to visit
1111   bigstack: array of Integer = nil;
1112   sp: Integer = 0;
1113
1114   procedure spush (id: Integer); inline;
1115   var
1116     xsp: Integer;
1117   begin
1118     if (sp < length(stack)) then
1119     begin
1120       // use "small stack"
1121       stack[sp] := id;
1122       Inc(sp);
1123     end
1124     else
1125     begin
1126       // use "big stack"
1127       xsp := sp-length(stack);
1128       if (xsp < length(bigstack)) then
1129       begin
1130         // reuse
1131         bigstack[xsp] := id;
1132       end
1133       else
1134       begin
1135         // grow
1136         SetLength(bigstack, length(bigstack)+1);
1137         bigstack[high(bigstack)] := id;
1138       end;
1139       Inc(sp);
1140     end;
1141   end;
1142
1143   (*
1144   function spop (): Integer; inline;
1145   begin
1146     {$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF}
1147     if (sp <= length(stack)) then
1148     begin
1149       // use "small stack"
1150       Dec(sp);
1151       result := stack[sp];
1152     end
1153     else
1154     begin
1155       // use "big stack"
1156       Dec(sp);
1157       result := bigstack[sp-length(stack)];
1158     end;
1159   end;
1160   *)
1161
1162 var
1163   nodeId: Integer;
1164   node: PTreeNode;
1165 begin
1166   if not assigned(checker) then begin result := -1; exit; end;
1167   //if not assigned(visitor) then begin result := -1; exit; end;
1168   try
1169     {$IFDEF aabbtree_query_count}
1170     mNodesVisited := 0;
1171     mNodesDeepVisited := 0;
1172     {$ENDIF}
1173
1174     // start from root node
1175     spush(mRootNodeId);
1176
1177     // while there are still nodes to visit
1178     while (sp > 0) do
1179     begin
1180       // get the next node id to visit
1181       //nodeId := spop();
1182       {$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF}
1183       if (sp <= length(stack)) then
1184       begin
1185         // use "small stack"
1186         Dec(sp);
1187         nodeId := stack[sp];
1188       end
1189       else
1190       begin
1191         // use "big stack"
1192         Dec(sp);
1193         nodeId := bigstack[sp-length(stack)];
1194       end;
1195
1196       // skip it if it is a nil node
1197       if (nodeId = TTreeNode.NullTreeNode) then continue;
1198       {$IFDEF aabbtree_query_count}Inc(mNodesVisited);{$ENDIF}
1199       // get the corresponding node
1200       node := @mNodes[nodeId];
1201       // should we investigate this node?
1202       if (checker(node)) then
1203       begin
1204         // if the node is a leaf
1205         if (node.leaf) then
1206         begin
1207           // call visitor on it
1208           {$IFDEF aabbtree_query_count}Inc(mNodesDeepVisited);{$ENDIF}
1209           if assigned(visitor) then
1210           begin
1211             if (visitor(node.flesh)) then begin result := nodeId; exit; end;
1212           end;
1213         end
1214         else
1215         begin
1216           // if the node is not a leaf, we need to visit its children
1217           spush(node.children[TTreeNode.Left]);
1218           spush(node.children[TTreeNode.Right]);
1219         end;
1220       end;
1221     end;
1222
1223     result := -1; // oops
1224   finally
1225     bigstack := nil;
1226   end;
1227 end;
1228
1229
1230 // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
1231 // 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
1232 constructor TDynAABBTree.Create (extraAABBGap: Float=0.0);
1233 begin
1234   mExtraGap := extraAABBGap;
1235   setup();
1236 end;
1237
1238
1239 destructor TDynAABBTree.Destroy ();
1240 begin
1241   mNodes := nil;
1242   inherited;
1243 end;
1244
1245
1246 // clear all the nodes and reset the tree
1247 procedure TDynAABBTree.reset ();
1248 begin
1249   mNodes := nil;
1250   setup();
1251 end;
1252
1253
1254 function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end;
1255
1256
1257 // return the root AABB of the tree
1258 procedure TDynAABBTree.getRootAABB (var aabb: AABB2D);
1259 begin
1260   {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mNodeCount));{$ENDIF}
1261   aabb := mNodes[mRootNodeId].aabb;
1262 end;
1263
1264
1265 // does the given id represents a valid object?
1266 // WARNING: ids of removed objects can be reused on later insertions!
1267 function TDynAABBTree.isValidId (id: Integer): Boolean;
1268 begin
1269   result := (id >= 0) and (id < mNodeCount) and (mNodes[id].leaf);
1270 end;
1271
1272
1273 // get object by nodeid; can return nil for invalid ids
1274 function TDynAABBTree.getNodeObjectId (nodeid: Integer): TTreeFlesh;
1275 begin
1276   if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil;
1277 end;
1278
1279 // get fat object AABB by nodeid; returns random shit for invalid ids
1280 procedure TDynAABBTree.getNodeFatAABB (var aabb: AABB2D; nodeid: Integer);
1281 begin
1282   if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].isfree) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0);
1283 end;
1284
1285
1286 // insert an object into the tree
1287 // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error
1288 // AABB for static object will not be "fat" (simple optimization)
1289 // WARNING! inserting the same object several times *WILL* break everything!
1290 function TDynAABBTree.insertObject (flesh: TTreeFlesh; staticObject: Boolean=false): Integer;
1291 var
1292   aabb: AABB2D;
1293   nodeId: Integer;
1294 begin
1295   if not getFleshAABB(aabb, flesh) then
1296   begin
1297     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);
1298     //raise Exception.Create('trying to insert invalid flesh in dyntree');
1299     result := -1;
1300     exit;
1301   end;
1302   if not aabb.valid then
1303   begin
1304     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);
1305     raise Exception.Create('trying to insert invalid aabb in dyntree');
1306     result := -1;
1307     exit;
1308   end;
1309   //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);
1310   nodeId := insertObjectInternal(aabb, staticObject);
1311   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF}
1312   mNodes[nodeId].flesh := flesh;
1313   result := nodeId;
1314 end;
1315
1316
1317 // remove an object from the tree
1318 // WARNING: ids of removed objects can be reused on later insertions!
1319 procedure TDynAABBTree.removeObject (nodeId: Integer);
1320 begin
1321   if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree');
1322   // remove the node from the tree
1323   removeLeafNode(nodeId);
1324   releaseNode(nodeId);
1325 end;
1326
1327
1328 function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
1329 var
1330   newAABB: AABB2D;
1331 begin
1332   if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject');
1333
1334   if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject');
1335   if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTree.updateObject');
1336
1337   // if the new AABB is still inside the fat AABB of the node
1338   if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end;
1339
1340   // if the new AABB is outside the fat AABB, we remove the corresponding node
1341   removeLeafNode(nodeId);
1342
1343   // compute the fat AABB by inflating the AABB with a constant gap
1344   mNodes[nodeId].aabb := newAABB;
1345   if (not forceReinsert) and ((dispX <> 0) or (dispY <> 0)) then
1346   begin
1347     mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX-mExtraGap;
1348     mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY-mExtraGap;
1349     mNodes[nodeId].aabb.maxX := mNodes[nodeId].aabb.maxX+mExtraGap;
1350     mNodes[nodeId].aabb.maxY := mNodes[nodeId].aabb.maxY+mExtraGap;
1351   end;
1352
1353   // inflate the fat AABB in direction of the linear motion of the AABB
1354   if (dispX < 0.0) then
1355   begin
1356     mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX+LinearMotionGapMultiplier*dispX;
1357   end
1358   else
1359   begin
1360     mNodes[nodeId].aabb.maxX := mNodes[nodeId].aabb.maxX+LinearMotionGapMultiplier*dispX;
1361   end;
1362   if (dispY < 0.0) then
1363   begin
1364     mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY+LinearMotionGapMultiplier*dispY;
1365   end
1366   else
1367   begin
1368     mNodes[nodeId].aabb.maxY := mNodes[nodeId].aabb.maxY+LinearMotionGapMultiplier*dispY;
1369   end;
1370
1371   {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].aabb.contains(newAABB));{$ENDIF}
1372
1373   // reinsert the node into the tree
1374   insertLeafNode(nodeId);
1375
1376   result := true;
1377 end;
1378
1379
1380 // report all shapes overlapping with the AABB given in parameter
1381 function TDynAABBTree.aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB): Boolean;
1382 var
1383   caabb: AABB2D;
1384   function checker (node: PTreeNode): Boolean;
1385   begin
1386     result := caabb.overlaps(node.aabb);
1387   end;
1388 begin
1389   if not assigned(cb) then exit;
1390   if (aw < 1) or (ah < 1) then exit;
1391   caabb := AABB2D.Create(ax, ay, ax+aw, ay+ah);
1392   result := (visit(checker, cb) <> -1);
1393 end;
1394
1395
1396 // report body that contains the given point, or nil
1397 function TDynAABBTree.pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh;
1398 var
1399   nid: Integer;
1400   function checker (node: PTreeNode): Boolean;
1401   begin
1402     result := node.aabb.contains(ax, ay);
1403   end;
1404 begin
1405   nid := visit(checker, cb);
1406   {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF}
1407   if (nid >= 0) then result := mNodes[nid].flesh else result := nil;
1408 end;
1409
1410
1411 // segment querying method
1412 function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
1413 var
1414   maxFraction: Float = 1.0e100; // infinity
1415   curax, curay: Float;
1416   curbx, curby: Float;
1417   dirx, diry: Float;
1418   invlen: Float;
1419
1420   function checker (node: PTreeNode): Boolean;
1421   begin
1422     result := node.aabb.intersects(curax, curay, curbx, curby);
1423   end;
1424
1425   function visitor (flesh: TTreeFlesh): Boolean;
1426   var
1427     hitFraction: Float;
1428   begin
1429     hitFraction := cb(flesh, curax, curay, curbx, curby);
1430     // if the user returned a hitFraction of zero, it means that the raycasting should stop here
1431     if (hitFraction = 0.0) then
1432     begin
1433       qr.dist := 0;
1434       qr.flesh := flesh;
1435       result := true;
1436       exit;
1437     end;
1438     // if the user returned a positive fraction
1439     if (hitFraction > 0.0) then
1440     begin
1441       // we update the maxFraction value and the ray AABB using the new maximum fraction
1442       if (hitFraction < maxFraction) then
1443       begin
1444         maxFraction := hitFraction;
1445         qr.dist := hitFraction;
1446         qr.flesh := flesh;
1447         // fix curb here
1448         //curb := cura+dir*hitFraction;
1449         curbx := curax+dirx*hitFraction;
1450         curby := curay+diry*hitFraction;
1451       end;
1452     end;
1453     result := false; // continue
1454   end;
1455
1456 begin
1457   qr.reset();
1458
1459   if (ax >= bx) or (ay >= by) then begin result := false; exit; end;
1460
1461   curax := ax;
1462   curay := ay;
1463   curbx := bx;
1464   curby := by;
1465
1466   dirx := (curbx-curax);
1467   diry := (curby-curay);
1468   // normalize
1469   invlen := 1.0/sqrt(dirx*dirx+diry*diry);
1470   dirx *= invlen;
1471   diry *= invlen;
1472
1473   visit(checker, visitor);
1474
1475   result := qr.valid;
1476 end;
1477
1478
1479 end.