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