X-Git-Url: http://deadsoftware.ru/gitweb?a=blobdiff_plain;f=src%2Fgame%2Fz_aabbtree.pas;h=88c7b8862aee31067522d79fd922d20d980e8fb2;hb=4c4a0406b07cdfd5051e388e8b00f02e008ed140;hp=a64e6322b7cbd98518e4ee9671cdb2a70aaccd94;hpb=cac277b4226c3de6401dae59c91dd35b77625304;p=d2df-sdl.git diff --git a/src/game/z_aabbtree.pas b/src/game/z_aabbtree.pas index a64e632..88c7b88 100644 --- a/src/game/z_aabbtree.pas +++ b/src/game/z_aabbtree.pas @@ -29,8 +29,6 @@ uses type {$IFDEF aabbtree_use_floats}TreeNumber = Single;{$ELSE}TreeNumber = Integer;{$ENDIF} - TTreeFlesh = TObject; - // ////////////////////////////////////////////////////////////////////////// // type @@ -39,17 +37,25 @@ type origX, origY: Single; dirX, dirY: Single; + function getOrigN (idx: Integer): Single; inline; + function getDirN (idx: Integer): Single; inline; + public constructor Create (ax, ay: Single; aangle: Single); overload; constructor Create (ax0, ay0, ax1, ay1: Single); overload; - constructor Create (var aray: Ray2D); overload; + constructor Create (constref aray: Ray2D); overload; - procedure copyFrom (var aray: Ray2D); inline; + procedure copyFrom (constref aray: Ray2D); inline; procedure normalizeDir (); inline; procedure setXYAngle (ax, ay: Single; aangle: Single); inline; procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Single); inline; + + procedure atTime (time: Single; out rx, ry: Integer); inline; + + property orig[idx: Integer]: Single read getOrigN; + property dir[idx: Integer]: Single read getDirN; end; // ////////////////////////////////////////////////////////////////////////// // @@ -64,38 +70,46 @@ type function getcenterY (): TreeNumber; inline; function getextentX (): TreeNumber; inline; function getextentY (): TreeNumber; inline; + function getMinN (idx: Integer): TreeNumber; inline; + function getMaxN (idx: Integer): TreeNumber; inline; public constructor Create (x0, y0, x1, y1: TreeNumber); overload; - constructor Create (var aabb: AABB2D); overload; - constructor Create (var aabb0, aabb1: AABB2D); overload; + constructor Create (constref aabb: AABB2D); overload; + constructor Create (constref aabb0, aabb1: AABB2D); overload; - procedure copyFrom (var aabb: AABB2D); inline; + constructor CreateWH (ax, ay, w, h: TreeNumber); + + procedure copyFrom (constref aabb: AABB2D); inline; procedure setDims (x0, y0, x1, y1: TreeNumber); inline; - procedure setMergeTwo (var aabb0, aabb1: AABB2D); inline; + procedure setMergeTwo (constref aabb0, aabb1: AABB2D); inline; function volume (): TreeNumber; inline; - procedure merge (var aabb: AABB2D); inline; + procedure merge (constref aabb: AABB2D); inline; // return true if the current AABB contains the AABB given in parameter - function contains (var aabb: AABB2D): Boolean; inline; overload; + function contains (constref aabb: AABB2D): Boolean; inline; overload; function contains (ax, ay: TreeNumber): Boolean; inline; overload; // return true if the current AABB is overlapping with the AABB in parameter // two AABBs overlap if they overlap in the two axes at the same time - function overlaps (var aabb: AABB2D): Boolean; inline; overload; + function overlaps (constref aabb: AABB2D): Boolean; inline; overload; // ray direction must be normalized - function intersects (var ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; - function intersects (ax, ay, bx, by: Single): Boolean; inline; overload; + function intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; + function intersects (ax, ay, bx, by: Single; tmino: PSingle=nil): Boolean; inline; overload; + function intersects (constref ray: Ray2D; maxtime: Single; tmino: PSingle=nil): Boolean; inline; overload; property valid: Boolean read getvalid; property centerX: TreeNumber read getcenterX; property centerY: TreeNumber read getcenterY; property extentX: TreeNumber read getextentX; property extentY: TreeNumber read getextentY; + + property min[idx: Integer]: TreeNumber read getMinN; + property max[idx: Integer]: TreeNumber read getMaxN; end; @@ -133,7 +147,10 @@ type // ////////////////////////////////////////////////////////////////////////// // // Dynamic AABB Tree: can be used to speed up broad phase in various engines type - TDynAABBTree = class(TObject) + generic TDynAABBTreeBase = class(TObject) + public + type TTreeFlesh = ITP; + private type PTreeNode = ^TTreeNode; @@ -167,12 +184,16 @@ type procedure dumpToLog (); end; - TVisitCheckerCB = function (node: PTreeNode): Boolean is nested; + TVisitCheckerCB = function (node: PTreeNode): Boolean of object; //TVisitVisitorCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested; + const ModeNoChecks = 0; + const ModeAABB = 1; + const ModePoint = 2; + public // return `true` to stop - type TForEachLeafCB = function (abody: TTreeFlesh; var aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here! + type TForEachLeafCB = function (abody: TTreeFlesh; constref aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here! public // in the broad-phase collision detection (dynamic AABB tree), the AABBs are @@ -184,6 +205,22 @@ type const LinearMotionGapMultiplier = 17; // *10 {$ENDIF} + public + // called when a overlapping node has been found during the call to forEachAABBOverlap() + // return `true` to stop + type TQueryOverlapCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested; + type TSegQueryCallback = function (abody: TTreeFlesh; var ray: Ray2D): Single is nested; // return hit time + + PSegmentQueryResult = ^TSegmentQueryResult; + TSegmentQueryResult = record + time: Single; // <0: nothing was hit + flesh: TTreeFlesh; + + constructor Create (fuckyoufpc: Boolean); + procedure reset (); inline; + function valid (): Boolean; inline; + end; + private mNodes: array of TTreeNode; // nodes of the tree mRootNodeId: Integer; // id of the root node of the tree @@ -195,19 +232,23 @@ type // without triggering a large modification of the tree which can be costly mExtraGap: TreeNumber; - public - // called when a overlapping node has been found during the call to forEachAABBOverlap() - // return `true` to stop - type TQueryOverlapCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested; - type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Single): Single is nested; // return dist from (ax,ay) to abody + chkAABB: AABB2D; // for checkers + qSRes: PSegmentQueryResult; // for queries + // for segment query + curax, curay: Single; + curbx, curby: Single; + dirx, diry: Single; + traceRay: Ray2D; + sqcb: TSegQueryCallback; + vstack: array of Integer; // for `visit()` + vstused: Integer; // to support recursive queries - TSegmentQueryResult = record - dist: Single; // <0: nothing was hit - flesh: TTreeFlesh; + function checkerAABB (node: PTreeNode): Boolean; + function checkerPoint (node: PTreeNode): Boolean; + function checkerRay (node: PTreeNode): Boolean; + function visitorRay (flesh: TTreeFlesh; tag: Integer): Boolean; - procedure reset (); inline; - function valid (): Boolean; inline; - end; + type TQueryOverlapDg = function (abody: TTreeFlesh; atag: Integer): Boolean of object; private function allocateNode (): Integer; @@ -216,9 +257,11 @@ type procedure removeLeafNode (nodeId: Integer); function balanceSubTreeAtNode (nodeId: Integer): Integer; function computeHeight (nodeId: Integer): Integer; - function insertObjectInternal (var aabb: AABB2D; staticObject: Boolean): Integer; + function insertObjectInternal (constref aabb: AABB2D; staticObject: Boolean): Integer; procedure setup (); - function visit (var caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer; + function visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer; + + function forEachNode (nodeId: Integer; dg: TForEachLeafCB): Boolean; public {$IFDEF aabbtree_query_count} @@ -233,23 +276,23 @@ type procedure reset (); function forEachLeaf (dg: TForEachLeafCB): Boolean; // WARNING! don't modify AABB/tree here! - procedure getRootAABB (var aabb: AABB2D); + procedure getRootAABB (out aabb: AABB2D); function isValidId (id: Integer): Boolean; inline; function getNodeObjectId (nodeid: Integer): TTreeFlesh; inline; - procedure getNodeFatAABB (var aabb: AABB2D; nodeid: Integer); inline; + procedure getNodeFatAABB (out aabb: AABB2D; nodeid: Integer); inline; // returns `false` if nodeid is not leaf function getNodeXY (nodeid: Integer; out x, y: Integer): Boolean; inline; // return `false` for invalid flesh - function getFleshAABB (var aabb: AABB2D; flesh: TTreeFlesh; tag: Integer): Boolean; virtual; abstract; + function getFleshAABB (out aabb: AABB2D; flesh: TTreeFlesh; tag: Integer): Boolean; virtual; abstract; // insert an object into the tree // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error // AABB for static object will not be "fat" (simple optimization) // WARNING! inserting the same object several times *WILL* break everything! - function insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer; + function insertObject (flesh: TTreeFlesh; tag: Integer=-1; staticObject: Boolean=false): Integer; // remove an object from the tree // WARNING: ids of removed objects can be reused on later insertions! @@ -274,8 +317,8 @@ type function updateObject (nodeId: Integer; forceReinsert: Boolean=false): Boolean; overload; function aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; - function pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB): TTreeFlesh; - function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback): Boolean; + function pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; + function segmentQuery (out qr: TSegmentQueryResult; const ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean; function computeTreeHeight (): Integer; // compute the height of the tree @@ -292,6 +335,16 @@ type end; +function dtMinI (a, b: Integer): Integer; inline; +function dtMaxI (a, b: Integer): Integer; inline; + +function dtMinF (a, b: TreeNumber): TreeNumber; inline; +function dtMaxF (a, b: TreeNumber): TreeNumber; inline; + +function minSingle (a, b: Single): Single; inline; +function maxSingle (a, b: Single): Single; inline; + + implementation uses @@ -299,20 +352,27 @@ uses // ////////////////////////////////////////////////////////////////////////// // -function minI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end; -function maxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end; +function dtMinI (a, b: Integer): Integer; inline; begin if (a < b) then result := a else result := b; end; +function dtMaxI (a, b: Integer): Integer; inline; begin if (a > b) then result := a else result := b; end; -function minF (a, b: TreeNumber): TreeNumber; inline; begin if (a < b) then result := a else result := b; end; -function maxF (a, b: TreeNumber): TreeNumber; inline; begin if (a > b) then result := a else result := b; end; +function dtMinF (a, b: TreeNumber): TreeNumber; inline; begin if (a < b) then result := a else result := b; end; +function dtMaxF (a, b: TreeNumber): TreeNumber; inline; begin if (a > b) then result := a else result := b; end; + +function minSingle (a, b: Single): Single; inline; begin if (a < b) then result := a else result := b; end; +function maxSingle (a, b: Single): Single; inline; begin if (a > b) then result := a else result := b; end; // ////////////////////////////////////////////////////////////////////////// // constructor Ray2D.Create (ax, ay: Single; aangle: Single); begin setXYAngle(ax, ay, aangle); end; constructor Ray2D.Create (ax0, ay0, ax1, ay1: Single); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); end; -constructor Ray2D.Create (var aray: Ray2D); overload; begin copyFrom(aray); end; +constructor Ray2D.Create (constref aray: Ray2D); overload; begin copyFrom(aray); end; + + +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; +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; -procedure Ray2D.copyFrom (var aray: Ray2D); inline; +procedure Ray2D.copyFrom (constref aray: Ray2D); inline; begin origX := aray.origX; origY := aray.origY; @@ -347,23 +407,38 @@ begin end; +procedure Ray2D.atTime (time: Single; out rx, ry: Integer); inline; +begin + rx := round(origX+dirX*time); + ry := round(origY+dirY*time); +end; + + // ////////////////////////////////////////////////////////////////////////// // constructor AABB2D.Create (x0, y0, x1, y1: TreeNumber); overload; begin setDims(x0, y0, x1, y1); end; -constructor AABB2D.Create (var aabb: AABB2D); overload; +constructor AABB2D.Create (constref aabb: AABB2D); overload; begin copyFrom(aabb); end; -constructor AABB2D.Create (var aabb0, aabb1: AABB2D); overload; +constructor AABB2D.Create (constref aabb0, aabb1: AABB2D); overload; begin setMergeTwo(aabb0, aabb1); end; -function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end; +constructor AABB2D.CreateWH (ax, ay, w, h: TreeNumber); +begin + minX := ax; + minY := ay; + maxX := ax+w-1; + maxY := ay+h-1; +end; + +function AABB2D.getvalid (): Boolean; inline; begin result := (minX <= maxX) and (minY <= maxY); end; {$IFDEF aabbtree_use_floats} function AABB2D.getcenterX (): TreeNumber; inline; begin result := (minX+maxX)/2.0; end; @@ -372,10 +447,13 @@ function AABB2D.getcenterY (): TreeNumber; inline; begin result := (minY+maxY)/2 function AABB2D.getcenterX (): TreeNumber; inline; begin result := (minX+maxX) div 2; end; function AABB2D.getcenterY (): TreeNumber; inline; begin result := (minY+maxY) div 2; end; {$ENDIF} -function AABB2D.getextentX (): TreeNumber; inline; begin result := (maxX-minX); end; -function AABB2D.getextentY (): TreeNumber; inline; begin result := (maxY-minY); end; +function AABB2D.getextentX (): TreeNumber; inline; begin result := maxX-minX+1; end; +function AABB2D.getextentY (): TreeNumber; inline; begin result := maxY-minY+1; end; + +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; +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; -procedure AABB2D.copyFrom (var aabb: AABB2D); inline; +procedure AABB2D.copyFrom (constref aabb: AABB2D); inline; begin minX := aabb.minX; minY := aabb.minY; @@ -389,26 +467,26 @@ end; procedure AABB2D.setDims (x0, y0, x1, y1: TreeNumber); inline; begin - minX := minF(x0, x1); - minY := minF(y0, y1); - maxX := maxF(x0, x1); - maxY := maxF(y0, y1); + minX := dtMinF(x0, x1); + minY := dtMinF(y0, y1); + maxX := dtMaxF(x0, x1); + maxY := dtMaxF(y0, y1); {$IF DEFINED(D2F_DEBUG)} if not valid then raise Exception.Create('setDims: result is fucked'); {$ENDIF} end; -procedure AABB2D.setMergeTwo (var aabb0, aabb1: AABB2D); inline; +procedure AABB2D.setMergeTwo (constref aabb0, aabb1: AABB2D); inline; begin {$IF DEFINED(D2F_DEBUG)} if not aabb0.valid then raise Exception.Create('setMergeTwo: aabb0 is fucked'); if not aabb1.valid then raise Exception.Create('setMergeTwo: aabb0 is fucked'); {$ENDIF} - minX := minF(aabb0.minX, aabb1.minX); - minY := minF(aabb0.minY, aabb1.minY); - maxX := maxF(aabb0.maxX, aabb1.maxX); - maxY := maxF(aabb0.maxY, aabb1.maxY); + minX := dtMinF(aabb0.minX, aabb1.minX); + minY := dtMinF(aabb0.minY, aabb1.minY); + maxX := dtMaxF(aabb0.maxX, aabb1.maxX); + maxY := dtMaxF(aabb0.maxY, aabb1.maxY); {$IF DEFINED(D2F_DEBUG)} if not valid then raise Exception.Create('setMergeTwo: result is fucked'); {$ENDIF} @@ -417,26 +495,26 @@ end; function AABB2D.volume (): TreeNumber; inline; begin - result := (maxX-minX)*(maxY-minY); + result := (maxX-minX+1)*(maxY-minY+1); end; -procedure AABB2D.merge (var aabb: AABB2D); inline; +procedure AABB2D.merge (constref aabb: AABB2D); inline; begin {$IF DEFINED(D2F_DEBUG)} if not aabb.valid then raise Exception.Create('merge: aabb is fucked'); {$ENDIF} - minX := minF(minX, aabb.minX); - minY := minF(minY, aabb.minY); - maxX := maxF(maxX, aabb.maxX); - maxY := maxF(maxY, aabb.maxY); + minX := dtMinF(minX, aabb.minX); + minY := dtMinF(minY, aabb.minY); + maxX := dtMaxF(maxX, aabb.maxX); + maxY := dtMaxF(maxY, aabb.maxY); {$IF DEFINED(D2F_DEBUG)} if not valid then raise Exception.Create('setMergeTwo: result is fucked'); {$ENDIF} end; -function AABB2D.contains (var aabb: AABB2D): Boolean; inline; overload; +function AABB2D.contains (constref aabb: AABB2D): Boolean; inline; overload; begin result := (aabb.minX >= minX) and (aabb.minY >= minY) and @@ -450,7 +528,7 @@ begin end; -function AABB2D.overlaps (var aabb: AABB2D): Boolean; inline; overload; +function AABB2D.overlaps (constref aabb: AABB2D): Boolean; inline; overload; begin result := false; // exit with no intersection if found separated along any axis @@ -462,7 +540,8 @@ end; // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/ -function AABB2D.intersects (var ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; +{ +function AABB2D.intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; var dinv, t1, t2, tmp: Single; tmin, tmax: Single; @@ -506,41 +585,98 @@ begin result := false; end; end; +} + + +function AABB2D.intersects (constref ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; +var + tmin, tmax, t1, t2, invd: Single; + i: Integer; +begin + tmin := -1.0e100; + tmax := 1.0e100; + for i := 0 to 1 do + begin + if (ray.dir[i] <> 0.0) then + begin + //t1 := (self.min[i]-ray.orig[i])/ray.dir[i]; + //t2 := (self.max[i]-ray.orig[i])/ray.dir[i]; + invd := 1.0/ray.dir[i]; + t1 := (self.min[i]-ray.orig[i])*invd; + t2 := (self.max[i]-ray.orig[i])*invd; + tmin := maxSingle(tmin, minSingle(t1, t2)); + tmax := minSingle(tmax, maxSingle(t1, t2)); + end + else if (ray.orig[i] <= self.min[i]) or (ray.orig[i] >= self.max[i]) then + begin + result := false; + exit; + end; + end; + + result := (tmax > tmin) and (tmax > 0.0); + if result then + begin + if (tmino <> nil) then tmino^ := tmin; + if (tmaxo <> nil) then tmaxo^ := tmin; + end; +end; + -function AABB2D.intersects (ax, ay, bx, by: Single): Boolean; inline; overload; +function AABB2D.intersects (ax, ay, bx, by: Single; tmino: PSingle=nil): Boolean; inline; overload; var tmin: Single; ray: Ray2D; begin result := true; + if (tmino <> nil) then tmino^ := 0.0; // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree) if (ax >= minX) and (ay >= minY) and (ax <= maxX) and (ay <= maxY) then exit; // a if (bx >= minX) and (by >= minY) and (bx <= maxX) and (by <= maxY) then exit; // b // nope, do it hard way ray := Ray2D.Create(ax, ay, bx, by); - if not intersects(ray, @tmin) then begin result := false; exit; end; + if not intersects(ray, @tmin) then begin if (tmino <> nil) then tmino^ := tmin; result := false; exit; end; + if (tmino <> nil) then tmino^ := tmin; if (tmin < 0) then exit; // inside, just in case - bx := bx-ax; - by := by-ay; + bx -= ax; + by -= ay; result := (tmin*tmin <= bx*bx+by*by); end; +function AABB2D.intersects (constref ray: Ray2D; maxtime: Single; tmino: PSingle=nil): Boolean; inline; overload; +var + tmin: Single; +begin + result := true; + if (ray.origX >= minX) and (ray.origY >= minY) and (ray.origX <= maxX) and (ray.origY <= maxY) then + begin + if (tmino <> nil) then tmino^ := 0.0; + exit; + end; + if not intersects(ray, @tmin) then begin if (tmino <> nil) then tmino^ := -1.0; result := false; exit; end; + if (tmin < 0) then tmin := 0; // inside + if (tmino <> nil) then tmino^ := tmin; + result := (tmin <= maxtime); +end; + + // ////////////////////////////////////////////////////////////////////////// // -procedure TDynAABBTree.TSegmentQueryResult.reset (); inline; begin dist := -1; flesh := nil; end; -function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; inline; begin result := (dist >= 0) and (flesh <> nil); end; +constructor TDynAABBTreeBase.TSegmentQueryResult.Create (fuckyoufpc: Boolean); begin time := -1; flesh := Default(ITP); end; +procedure TDynAABBTreeBase.TSegmentQueryResult.reset (); inline; begin time := -1; flesh := Default(ITP); end; +function TDynAABBTreeBase.TSegmentQueryResult.valid (): Boolean; inline; begin result := (time >= 0) and (flesh <> Default(ITP)); end; // ////////////////////////////////////////////////////////////////////////// // -function TDynAABBTree.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end; -function TDynAABBTree.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end; +function TDynAABBTreeBase.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end; +function TDynAABBTreeBase.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end; -procedure TDynAABBTree.TTreeNode.clear (); inline; +procedure TDynAABBTreeBase.TTreeNode.clear (); inline; begin parentId := 0; children[0] := 0; children[1] := 0; - flesh := nil; + flesh := Default(ITP); tag := 0; height := 0; aabb.minX := 0; @@ -549,7 +685,7 @@ begin aabb.maxY := 0; end; -procedure TDynAABBTree.TTreeNode.dumpToLog (); +procedure TDynAABBTreeBase.TTreeNode.dumpToLog (); begin e_WriteLog(Format('NODE: parentId=%d; children=[%d,%d]; height=%d; tag=%d; fleshX=%d; fleshY=%d; aabb=(%d,%d)-(%d,%d)', [parentId, children[0], children[1], Integer(height), tag, fleshX, fleshY, aabb.minX, aabb.minY, aabb.maxX, aabb.maxY]), @@ -559,7 +695,7 @@ end; // ////////////////////////////////////////////////////////////////////////// // // allocate and return a node to use in the tree -function TDynAABBTree.allocateNode (): Integer; +function TDynAABBTreeBase.allocateNode (): Integer; var i, newsz, freeNodeId: Integer; node: PTreeNode; @@ -597,14 +733,14 @@ end; // release a node -procedure TDynAABBTree.releaseNode (nodeId: Integer); +procedure TDynAABBTreeBase.releaseNode (nodeId: Integer); begin {$IFDEF aabbtree_many_asserts}assert(mNodeCount > 0);{$ENDIF} {$IFDEF aabbtree_many_asserts}assert((nodeId >= 0) and (nodeId < mAllocCount));{$ENDIF} {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].height >= 0);{$ENDIF} mNodes[nodeId].nextNodeId := mFreeNodeId; mNodes[nodeId].height := -1; - mNodes[nodeId].flesh := nil; + mNodes[nodeId].flesh := Default(ITP); mFreeNodeId := nodeId; Dec(mNodeCount); @@ -614,7 +750,7 @@ end; // insert a leaf node in the tree // 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 -procedure TDynAABBTree.insertLeafNode (nodeId: Integer); +procedure TDynAABBTreeBase.insertLeafNode (nodeId: Integer); var newNodeAABB, mergedAABBs, currentAndLeftAABB, currentAndRightAABB: AABB2D; currentNodeId: Integer; @@ -723,7 +859,7 @@ begin {$IFDEF aabbtree_many_asserts}assert(rightChild <> TTreeNode.NullTreeNode);{$ENDIF} // recompute the height of the node in the tree - mNodes[currentNodeId].height := maxI(mNodes[leftChild].height, mNodes[rightChild].height)+1; + mNodes[currentNodeId].height := dtMaxI(mNodes[leftChild].height, mNodes[rightChild].height)+1; {$IFDEF aabbtree_many_asserts}assert(mNodes[currentNodeId].height > 0);{$ENDIF} // recompute the AABB of the node @@ -737,7 +873,7 @@ end; // remove a leaf node from the tree -procedure TDynAABBTree.removeLeafNode (nodeId: Integer); +procedure TDynAABBTreeBase.removeLeafNode (nodeId: Integer); var currentNodeId, parentNodeId, grandParentNodeId, siblingNodeId: Integer; leftChildId, rightChildId: Integer; @@ -791,7 +927,7 @@ begin // recompute the AABB and the height of the current node mNodes[currentNodeId].aabb.setMergeTwo(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb); - mNodes[currentNodeId].height := maxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1; + mNodes[currentNodeId].height := dtMaxI(mNodes[leftChildId].height, mNodes[rightChildId].height)+1; {$IFDEF aabbtree_many_asserts}assert(mNodes[currentNodeId].height > 0);{$ENDIF} currentNodeId := mNodes[currentNodeId].parentId; @@ -810,7 +946,7 @@ end; // balance the sub-tree of a given node using left or right rotations // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry // this method returns the new root node id -function TDynAABBTree.balanceSubTreeAtNode (nodeId: Integer): Integer; +function TDynAABBTreeBase.balanceSubTreeAtNode (nodeId: Integer): Integer; var nodeA, nodeB, nodeC, nodeF, nodeG: PTreeNode; nodeBId, nodeCId, nodeFId, nodeGId: Integer; @@ -882,8 +1018,8 @@ begin nodeC.aabb.setMergeTwo(nodeA.aabb, nodeF.aabb); // recompute the height of node A and C - nodeA.height := maxI(nodeB.height, nodeG.height)+1; - nodeC.height := maxI(nodeA.height, nodeF.height)+1; + nodeA.height := dtMaxI(nodeB.height, nodeG.height)+1; + nodeC.height := dtMaxI(nodeA.height, nodeF.height)+1; {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF} {$IFDEF aabbtree_many_asserts}assert(nodeC.height > 0);{$ENDIF} end @@ -899,8 +1035,8 @@ begin nodeC.aabb.setMergeTwo(nodeA.aabb, nodeG.aabb); // recompute the height of node A and C - nodeA.height := maxI(nodeB.height, nodeF.height)+1; - nodeC.height := maxI(nodeA.height, nodeG.height)+1; + nodeA.height := dtMaxI(nodeB.height, nodeF.height)+1; + nodeC.height := dtMaxI(nodeA.height, nodeG.height)+1; {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF} {$IFDEF aabbtree_many_asserts}assert(nodeC.height > 0);{$ENDIF} end; @@ -958,8 +1094,8 @@ begin nodeB.aabb.setMergeTwo(nodeA.aabb, nodeF.aabb); // recompute the height of node A and B - nodeA.height := maxI(nodeC.height, nodeG.height)+1; - nodeB.height := maxI(nodeA.height, nodeF.height)+1; + nodeA.height := dtMaxI(nodeC.height, nodeG.height)+1; + nodeB.height := dtMaxI(nodeA.height, nodeF.height)+1; {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF} {$IFDEF aabbtree_many_asserts}assert(nodeB.height > 0);{$ENDIF} end @@ -975,8 +1111,8 @@ begin nodeB.aabb.setMergeTwo(nodeA.aabb, nodeG.aabb); // recompute the height of node A and B - nodeA.height := maxI(nodeC.height, nodeF.height)+1; - nodeB.height := maxI(nodeA.height, nodeG.height)+1; + nodeA.height := dtMaxI(nodeC.height, nodeF.height)+1; + nodeB.height := dtMaxI(nodeA.height, nodeG.height)+1; {$IFDEF aabbtree_many_asserts}assert(nodeA.height > 0);{$ENDIF} {$IFDEF aabbtree_many_asserts}assert(nodeB.height > 0);{$ENDIF} end; @@ -992,7 +1128,7 @@ end; // compute the height of a given node in the tree -function TDynAABBTree.computeHeight (nodeId: Integer): Integer; +function TDynAABBTreeBase.computeHeight (nodeId: Integer): Integer; var node: PTreeNode; leftHeight, rightHeight: Integer; @@ -1008,12 +1144,12 @@ begin rightHeight := computeHeight(node.children[TTreeNode.Right]); // return the height of the node - result := 1+maxI(leftHeight, rightHeight); + result := 1+dtMaxI(leftHeight, rightHeight); end; // internally add an object into the tree -function TDynAABBTree.insertObjectInternal (var aabb: AABB2D; staticObject: Boolean): Integer; +function TDynAABBTreeBase.insertObjectInternal (constref aabb: AABB2D; staticObject: Boolean): Integer; var nodeId: Integer; node: PTreeNode; @@ -1048,13 +1184,14 @@ end; // initialize the tree -procedure TDynAABBTree.setup (); +procedure TDynAABBTreeBase.setup (); var i: Integer; begin mRootNodeId := TTreeNode.NullTreeNode; mNodeCount := 0; mAllocCount := 8192; + vstused := 0; SetLength(mNodes, mAllocCount); //memset(mNodes, 0, mAllocCount*TTreeNode.sizeof); @@ -1072,253 +1209,234 @@ end; // also, checks if the tree structure is valid (for debugging purpose) -function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean; - function forEachNode (nodeId: Integer): Boolean; - var - pNode: PTreeNode; - leftChild, rightChild, height: Integer; - aabb: AABB2D; +function TDynAABBTreeBase.forEachNode (nodeId: Integer; dg: TForEachLeafCB): Boolean; +var + pNode: PTreeNode; + leftChild, rightChild, height: Integer; + aabb: AABB2D; +begin + result := false; + if (nodeId = TTreeNode.NullTreeNode) then exit; + // if it is the root + if (nodeId = mRootNodeId) then assert(mNodes[nodeId].parentId = TTreeNode.NullTreeNode); + // get the children nodes + pNode := @mNodes[nodeId]; + assert(pNode.height >= 0); + if (not pNode.aabb.valid) then begin - result := false; - if (nodeId = TTreeNode.NullTreeNode) then exit; - // if it is the root - if (nodeId = mRootNodeId) then assert(mNodes[nodeId].parentId = TTreeNode.NullTreeNode); - // get the children nodes - pNode := @mNodes[nodeId]; - assert(pNode.height >= 0); - if (not pNode.aabb.valid) then + {$IFDEF aabbtree_use_floats} + 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); + {$ELSE} + 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); + {$ENDIF} + if pNode.leaf then begin + getFleshAABB(aabb, pNode.flesh, pNode.tag); {$IFDEF aabbtree_use_floats} - 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); + 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); {$ELSE} - 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); + 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); {$ENDIF} - if pNode.leaf then - begin - getFleshAABB(aabb, pNode.flesh, pNode.tag); - {$IFDEF aabbtree_use_floats} - 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); - {$ELSE} - 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); - {$ENDIF} - end; - end; - assert(pNode.aabb.valid); - assert(pNode.aabb.volume > 0); - // if the current node is a leaf - if (pNode.leaf) then - begin - assert(pNode.height = 0); - if assigned(dg) then result := dg(pNode.flesh, pNode.aabb); - end - else - begin - leftChild := pNode.children[TTreeNode.Left]; - rightChild := pNode.children[TTreeNode.Right]; - // check that the children node Ids are valid - assert((0 <= leftChild) and (leftChild < mAllocCount)); - assert((0 <= rightChild) and (rightChild < mAllocCount)); - // check that the children nodes have the correct parent node - assert(mNodes[leftChild].parentId = nodeId); - assert(mNodes[rightChild].parentId = nodeId); - // check the height of node - height := 1+maxI(mNodes[leftChild].height, mNodes[rightChild].height); - assert(mNodes[nodeId].height = height); - // check the AABB of the node - aabb := AABB2D.Create(mNodes[leftChild].aabb, mNodes[rightChild].aabb); - assert(aabb.minX = mNodes[nodeId].aabb.minX); - assert(aabb.minY = mNodes[nodeId].aabb.minY); - assert(aabb.maxX = mNodes[nodeId].aabb.maxX); - assert(aabb.maxY = mNodes[nodeId].aabb.maxY); - // recursively check the children nodes - result := forEachNode(leftChild); - if not result then result := forEachNode(rightChild); end; end; + assert(pNode.aabb.valid); + assert(pNode.aabb.volume > 0); + // if the current node is a leaf + if (pNode.leaf) then + begin + assert(pNode.height = 0); + if assigned(dg) then result := dg(pNode.flesh, pNode.aabb); + end + else + begin + leftChild := pNode.children[TTreeNode.Left]; + rightChild := pNode.children[TTreeNode.Right]; + // check that the children node Ids are valid + assert((0 <= leftChild) and (leftChild < mAllocCount)); + assert((0 <= rightChild) and (rightChild < mAllocCount)); + // check that the children nodes have the correct parent node + assert(mNodes[leftChild].parentId = nodeId); + assert(mNodes[rightChild].parentId = nodeId); + // check the height of node + height := 1+dtMaxI(mNodes[leftChild].height, mNodes[rightChild].height); + assert(mNodes[nodeId].height = height); + // check the AABB of the node + aabb := AABB2D.Create(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + assert(aabb.minX = mNodes[nodeId].aabb.minX); + assert(aabb.minY = mNodes[nodeId].aabb.minY); + assert(aabb.maxX = mNodes[nodeId].aabb.maxX); + assert(aabb.maxY = mNodes[nodeId].aabb.maxY); + // recursively check the children nodes + result := forEachNode(leftChild, dg); + if not result then result := forEachNode(rightChild, dg); + end; +end; + +// also, checks if the tree structure is valid (for debugging purpose) +function TDynAABBTreeBase.forEachLeaf (dg: TForEachLeafCB): Boolean; begin // recursively check each node - result := forEachNode(mRootNodeId); + result := forEachNode(mRootNodeId, dg); end; // return `true` from visitor to stop immediately // checker should check if this node should be considered to further checking // returns tree node if visitor says stop or -1 -const ModeNoChecks = 0; -const ModeAABB = 1; -const ModePoint = 2; -function TDynAABBTree.visit (var caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer; -var - stack: array [0..2048] of Integer; // stack with the nodes to visit - bigstack: array of Integer = nil; - sp: Integer = 0; - - procedure spush (id: Integer); inline; - var - xsp: Integer; - begin - if (sp < length(stack)) then - begin - // use "small stack" - stack[sp] := id; - Inc(sp); - end - else - begin - // use "big stack" - xsp := sp-length(stack); - if (xsp < length(bigstack)) then - begin - // reuse - bigstack[xsp] := id; - end - else - begin - // grow - SetLength(bigstack, length(bigstack)+1); - bigstack[high(bigstack)] := id; - end; - Inc(sp); - end; - end; - - function spop (): Integer; inline; - begin - //{$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF} - if (sp <= length(stack)) then - begin - // use "small stack" - Dec(sp); - result := stack[sp]; - end - else - begin - // use "big stack" - Dec(sp); - result := bigstack[sp-length(stack)]; - end; - end; - +function TDynAABBTreeBase.visit (constref caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; visdg: TQueryOverlapDg; tagmask: Integer): Integer; +const + StackGran = 1024; var + oldvstused: Integer; + vsp: Integer; + vstk: array of Integer; nodeId: Integer; node: PTreeNode; doNode: Boolean = false; begin if not assigned(checker) then begin result := -1; exit; end; - if not assigned(visitor) then raise Exception.Create('dyntree: empty visitors aren''t supported'); - //if not assigned(visitor) then begin result := -1; exit; end; - //try - {$IFDEF aabbtree_query_count} - mNodesVisited := 0; - mNodesDeepVisited := 0; - {$ENDIF} + //if not assigned(visitor) and not assigned(visdg) then raise Exception.Create('dyntree: empty visitors aren''t supported'); + oldvstused := vstused; + if (vstused+StackGran > Length(vstack)) then SetLength(vstack, vstused+StackGran); + vsp := vstused; + vstk := vstack; + + {$IFDEF aabbtree_query_count} + mNodesVisited := 0; + mNodesDeepVisited := 0; + {$ENDIF} - // start from root node + // start from root node + // we can't have nested functions in generics, sorry + {$IF FALSE} spush(mRootNodeId); + {$ELSE} + if (vsp >= Length(vstk)) then SetLength(vstk, vsp+StackGran); + vstk[vsp] := mRootNodeId; + Inc(vsp); + {$ENDIF} - // while there are still nodes to visit - while (sp > 0) do - begin - // get the next node id to visit - {$IF TRUE} - nodeId := spop(); - {$ELSE} - if (sp <= length(stack)) then + // while there are still nodes to visit + while (vsp > oldvstused) do + begin + // get the next node id to visit + // we can't have nested functions in generics, sorry + {$IF FALSE} + nodeId := spop(); + {$ELSE} + Dec(vsp); + nodeId := vstk[vsp]; + {$ENDIF} + // skip it if it is a nil node + if (nodeId = TTreeNode.NullTreeNode) then continue; + {$IFDEF aabbtree_query_count}Inc(mNodesVisited);{$ENDIF} + // get the corresponding node + node := @mNodes[nodeId]; + // should we investigate this node? + case mode of + ModeNoChecks: doNode := checker(node); + ModeAABB: begin - // use "small stack" - Dec(sp); - nodeId := stack[sp]; - end - else + //doNode := caabb.overlaps(node.aabb); + // this gives small speedup (or not...) + // exit with no intersection if found separated along any axis + if (caabb.maxX < node.aabb.minX) or (caabb.minX > node.aabb.maxX) then doNode := false + else if (caabb.maxY < node.aabb.minY) or (caabb.minY > node.aabb.maxY) then doNode := false + else doNode := true; + end; + ModePoint: begin - // use "big stack" - Dec(sp); - nodeId := bigstack[sp-length(stack)]; + //doNode := node.aabb.contains(caabb.minX, caabb.minY); + // this gives small speedup + doNode := (caabb.minX >= node.aabb.minX) and (caabb.minY >= node.aabb.minY) and (caabb.minX <= node.aabb.maxX) and (caabb.minY <= node.aabb.maxY); end; - {$ENDIF} - // skip it if it is a nil node - if (nodeId = TTreeNode.NullTreeNode) then continue; - {$IFDEF aabbtree_query_count}Inc(mNodesVisited);{$ENDIF} - // get the corresponding node - node := @mNodes[nodeId]; - // should we investigate this node? - case mode of - ModeNoChecks: doNode := checker(node); - ModeAABB: - begin - //doNode := caabb.overlaps(node.aabb); - // this gives small speedup (or not...) - // exit with no intersection if found separated along any axis - if (caabb.maxX < node.aabb.minX) or (caabb.minX > node.aabb.maxX) then doNode := false - else if (caabb.maxY < node.aabb.minY) or (caabb.minY > node.aabb.maxY) then doNode := false - else doNode := true; - end; - ModePoint: - begin - //doNode := node.aabb.contains(caabb.minX, caabb.minY); - // this gives small speedup - doNode := (caabb.minX >= node.aabb.minX) and (caabb.minY >= node.aabb.minY) and (caabb.minX <= node.aabb.maxX) and (caabb.minY <= node.aabb.maxY); - end; - end; - if doNode then + end; + if doNode then + begin + // if the node is a leaf + if (node.leaf) then begin - // if the node is a leaf - if (node.leaf) then + // call visitor on it + {$IFDEF aabbtree_query_count}Inc(mNodesDeepVisited);{$ENDIF} + if (tagmask = -1) or ((node.tag and tagmask) <> 0) then begin - // call visitor on it - {$IFDEF aabbtree_query_count}Inc(mNodesDeepVisited);{$ENDIF} - if ((node.tag and tagmask) <> 0) then + doNode := false; + // update object vars from cache, so recursive calls to `visit()` will work + vstack := vstk; + vstused := vsp; + // call callbacks + if assigned(visitor) then doNode := visitor(node.flesh, node.tag); + if assigned(visdg) and visdg(node.flesh, node.tag) then doNode := true; + // do some sanity checks + if (vstused <> vsp) then raise Exception.Create('internal error in dyntree visitor'); + // should we exit? + if doNode then begin - if (visitor(node.flesh, node.tag)) then begin result := nodeId; bigstack := nil; exit; end; + result := nodeId; + vstack := vstk; + vstused := oldvstused; + exit; end; - end - else - begin - // if the node is not a leaf, we need to visit its children + end; + end + else + begin + // if the node is not a leaf, we need to visit its children + // we can't have nested functions in generics, sorry + {$IF FALSE} spush(node.children[TTreeNode.Left]); spush(node.children[TTreeNode.Right]); - end; + {$ELSE} + if (vsp+2 > Length(vstk)) then SetLength(vstk, vsp+StackGran); + vstk[vsp] := node.children[TTreeNode.Left]; + Inc(vsp); + vstk[vsp] := node.children[TTreeNode.Right]; + Inc(vsp); + {$ENDIF} end; end; + end; - result := -1; // oops - bigstack := nil; - //finally - // bigstack := nil; - //end; + result := -1; // oops + vstack := vstk; + vstused := oldvstused; end; // add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds // 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 -constructor TDynAABBTree.Create (extraAABBGap: TreeNumber=0); +constructor TDynAABBTreeBase.Create (extraAABBGap: TreeNumber=0); begin mExtraGap := extraAABBGap; + mNodes := nil; + SetLength(vstack, 2048); + vstused := 0; setup(); end; -destructor TDynAABBTree.Destroy (); +destructor TDynAABBTreeBase.Destroy (); begin mNodes := nil; + vstack := nil; inherited; end; // clear all the nodes and reset the tree -procedure TDynAABBTree.reset (); +procedure TDynAABBTreeBase.reset (); begin mNodes := nil; setup(); end; -function TDynAABBTree.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end; +function TDynAABBTreeBase.computeTreeHeight (): Integer; begin result := computeHeight(mRootNodeId); end; // return the root AABB of the tree -procedure TDynAABBTree.getRootAABB (var aabb: AABB2D); +procedure TDynAABBTreeBase.getRootAABB (out aabb: AABB2D); begin {$IFDEF aabbtree_many_asserts}assert((mRootNodeId >= 0) and (mRootNodeId < mAllocCount));{$ENDIF} aabb := mNodes[mRootNodeId].aabb; @@ -1327,25 +1445,25 @@ end; // does the given id represents a valid object? // WARNING: ids of removed objects can be reused on later insertions! -function TDynAABBTree.isValidId (id: Integer): Boolean; +function TDynAABBTreeBase.isValidId (id: Integer): Boolean; begin result := (id >= 0) and (id < mAllocCount) and (mNodes[id].leaf); end; // get object by nodeid; can return nil for invalid ids -function TDynAABBTree.getNodeObjectId (nodeid: Integer): TTreeFlesh; +function TDynAABBTreeBase.getNodeObjectId (nodeid: Integer): TTreeFlesh; begin - if (nodeid >= 0) and (nodeid < mAllocCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil; + if (nodeid >= 0) and (nodeid < mAllocCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := Default(ITP); end; // get fat object AABB by nodeid; returns random shit for invalid ids -procedure TDynAABBTree.getNodeFatAABB (var aabb: AABB2D; nodeid: Integer); +procedure TDynAABBTreeBase.getNodeFatAABB (out aabb: AABB2D; nodeid: Integer); begin - if (nodeid >= 0) and (nodeid < mAllocCount) and (not mNodes[nodeid].isfree) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0); + 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); end; -function TDynAABBTree.getNodeXY (nodeid: Integer; out x, y: Integer): Boolean; inline; +function TDynAABBTreeBase.getNodeXY (nodeid: Integer; out x, y: Integer): Boolean; inline; begin if (nodeid >= 0) and (nodeid < mAllocCount) and (mNodes[nodeid].leaf) then begin @@ -1372,7 +1490,7 @@ end; // this method creates a new leaf node in the tree and returns the id of the corresponding node or -1 on error // AABB for static object will not be "fat" (simple optimization) // WARNING! inserting the same object several times *WILL* break everything! -function TDynAABBTree.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer; +function TDynAABBTreeBase.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer; var aabb: AABB2D; nodeId, fx, fy: Integer; @@ -1414,24 +1532,24 @@ end; // remove an object from the tree // WARNING: ids of removed objects can be reused on later insertions! -procedure TDynAABBTree.removeObject (nodeId: Integer); +procedure TDynAABBTreeBase.removeObject (nodeId: Integer); begin - if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree'); + if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase'); // remove the node from the tree removeLeafNode(nodeId); releaseNode(nodeId); end; -function TDynAABBTree.updateObject (nodeId: Integer; forceReinsert: Boolean=false): Boolean; overload; +function TDynAABBTreeBase.updateObject (nodeId: Integer; forceReinsert: Boolean=false): Boolean; overload; var newAABB: AABB2D; dispX, dispY: TreeNumber; begin - if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject'); + if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject'); - if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTree.updateObject'); - if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTree.updateObject'); + if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject'); + if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject'); dispX := newAABB.minX-mNodes[nodeId].fleshX; dispY := newAABB.minY-mNodes[nodeId].fleshY; @@ -1442,16 +1560,16 @@ begin result := updateObject(nodeId, dispX, dispY, forceReinsert); end; -function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload; +function TDynAABBTreeBase.updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; overload; var newAABB: AABB2D; fx, fy: Integer; node: PTreeNode; begin - if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject'); + if (nodeId < 0) or (nodeId >= mAllocCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTreeBase.updateObject'); - if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTree.updateObject'); - if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTree.updateObject'); + if not getFleshAABB(newAABB, mNodes[nodeId].flesh, mNodes[nodeId].tag) then raise Exception.Create('invalid flesh dimensions in TDynAABBTreeBase.updateObject'); + if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTreeBase.updateObject'); fx := newAABB.minX; fy := newAABB.minY; @@ -1512,116 +1630,192 @@ begin end; +function TDynAABBTreeBase.checkerAABB (node: PTreeNode): Boolean; +begin + result := chkAABB.overlaps(node.aabb); +end; + + // report all shapes overlapping with the AABB given in parameter -function TDynAABBTree.aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; -var - caabb: AABB2D; - function checker (node: PTreeNode): Boolean; - begin - result := caabb.overlaps(node.aabb); - end; +function TDynAABBTreeBase.aabbQuery (ax, ay, aw, ah: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; var nid: Integer; + oldaabb: AABB2D; begin - result := nil; + result := Default(ITP); if not assigned(cb) then exit; if (aw < 1) or (ah < 1) then exit; - //caabb := AABB2D.Create(ax, ay, ax+aw, ay+ah); - caabb.minX := ax; - caabb.minY := ay; - caabb.maxX := ax+aw; - caabb.maxY := ay+ah; - nid := visit(caabb, ModeAABB, checker, cb, tagmask); - if (nid >= 0) then result := mNodes[nid].flesh else result := nil; + //chkAABB := AABB2D.Create(ax, ay, ax+aw, ay+ah); + oldaabb := chkAABB; + chkAABB.minX := ax; + chkAABB.minY := ay; + chkAABB.maxX := ax+aw; + chkAABB.maxY := ay+ah; + nid := visit(chkAABB, ModeAABB, checkerAABB, cb, nil, tagmask); + chkAABB := oldaabb; + if (nid >= 0) then result := mNodes[nid].flesh else result := Default(ITP); +end; + + +function TDynAABBTreeBase.checkerPoint (node: PTreeNode): Boolean; +begin + result := node.aabb.contains(chkAABB.minX, chkAABB.minY); end; // report body that contains the given point, or nil -function TDynAABBTree.pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB): TTreeFlesh; - function checker (node: PTreeNode): Boolean; - begin - result := node.aabb.contains(ax, ay); - end; - function dummycb (abody: TTreeFlesh; atag: Integer): Boolean; begin result := false; end; +function TDynAABBTreeBase.pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; var nid: Integer; - caabb: AABB2D; + oldaabb: AABB2D; begin - if not assigned(cb) then cb := dummycb; - caabb := AABB2D.Create(ax, ay, ax+1, ay+1); - nid := visit(caabb, ModePoint, checker, cb); + oldaabb := chkAABB; + chkAABB := AABB2D.Create(ax, ay, ax+1, ay+1); + nid := visit(chkAABB, ModePoint, checkerPoint, cb, nil, tagmask); {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mAllocCount) and (mNodes[nid].leaf)));{$ENDIF} - if (nid >= 0) then result := mNodes[nid].flesh else result := nil; + chkAABB := oldaabb; + if (nid >= 0) then result := mNodes[nid].flesh else result := Default(ITP); end; -// segment querying method -function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback): Boolean; -var - maxFraction: Single = 1.0e100; // infinity - curax, curay: Single; - curbx, curby: Single; - dirx, diry: Single; - invlen: Single; - caabb: AABB2D; +function TDynAABBTreeBase.checkerRay (node: PTreeNode): Boolean; +//var tmin: Single = 0; +begin + {$IF FALSE} + result := node.aabb.intersects(curax, curay, curbx, curby, @tmin); + e_WriteLog(Format('intersect: (%f,%f)-(%f,%f) (%d,%d)-(%d,%d) tmin=%f res=%d', [ + minSingle(curax, curbx), + minSingle(curay, curby), + maxSingle(curax, curbx), + maxSingle(curay, curby), + node.aabb.minX, node.aabb.minY, + node.aabb.maxX, node.aabb.maxY, + tmin, + Integer(result), + ]), MSG_NOTIFY); + {$ELSE} + result := false; + if (node.aabb.maxX < minSingle(curax, curbx)) or (node.aabb.maxY < minSingle(curay, curby)) then exit; + if (node.aabb.minX > maxSingle(curax, curbx)) or (node.aabb.minY > maxSingle(curay, curby)) then exit; + result := node.aabb.intersects(traceRay, qSRes.time{, @tmin}); + { + e_WriteLog(Format('intersect: (%f,%f)-(%f,%f) (%d,%d)-(%d,%d) tmin=%f res=%d frac=%f', [ + curax, curay, curbx, curby, + node.aabb.minX, node.aabb.minY, + node.aabb.maxX, node.aabb.maxY, + tmin, + Integer(result), + qSRes.time + ]), MSG_NOTIFY); + } + {$ENDIF} +end; + - function checker (node: PTreeNode): Boolean; +function TDynAABBTreeBase.visitorRay (flesh: TTreeFlesh; tag: Integer): Boolean; +var + hitFraction: Single; + ray: Ray2D; +begin + ray.origX := curax; + ray.origY := curay; + ray.dirX := dirx; + ray.dirY := diry; + hitFraction := sqcb(flesh, ray); + // if the user returned a hitFraction of zero, it means that the raycasting should stop here + if (hitFraction = 0.0) then begin - result := node.aabb.intersects(curax, curay, curbx, curby); + qSRes.time := 0; + qSRes.flesh := flesh; + result := true; + exit; end; - - function visitor (flesh: TTreeFlesh; tag: Integer): Boolean; - var - hitFraction: Single; + // if the user returned a positive fraction + if (hitFraction > 0.0) then begin - hitFraction := cb(flesh, curax, curay, curbx, curby); - // if the user returned a hitFraction of zero, it means that the raycasting should stop here - if (hitFraction = 0.0) then - begin - qr.dist := 0; - qr.flesh := flesh; - result := true; - exit; - end; - // if the user returned a positive fraction - if (hitFraction > 0.0) then + // we update the maxFraction value and the ray AABB using the new maximum fraction + if (hitFraction < qSRes.time) then begin - // we update the maxFraction value and the ray AABB using the new maximum fraction - if (hitFraction < maxFraction) then - begin - maxFraction := hitFraction; - qr.dist := hitFraction; - qr.flesh := flesh; - // fix curb here - //curb := cura+dir*hitFraction; - curbx := curax+dirx*hitFraction; - curby := curay+diry*hitFraction; - end; + qSRes.time := hitFraction; + qSRes.flesh := flesh; + // fix curb here + //curb := cura+dir*hitFraction; + curbx := curax+dirx*hitFraction; + curby := curay+diry*hitFraction; end; - result := false; // continue end; + result := false; // continue +end; + +// segment querying method +function TDynAABBTreeBase.segmentQuery (out qr: TSegmentQueryResult; const ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback; tagmask: Integer=-1): Boolean; +var + oldcurax, oldcuray: Single; + oldcurbx, oldcurby: Single; + olddirx, olddiry: Single; + invlen: Single; + osres: PSegmentQueryResult; + osqcb: TSegQueryCallback; + oldray: Ray2D; begin - qr.reset(); + qr := TSegmentQueryResult.Create(false); - if (ax >= bx) or (ay >= by) then begin result := false; exit; end; + if (ax = bx) and (ay = by) then begin result := false; exit; end; + oldcurax := curax; + oldcuray := curay; + oldcurbx := curbx; + oldcurby := curby; + olddirx := dirx; + olddiry := diry; + oldray := traceRay; + + qr.time := 1.0e100; // infinity + //qr.time := sqrt((bx-ax)*(bx-ax)+(by-ay)*(by-ay))+1.0; curax := ax; curay := ay; curbx := bx; curby := by; - dirx := (curbx-curax); - diry := (curby-curay); + dirx := curbx-curax; + diry := curby-curay; // normalize invlen := 1.0/sqrt(dirx*dirx+diry*diry); dirx *= invlen; diry *= invlen; - caabb := AABB2D.Create(0, 0, 1, 1); - visit(caabb, ModeNoChecks, checker, visitor); - - result := qr.valid; + traceRay.origX := curax; + traceRay.origY := curay; + traceRay.dirX := dirx; + traceRay.dirY := diry; + + //chkAABB := AABB2D.Create(0, 0, 1, 1); + osres := qSRes; + qSRes := @qr; + osqcb := sqcb; + sqcb := cb; + visit(chkAABB, ModeNoChecks, checkerRay, nil, visitorRay, tagmask); + qSRes := osres; + sqcb := osqcb; + + curax := oldcurax; + curay := oldcuray; + curbx := oldcurbx; + curby := oldcurby; + dirx := olddirx; + diry := olddiry; + traceRay := oldray; + + if qr.valid and (qr.time <= (bx-ax)*(bx-ax)+(by-ay)*(by-ay)) then + begin + result := true; + end + else + begin + result := false; + qr.flesh := nil; + end; end;