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