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