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