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