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