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