src/game/z_aabbtree.pas

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