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